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 "calibration/magnetometer/mag_cal.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 // Save accumulate variables to calc. mean and var. 46 float acc_x, acc_y, acc_z; 47 float acc_xx, acc_yy, acc_zz; 48 49 // Mean and var. 50 float mean_x, mean_y, mean_z; 51 float var_x, var_y, var_z; 52 53 // # of samples used in the stillness detector. 54 uint32_t nsamples; 55 56 // Start timer for a new still detection (in ns). 57 uint64_t start_time; 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 // Complete accel calibration struct. 131 struct AccelCal { 132 struct AccelCalAlgo ac1[ACCEL_CAL_NUM_TEMP_WINDOWS]; 133 struct AccelStillDet asd; 134 #ifdef ACCEL_CAL_DBG_ENABLED 135 struct AccelStatsMem adf; 136 #endif 137 138 // Offsets are only updated while the accelerometer is not running. Hence need 139 // to store a new offset, which gets updated during a power down event. 140 float x_bias_new, y_bias_new, z_bias_new; 141 142 // Offset values that get subtracted from live data 143 float x_bias, y_bias, z_bias; 144 145 #ifdef IMU_TEMP_DBG_ENABLED 146 // Temporary time variable used to to print an IMU temperature value with a 147 // lower custom sample rate. 148 uint64_t temp_time_nanos; 149 #endif 150 }; 151 152 /* This function runs the accel calibration algorithm. 153 * sample_time_nanos -> is the sensor timestamp in ns and 154 * is used to check the stillness time. 155 * x,y,z -> is the sensor data (m/s^2) for the three axes. 156 * Data is converted to g’s inside the function. 157 * temp -> is the temperature of the IMU (degree C). 158 */ 159 void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, 160 float y, float z, float temp); 161 162 /* This function initializes the accCalRun data struct. 163 * t0 -> Sets the time how long the accel has to be still in ns. 164 * n_s -> Defines the minimum number of samples for the stillness. 165 * th -> Sets the threshold for the stillness VAR in (g rms)^2. 166 * fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the 167 * sphere cap (Bucket) is needed to reach full. 168 */ 169 void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th, 170 uint32_t fx, uint32_t fxb, uint32_t fy, uint32_t fyb, 171 uint32_t fz, uint32_t fzb, uint32_t fle); 172 173 void accelCalDestroy(struct AccelCal *acc); 174 175 // Ensures that the offset is only updated during Sensor power down. 176 bool accelCalUpdateBias(struct AccelCal *acc, float *x, float *y, float *z); 177 178 void accelCalBiasSet(struct AccelCal *acc, float x, float y, float z); 179 180 void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z); 181 182 #ifdef ACCEL_CAL_DBG_ENABLED 183 void accelCalDebPrint(struct AccelCal *acc, float temp); 184 #endif 185 186 #ifdef __cplusplus 187 } 188 #endif 189 190 #endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_ 191