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 /* 18 * This module contains the algorithms for producing a gyroscope offset 19 * calibration. The algorithm looks for periods of stillness as indicated by 20 * accelerometer, magnetometer and gyroscope, and computes a bias estimate by 21 * taking the average of the gyroscope during the stillness times. 22 * 23 * Currently, this algorithm is tuned such that the device is only considered 24 * still when the device is on a stationary surface (e.g., not on a person). 25 * 26 * NOTE - Time units are agnostic (i.e., determined by the user's application 27 * and usage). However, typical time units are nanoseconds. 28 * 29 * Required Sensors and Units: 30 * - Gyroscope [rad/sec] 31 * - Accelerometer [m/sec^2] 32 * 33 * Optional Sensors and Units: 34 * - Magnetometer [micro-Tesla, uT] 35 * - Temperature [Celsius] 36 * 37 * #define GYRO_CAL_DBG_ENABLED to enable debug printout statements. 38 * data to assist in tuning the GyroCal parameters. 39 */ 40 41 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_ 42 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_ 43 44 #include "calibration/gyroscope/gyro_stillness_detect.h" 45 46 #ifdef __cplusplus 47 extern "C" { 48 #endif 49 50 #ifdef GYRO_CAL_DBG_ENABLED 51 // Debug printout state enumeration. 52 enum GyroCalDebugState { 53 GYRO_IDLE = 0, 54 GYRO_WAIT_STATE, 55 GYRO_PRINT_OFFSET, 56 GYRO_PRINT_STILLNESS_DATA, 57 GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE, 58 GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN, 59 GYRO_PRINT_ACCEL_STATS, 60 GYRO_PRINT_GYRO_STATS, 61 GYRO_PRINT_MAG_STATS 62 }; 63 64 // Gyro Cal debug information/data tracking structure. 65 struct DebugGyroCal { 66 uint64_t start_still_time_nanos; 67 uint64_t end_still_time_nanos; 68 uint64_t stillness_duration_nanos; 69 float mean_sampling_rate_hz; 70 float accel_stillness_conf; 71 float gyro_stillness_conf; 72 float mag_stillness_conf; 73 float calibration[3]; 74 float accel_mean[3]; 75 float gyro_mean[3]; 76 float mag_mean[3]; 77 float accel_var[3]; 78 float gyro_var[3]; 79 float mag_var[3]; 80 float gyro_winmean_min[3]; 81 float gyro_winmean_max[3]; 82 float temperature_min_celsius; 83 float temperature_max_celsius; 84 float temperature_mean_celsius; 85 bool using_mag_sensor; 86 }; 87 88 // Data structure for sample rate estimation. 89 struct SampleRateData { 90 uint64_t last_timestamp_nanos; 91 uint64_t time_delta_accumulator; 92 size_t num_samples; 93 }; 94 #endif // GYRO_CAL_DBG_ENABLED 95 96 // Data structure for tracking min/max window mean during device stillness. 97 struct MinMaxWindowMeanData { 98 float gyro_winmean_min[3]; 99 float gyro_winmean_max[3]; 100 }; 101 102 // Data structure for tracking temperature data during device stillness. 103 struct TemperatureMeanData { 104 float temperature_min_celsius; 105 float temperature_max_celsius; 106 float latest_temperature_celsius; 107 float mean_accumulator; 108 size_t num_points; 109 }; 110 111 struct GyroCal { 112 // Stillness detectors. 113 struct GyroStillDet accel_stillness_detect; 114 struct GyroStillDet mag_stillness_detect; 115 struct GyroStillDet gyro_stillness_detect; 116 117 // Data for tracking temperature mean during periods of device stillness. 118 struct TemperatureMeanData temperature_mean_tracker; 119 120 // Data for tracking gyro mean during periods of device stillness. 121 struct MinMaxWindowMeanData window_mean_tracker; 122 123 // Aggregated sensor stillness threshold required for gyro bias calibration. 124 float stillness_threshold; 125 126 // Min and max durations for gyro bias calibration. 127 uint64_t min_still_duration_nanos; 128 uint64_t max_still_duration_nanos; 129 130 // Duration of the stillness processing windows. 131 uint64_t window_time_duration_nanos; 132 133 // Timestamp when device started a still period. 134 uint64_t start_still_time_nanos; 135 136 // Gyro offset estimate, and the associated calibration temperature, 137 // timestamp, and stillness confidence values. 138 float bias_x, bias_y, bias_z; // [rad/sec] 139 float bias_temperature_celsius; 140 float stillness_confidence; 141 uint64_t calibration_time_nanos; 142 143 // Current window end-time for all sensors. Used to assist in keeping 144 // sensor data collection in sync. On initialization this will be set to 145 // zero indicating that sensor data will be dropped until a valid end-time 146 // is set from the first gyro timestamp received. 147 uint64_t stillness_win_endtime_nanos; 148 149 // Watchdog timer to reset to a known good state when data capture stalls. 150 uint64_t gyro_watchdog_start_nanos; 151 uint64_t gyro_watchdog_timeout_duration_nanos; 152 bool gyro_watchdog_timeout; 153 154 // Flag is "true" when the magnetometer is used. 155 bool using_mag_sensor; 156 157 // Flag set by user to control whether calibrations are used (default: 158 // "true"). 159 bool gyro_calibration_enable; 160 161 // Flag is 'true' when a new calibration update is ready. 162 bool new_gyro_cal_available; 163 164 // Flag to indicate if device was previously still. 165 bool prev_still; 166 167 // Min and maximum stillness window mean. This is used to check the stability 168 // of the mean values computed for the gyroscope (i.e., provides further 169 // validation for stillness). 170 float gyro_winmean_min[3]; 171 float gyro_winmean_max[3]; 172 float stillness_mean_delta_limit; 173 174 // The mean temperature over the stillness period. The limit is used to check 175 // for temperature stability and provide a gate for when temperature is 176 // rapidly changing. 177 float temperature_mean_celsius; 178 float temperature_delta_limit_celsius; 179 180 //---------------------------------------------------------------- 181 182 #ifdef GYRO_CAL_DBG_ENABLED 183 // Debug info. 184 struct DebugGyroCal debug_gyro_cal; // Debug data structure. 185 enum GyroCalDebugState debug_state; // Debug printout state machine. 186 enum GyroCalDebugState next_state; // Debug state machine next state. 187 uint64_t wait_timer_nanos; // Debug message throttle timer. 188 189 struct SampleRateData sample_rate_estimator; // Debug sample rate estimator. 190 191 size_t debug_calibration_count; // Total number of cals performed. 192 size_t debug_watchdog_count; // Total number of watchdog timeouts. 193 bool debug_print_trigger; // Flag used to trigger data printout. 194 #endif // GYRO_CAL_DBG_ENABLED 195 }; 196 197 /////// FUNCTION PROTOTYPES ////////////////////////////////////////// 198 199 // Initialize the gyro calibration data structure. 200 void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration, 201 uint64_t max_still_duration_nanos, float bias_x, float bias_y, 202 float bias_z, uint64_t calibration_time_nanos, 203 uint64_t window_time_duration_nanos, float gyro_var_threshold, 204 float gyro_confidence_delta, float accel_var_threshold, 205 float accel_confidence_delta, float mag_var_threshold, 206 float mag_confidence_delta, float stillness_threshold, 207 float stillness_mean_delta_limit, 208 float temperature_delta_limit_celsius, 209 bool gyro_calibration_enable); 210 211 // Void all pointers in the gyro calibration data structure. 212 void gyroCalDestroy(struct GyroCal* gyro_cal); 213 214 // Get the most recent bias calibration value. 215 void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y, 216 float* bias_z, float* temperature_celsius); 217 218 // Set an initial bias calibration value. 219 void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y, 220 float bias_z, uint64_t calibration_time_nanos); 221 222 // Remove gyro bias from the calibration [rad/sec]. 223 void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi, 224 float* xo, float* yo, float* zo); 225 226 // Returns true when a new gyro calibration is available. 227 bool gyroCalNewBiasAvailable(struct GyroCal* gyro_cal); 228 229 // Update the gyro calibration with gyro data [rad/sec]. 230 void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, 231 float x, float y, float z, float temperature_celsius); 232 233 // Update the gyro calibration with mag data [micro Tesla]. 234 void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, 235 float x, float y, float z); 236 237 // Update the gyro calibration with accel data [m/sec^2]. 238 void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, 239 float x, float y, float z); 240 241 #ifdef GYRO_CAL_DBG_ENABLED 242 // Print debug data report. 243 void gyroCalDebugPrint(struct GyroCal* gyro_cal, 244 uint64_t timestamp_nanos_nanos); 245 #endif 246 247 #ifdef __cplusplus 248 } 249 #endif 250 251 #endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_ 252
