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