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