1 #ifndef GYRO_CAL_H_ 2 #define GYRO_CAL_H_ 3 4 /* 5 * Copyright (C) 2016 The Android Open Source Project 6 * 7 * Licensed under the Apache License, Version 2.0 (the "License"); 8 * you may not use this file except in compliance with the License. 9 * You may obtain a copy of the License at 10 * 11 * http://www.apache.org/licenses/LICENSE-2.0 12 * 13 * Unless required by applicable law or agreed to in writing, software 14 * distributed under the License is distributed on an "AS IS" BASIS, 15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 * See the License for the specific language governing permissions and 17 * limitations under the License. 18 */ 19 20 /////////////////////////////////////////////////////////////// 21 /* 22 * This module contains the algorithms for producing a 23 * gyroscope offset calibration. The algorithm looks 24 * for periods of stillness as indicated by accelerometer, 25 * magnetometer and gyroscope, and computes a bias estimate 26 * by taking the average of the gyroscope during the 27 * stillness times. 28 * 29 * Currently, this algorithm is tuned such that the device 30 * is only considered still when the device is on a 31 * stationary surface (e.g., not on a person). 32 * 33 * NOTE - Time units are agnostic (i.e., determined by the 34 * user's application and usage). However, typical time units 35 * are nanoseconds. 36 * 37 * Required Sensors and Units: 38 * - Gyroscope [rad/sec] 39 * - Accelerometer [m/sec^2] 40 * 41 * Optional Sensors and Units: 42 * - Magnetometer [micro-Tesla, uT] 43 * - Temperature [Celcius] 44 */ 45 /////////////////////////////////////////////////////////////// 46 47 #include <seos.h> 48 #include <stdint.h> 49 #include <stdbool.h> 50 #include <stdio.h> 51 #include <sys/types.h> 52 #include <string.h> 53 #include <algos/gyro_stillness_detect.h> 54 55 #ifdef GYRO_CAL_DBG_ENABLED 56 // Debug: Number of past calibrations to store. 57 #define DEBUG_GYRO_SHORTTERM_NUM_CAL 30 58 #define DEBUG_GYRO_LONGTERM_NUM_CAL 30 59 #define DEBUG_GYRO_CAL_LIMIT 7200000000000 //2hours [nsec] 60 61 // Gyro Cal debug information/data tracking structure. 62 struct debugGyroCal_t{ 63 float temperature_celcius; 64 float accel_stillness_conf; 65 float gyro_stillness_conf; 66 float mag_stillness_conf; 67 uint64_t calibration_time; 68 uint64_t calibration_time_duration; 69 bool used_mag_sensor; 70 float calibration[3]; 71 float accel_mean[3]; 72 float gyro_mean[3]; 73 float mag_mean[3]; 74 float accel_var[3]; 75 float gyro_var[3]; 76 float mag_var[3]; 77 }; 78 #endif 79 80 struct gyroCal_t{ 81 82 // Stillness detectors. 83 struct gyroStillDet_t accel_stillness_detect; 84 struct gyroStillDet_t mag_stillness_detect; 85 struct gyroStillDet_t gyro_stillness_detect; 86 87 // Flag is "true" when the magnetometer is used. 88 bool using_mag_sensor; 89 90 // Flag set by user to control whether calibrations are used 91 // (default: "true"). 92 bool gyro_calibration_enable; 93 94 // Latest temperature measurement. 95 float latest_temperature_celcius; 96 97 // Aggregated sensor stillness threshold required for gyro bias calibration. 98 float stillness_threshold; 99 100 // Min and max durations for gyro bias calibration. 101 uint64_t min_still_duration; 102 uint64_t max_still_duration; 103 104 // Duration of the stillness processing windows. 105 uint64_t window_time_duration; 106 107 // Flag to indicate if device was previously still. 108 bool prev_still; 109 110 // Timestamp when device started a still period. 111 uint64_t start_still_time; 112 113 // gyro bias estimates and last calibration timestamp. 114 float bias_x, bias_y, bias_z; // [rad/sec] 115 uint64_t calibration_time; 116 uint64_t calibration_time_duration; 117 float stillness_confidence; 118 bool new_gyro_cal_available; //true when a new cal is ready. 119 120 // Current window end time for all sensors. Used to assist in keeping 121 // sensor data collection in sync. On initialization this will be set to 122 // zero indicating that sensor data will be dropped until a valid end time 123 // is set from the first gyro timestamp received. 124 uint64_t stillness_win_endtime; 125 126 // Watchdog timer to reset to a known good state when data capture stalls. 127 uint64_t gyro_watchdog_start; 128 uint64_t gyro_watchdog_timeout_duration; 129 bool gyro_watchdog_timeout; 130 131 //---------------------------------------------------------------- 132 133 #ifdef GYRO_CAL_DBG_ENABLED 134 // Debug info. 135 bool debug_processed_data_available; //flag on a per window basis. 136 uint64_t debug_processed_data_time; //flag time stamp. 137 uint32_t debug_calibration_count; //total number of cals performed. 138 uint32_t debug_watchdog_count; //total number of watchdog timeouts. 139 140 // Debug short-term history data. 141 struct debugGyroCal_t debug_cal_data[DEBUG_GYRO_SHORTTERM_NUM_CAL]; 142 uint8_t debug_num_cals; //number of calibrations collected. 143 uint8_t debug_head; //index of last valid calibration. 144 145 // Debug long-term history data (limited collection frequency). 146 struct debugGyroCal_t debug_cal_data_hist[DEBUG_GYRO_LONGTERM_NUM_CAL]; 147 uint8_t debug_num_cals_hist; //number of calibrations collected. 148 uint8_t debug_head_hist; //index of last valid calibration. 149 #endif 150 }; 151 152 /////// FUNCTION PROTOTYPES ////////////////////////////////////////// 153 154 // Initialize the gyro calibration data structure. 155 void gyroCalInit(struct gyroCal_t* gyro_cal, uint64_t min_still_duration, 156 uint64_t max_still_duration, 157 float bias_x, float bias_y, float bias_z, 158 uint64_t calibration_time, 159 uint64_t window_time_duration, 160 float gyro_var_threshold, 161 float gyro_confidence_delta, 162 float accel_var_threshold, 163 float accel_confidence_delta, 164 float mag_var_threshold, 165 float mag_confidence_delta, 166 float stillness_threshold, 167 int remove_bias_enable); 168 169 // Void all pointers in the gyro calibration data structure. 170 void gyroCalDestroy(struct gyroCal_t* gyro_cal); 171 172 // Get the most recent bias calibration value. 173 void gyroCalGetBias(struct gyroCal_t* gyro_cal, 174 float* bias_x, float* bias_y, float* bias_z); 175 176 // Set an initial bias calibration value. 177 void gyroCalSetBias(struct gyroCal_t* gyro_cal, 178 float bias_x, float bias_y, float bias_z, 179 uint64_t calibration_time); 180 181 // Remove gyro bias from the calibration [rad/sec]. 182 void gyroCalRemoveBias(struct gyroCal_t* gyro_cal, float xi, float yi, 183 float zi, float* xo, float* yo, float* zo); 184 185 // Returns true when a new gyro calibration is available. 186 bool gyroCalNewBiasAvailable(struct gyroCal_t* gyro_cal); 187 188 // Update the gyro calibration with gyro data [rad/sec]. 189 void gyroCalUpdateGyro(struct gyroCal_t* gyro_cal, 190 uint64_t sample_time, 191 float x, float y, float z, 192 float temperature); 193 194 // Update the gyro calibration with mag data [micro Tesla]. 195 void gyroCalUpdateMag(struct gyroCal_t* gyro_cal, 196 uint64_t sample_time, 197 float x, float y, float z); 198 199 // Update the gyro calibration with accel data [m/sec^2]. 200 void gyroCalUpdateAccel(struct gyroCal_t* gyro_cal, 201 uint64_t sample_time, 202 float x, float y, float z); 203 204 #ifdef GYRO_CAL_DBG_ENABLED 205 // Print debug data report. 206 void gyroCalDebugPrint(struct gyroCal_t* gyro_cal, 207 int* state, 208 uint64_t sample_time); 209 #endif 210 211 #endif // GYRO_CAL_H_ 212