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