calibration.h

   1 /*
   2 // Copyright (c) 2015 Intel Corporation
   3 //
   4 // Licensed under the Apache License, Version 2.0 (the "License");
   5 // you may not use this file except in compliance with the License.
   6 // You may obtain a copy of the License at
   7 //
   8 //      http://www.apache.org/licenses/LICENSE-2.0
   9 //
  10 // Unless required by applicable law or agreed to in writing, software
  11 // distributed under the License is distributed on an "AS IS" BASIS,
  12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13 // See the License for the specific language governing permissions and
  14 // limitations under the License.
  15 */
  16
  17 #ifndef __CALIBRATION_H__
  18 #define __CALIBRATION_H__
  19
  20 #include "common.h"
  21
  22 #define MAGN_DS_SIZE 32
  23
  24
  25 typedef struct {
  26     /* hard iron offsets */
  27     double offset[3][1];
  28
  29     /* soft iron matrix */
  30     double w_invert[3][3];
  31
  32     /* geomagnetic strength */
  33     double bfield;
  34
  35     /* selection data */
  36     float sample[MAGN_DS_SIZE][3];
  37     unsigned int sample_count;
  38     float average[3];
  39 }
  40 compass_cal_t;
  41
  42
  43 typedef struct {
  44     float bias_x, bias_y, bias_z;
  45     int count;
  46     float min_x, min_y, min_z;
  47     float max_x, max_y, max_z;
  48 }
  49 gyro_cal_t;
  50
  51 /* Accelerometer bias estimation and compensation */
  52
  53 #define BUCKET_COUNT 3
  54 #define BUCKET_TOLERANCE 2.5    /* Maximum monitoring distance from value of interest, in m/s² */
  55 #define SLICES 100                  /* We currently have 3 buckets per axis, and 100 slices per bucket ; then we distribute incoming samples among slices */
  56
  57 typedef struct
  58 {
  59     int version;
  60     int bucket_count;
  61     int slices;
  62     float bucket_tolerance;
  63
  64     uint64_t bucket[3][BUCKET_COUNT][SLICES];   /* How many samples fell in each of the slices, per axis */
  65     uint64_t bucket_usage[3][BUCKET_COUNT];     /* How many samples fell in each of the buckets (= sum of the slice counts) */
  66
  67     /* Estimated bias, according to accumulated data */
  68     float accel_bias_x;
  69     float accel_bias_y;
  70     float accel_bias_z;
  71
  72     uint64_t last_estimation_ts;
  73 }
  74 accel_cal_t;
  75
  76
  77 typedef double mat_input_t[MAGN_DS_SIZE][3];
  78
  79
  80 void calibrate_compass  (int s, sensors_event_t* event);
  81 void compass_read_data  (int s);
  82 void compass_store_data (int s);
  83
  84 void calibrate_gyro     (int s, sensors_event_t* event);
  85 void gyro_cal_init      (int s);
  86 void gyro_store_data    (int s);
  87
  88 void calibrate_accel    (int s, sensors_event_t* event);
  89 void accel_cal_init     (int s);
  90 void accel_cal_store    (int s);
  91
  92 #endif