Home | History | Annotate | Download | only in gyroscope 
      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