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