1 /* 2 * Copyright (C) 2016 The Android Open Source Project 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 /* This module estimates the accelerometer offsets using the KASA sphere fit. 18 * The algorithm senses stillness and classifies the data into seven sphere caps 19 * (nx,nxb,ny,nyb,nz,nzb,nle). Once the buckets are full the data is used to 20 * fit the sphere calculating the offsets and the radius. This can be done, 21 * because when the accelerometer is still it sees only gravity and hence all 22 * the vectors should end onto a sphere. Furthermore the offset values are 23 * subtracted from the accelerometer data calibrating the sensor. 24 */ 25 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_ 26 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_ 27 28 #include <stdint.h> 29 #include <sys/types.h> 30 #include "common/math/kasa.h" 31 #include "common/math/mat.h" 32 33 #ifdef __cplusplus 34 extern "C" { 35 #endif 36 37 #define ACCEL_CAL_NUM_TEMP_WINDOWS 2 38 #ifdef ACCEL_CAL_DBG_ENABLED 39 #define DGB_HISTORY 10 40 #define TEMP_HISTOGRAM 25 41 #endif 42 43 // Data struct for the accel stillness detection. 44 struct AccelStillDet { 45 // Start timer for a new still detection (in ns). 46 uint64_t start_time; 47 48 // Save accumulate variables to calc. mean and var. 49 float acc_x, acc_y, acc_z; 50 float acc_xx, acc_yy, acc_zz; 51 52 // Mean and var. 53 float mean_x, mean_y, mean_z; 54 float var_x, var_y, var_z; 55 56 // # of samples used in the stillness detector. 57 uint32_t nsamples; 58 59 // Controling the Stillness algo with T0 and Th 60 // time the sensor must be still to trigger still detection. 61 uint32_t min_batch_window; 62 uint32_t max_batch_window; 63 64 // Need a minimum amount of samples, filters out low sample rates. 65 uint32_t min_batch_size; 66 67 // Setting Th to var_th. 68 float var_th; 69 70 // Total number of stillness. 71 uint32_t n_still; 72 }; 73 74 /* Struct for good data function. 75 * Counts the vectors that fall into the 7 76 * Sphere caps. 77 */ 78 struct AccelGoodData { 79 // Bucket counters. 80 uint32_t nx, nxb, ny, nyb, nz, nzb, nle; 81 82 // Bucket full values. 83 uint32_t nfx, nfxb, nfy, nfyb, nfz, nfzb, nfle; 84 85 // Temp check (in degree C). 86 float acc_t, acc_tt; 87 float var_t, mean_t; 88 89 // Eigen Values. 90 float e_x, e_y, e_z; 91 }; 92 93 #ifdef ACCEL_CAL_DBG_ENABLED 94 // Struct for stats and debug. 95 struct AccelStatsMem { 96 // Temp (in degree C). 97 uint32_t t_hist[TEMP_HISTOGRAM]; 98 uint64_t start_time_nanos; 99 100 // Offset update counter. 101 uint32_t noff; 102 uint32_t noff_max; 103 104 // Offset history. 105 float var_t[DGB_HISTORY]; 106 float mean_t[DGB_HISTORY]; 107 float x_o[DGB_HISTORY]; 108 float y_o[DGB_HISTORY]; 109 float z_o[DGB_HISTORY]; 110 float e_x[DGB_HISTORY]; 111 float e_y[DGB_HISTORY]; 112 float e_z[DGB_HISTORY]; 113 float rad[DGB_HISTORY]; 114 115 uint8_t n_o; 116 uint64_t cal_time[DGB_HISTORY]; 117 118 // Total Buckets counter. 119 uint32_t ntx, ntxb, nty, ntyb, ntz, ntzb, ntle; 120 }; 121 #endif 122 123 // Struct for an accel calibration for a single temperature window. 124 struct AccelCalAlgo { 125 struct AccelGoodData agd; 126 // TODO(mkramerm): Replace all abbreviations. 127 struct KasaFit akf; 128 }; 129 130 // AccelCal algorithm parameters (see the AccelCal for details). 131 struct AccelCalParameters { 132 // t0 -> Sets the time how long the accel has to be still in ns. 133 // n_s -> Defines the minimum number of samples for the stillness. 134 // th -> Sets the threshold for the stillness VAR in (g rms)^2. 135 // fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the 136 // sphere cap (Bucket) is needed to reach full. 137 uint32_t t0; 138 uint32_t n_s; 139 uint32_t fx; 140 uint32_t fxb; 141 uint32_t fy; 142 uint32_t fyb; 143 uint32_t fz; 144 uint32_t fzb; 145 uint32_t fle; 146 float th; 147 }; 148 149 // Complete accel calibration struct. 150 struct AccelCal { 151 struct AccelCalAlgo ac1[ACCEL_CAL_NUM_TEMP_WINDOWS]; 152 struct AccelStillDet asd; 153 #ifdef ACCEL_CAL_DBG_ENABLED 154 struct AccelStatsMem adf; 155 #endif 156 157 // Offsets are only updated while the accelerometer is not running. Hence need 158 // to store a new offset, which gets updated during a power down event. 159 float x_bias_new, y_bias_new, z_bias_new; 160 161 // Average temperature of the bias update. 162 float average_temperature_celsius; 163 164 // Offset values that get subtracted from live data 165 float x_bias, y_bias, z_bias; 166 167 #ifdef IMU_TEMP_DBG_ENABLED 168 // Temporary time variable used to to print an IMU temperature value with a 169 // lower custom sample rate. 170 uint64_t temp_time_nanos; 171 #endif 172 }; 173 174 /* This function runs the accel calibration algorithm. 175 * sample_time_nanos -> is the sensor timestamp in ns and 176 * is used to check the stillness time. 177 * x,y,z -> is the sensor data (m/s^2) for the three axes. 178 * Data is converted to g’s inside the function. 179 * temp -> is the temperature of the IMU (degree C). 180 */ 181 void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, 182 float y, float z, float temp); 183 184 /* This function initializes the accCalRun data struct. 185 * [parameters]: 186 * t0 -> Sets the time how long the accel has to be still in ns. 187 * n_s -> Defines the minimum number of samples for the stillness. 188 * th -> Sets the threshold for the stillness VAR in (g rms)^2. 189 * fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the 190 * sphere cap (Bucket) is needed to reach full. 191 */ 192 void accelCalInit(struct AccelCal *acc, 193 const struct AccelCalParameters *parameters); 194 195 void accelCalDestroy(struct AccelCal *acc); 196 197 // Ensures that the offset is only updated during Sensor power down. 198 bool accelCalUpdateBias(struct AccelCal *acc, float *x, float *y, float *z); 199 200 void accelCalBiasSet(struct AccelCal *acc, float x, float y, float z); 201 202 void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z); 203 204 // Returns true when a new accel calibration is available. 205 bool accelCalNewBiasAvailable(struct AccelCal *acc); 206 207 #ifdef ACCEL_CAL_DBG_ENABLED 208 void accelCalDebPrint(struct AccelCal *acc, float temp); 209 #endif 210 211 #ifdef __cplusplus 212 } 213 #endif 214 215 #endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_ 216