diff options
author | Yuval Peress <peress@chromium.org> | 2020-07-14 18:44:20 -0600 |
---|---|---|
committer | Commit Bot <commit-bot@chromium.org> | 2020-08-18 22:04:40 +0000 |
commit | 474c6a5321de1a56c0751963d20843a5d4eaf7c1 (patch) | |
tree | 7a05135570fd3919df069767a8f0db3dda4a5b47 /include | |
parent | bc932aea929e058ab7561fc341fbea59d18240a0 (diff) | |
download | chrome-ec-474c6a5321de1a56c0751963d20843a5d4eaf7c1.tar.gz |
common: gyro_cal: Implement gyroscope calibration
Implement the calibration code for the gyroscope which is ported over
from AOSP's https://android.googlesource.com/device/google/contexthub/+/refs/heads/master/firmware/os/algos/calibration/gyroscope/
BUG=b:138303429,b:137204366,chromium:1023858
TEST=Added unit tests
BRANCH=None
Signed-off-by: Yuval Peress <peress@chromium.org>
Change-Id: Ic1ab2efb66565cda0a96c9c06722136fb184df77
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2244934
Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>
Diffstat (limited to 'include')
-rw-r--r-- | include/gyro_cal.h | 163 | ||||
-rw-r--r-- | include/gyro_still_det.h | 91 | ||||
-rw-r--r-- | include/math_util.h | 7 |
3 files changed, 261 insertions, 0 deletions
diff --git a/include/gyro_cal.h b/include/gyro_cal.h new file mode 100644 index 0000000000..fb69464aec --- /dev/null +++ b/include/gyro_cal.h @@ -0,0 +1,163 @@ +/* Copyright 2020 The Chromium OS Authors. All rights reserved. + * Use of this source code is governed by a BSD-style license that can be + * found in the LICENSE file. + */ + +#ifndef __CROS_EC_GYRO_CAL_H +#define __CROS_EC_GYRO_CAL_H + +#include "common.h" +#include "gyro_still_det.h" +#include "math_util.h" +#include "stdbool.h" +#include "stddef.h" +#include "vec3.h" + +struct temperature_mean_data { + int16_t temperature_min_kelvin; + int16_t temperature_max_kelvin; + int16_t latest_temperature_kelvin; + int mean_accumulator; + size_t num_points; +}; + +/** Data structure for tracking min/max window mean during device stillness. */ +struct min_max_window_mean_data { + fpv3_t gyro_winmean_min; + fpv3_t gyro_winmean_max; +}; + +struct gyro_cal { + /** Stillness detector for accelerometer. */ + struct gyro_still_det accel_stillness_detect; + /** Stillness detector for magnetometer. */ + struct gyro_still_det mag_stillness_detect; + /** Stillness detector for gyroscope. */ + struct gyro_still_det gyro_stillness_detect; + + /** + * Data for tracking temperature mean during periods of device + * stillness. + */ + struct temperature_mean_data temperature_mean_tracker; + + /** Data for tracking gyro mean during periods of device stillness. */ + struct min_max_window_mean_data window_mean_tracker; + + /** + * Aggregated sensor stillness threshold required for gyro bias + * calibration. + */ + fp_t stillness_threshold; + + /** Min and max durations for gyro bias calibration. */ + uint32_t min_still_duration_us; + uint32_t max_still_duration_us; + + /** Duration of the stillness processing windows. */ + uint32_t window_time_duration_us; + + /** Timestamp when device started a still period. */ + uint64_t start_still_time_us; + + /** + * Gyro offset estimate, and the associated calibration temperature, + * timestamp, and stillness confidence values. + * [rad/sec] + */ + fp_t bias_x, bias_y, bias_z; + int bias_temperature_kelvin; + fp_t stillness_confidence; + uint32_t calibration_time_us; + + /** + * Current window end-time for all sensors. Used to assist in keeping + * sensor data collection in sync. On initialization this will be set to + * zero indicating that sensor data will be dropped until a valid + * end-time is set from the first gyro timestamp received. + */ + uint32_t stillness_win_endtime_us; + + /** + * Watchdog timer to reset to a known good state when data capture + * stalls. + */ + uint32_t gyro_window_start_us; + uint32_t gyro_window_timeout_duration_us; + + /** Flag is "true" when the magnetometer is used. */ + bool using_mag_sensor; + + /** Flag set by user to control whether calibrations are used. */ + bool gyro_calibration_enable; + + /** Flag is 'true' when a new calibration update is ready. */ + bool new_gyro_cal_available; + + /** Flag to indicate if device was previously still. */ + bool prev_still; + + /** + * Min and maximum stillness window mean. This is used to check the + * stability of the mean values computed for the gyroscope (i.e. + * provides further validation for stillness). + */ + fpv3_t gyro_winmean_min; + fpv3_t gyro_winmean_max; + fp_t stillness_mean_delta_limit; + + /** + * The mean temperature over the stillness period. The limit is used to + * check for temperature stability and provide a gate for when + * temperature is rapidly changing. + */ + fp_t temperature_mean_kelvin; + fp_t temperature_delta_limit_kelvin; +}; + +/** + * Data structure used to configure the gyroscope calibration in individual + * sensors. + */ +struct gyro_cal_data { + /** The gyro_cal struct to use. */ + struct gyro_cal gyro_cal; + /** The sensor ID of the accelerometer to use. */ + uint8_t accel_sensor_id; + /** + * The sensor ID of the accelerometer to use (use a number greater than + * SENSOR_COUNT to skip). + */ + uint8_t mag_sensor_id; +}; + +/** Reset trackers. */ +void init_gyro_cal(struct gyro_cal *gyro_cal); + +/** Get the most recent bias calibration value. */ +void gyro_cal_get_bias(struct gyro_cal *gyro_cal, fpv3_t bias, + int *temperature_kelvin, uint32_t *calibration_time_us); + +/** Set an initial bias calibration value. */ +void gyro_cal_set_bias(struct gyro_cal *gyro_cal, fpv3_t bias, + int temperature_kelvin, uint32_t calibration_time_us); + +/** Remove gyro bias from the calibration [rad/sec]. */ +void gyro_cal_remove_bias(struct gyro_cal *gyro_cal, fpv3_t in, fpv3_t out); + +/** Returns true when a new gyro calibration is available. */ +bool gyro_cal_new_bias_available(struct gyro_cal *gyro_cal); + +/** Update the gyro calibration with gyro data [rad/sec]. */ +void gyro_cal_update_gyro(struct gyro_cal *gyro_cal, uint32_t sample_time_us, + fp_t x, fp_t y, fp_t z, int temperature_kelvin); + +/** Update the gyro calibration with mag data [micro Tesla]. */ +void gyro_cal_update_mag(struct gyro_cal *gyro_cal, uint32_t sample_time_us, + fp_t x, fp_t y, fp_t z); + +/** Update the gyro calibration with accel data [m/sec^2]. */ +void gyro_cal_update_accel(struct gyro_cal *gyro_cal, uint32_t sample_time_us, + fp_t x, fp_t y, fp_t z); + +#endif /* __CROS_EC_GYRO_CAL_H */ diff --git a/include/gyro_still_det.h b/include/gyro_still_det.h new file mode 100644 index 0000000000..a776da7ae7 --- /dev/null +++ b/include/gyro_still_det.h @@ -0,0 +1,91 @@ +/* Copyright 2020 The Chromium OS Authors. All rights reserved. + * Use of this source code is governed by a BSD-style license that can be + * found in the LICENSE file. + */ + +#ifndef __CROS_EC_GYRO_STILL_DET_H +#define __CROS_EC_GYRO_STILL_DET_H + +#include "common.h" +#include "math_util.h" +#include "stdbool.h" +#include "vec3.h" + +struct gyro_still_det { + /** + * Variance threshold for the stillness confidence score. + * [sensor units]^2 + */ + fp_t var_threshold; + + /** + * Delta about the variance threshold for calculation of the stillness + * confidence score [0,1]. [sensor units]^2 + */ + fp_t confidence_delta; + + /** + * Flag to indicate when enough samples have been collected for + * a complete stillness calculation. + */ + bool stillness_window_ready; + + /** + * Flag to signal the beginning of a new stillness detection window. + * This is used to keep track of the window start time. + */ + bool start_new_window; + + /** Starting time stamp for the current window. */ + uint32_t window_start_time; + + /** + * Accumulator variables for tracking the sample mean during + * the stillness period. + */ + uint32_t num_acc_samples; + fpv3_t mean; + + /** + * Accumulator variables for computing the window sample mean and + * variance for the current window (used for stillness detection). + */ + uint32_t num_acc_win_samples; + fpv3_t win_mean; + fpv3_t assumed_mean; + fpv3_t acc_var; + + /** Stillness period mean (used for look-ahead). */ + fpv3_t prev_mean; + + /** Latest computed variance. */ + fpv3_t win_var; + + /** + * Stillness confidence score for current and previous sample + * windows [0,1] (used for look-ahead). + */ + fp_t stillness_confidence; + fp_t prev_stillness_confidence; + + /** Timestamp of last sample recorded. */ + uint32_t last_sample_time; +}; + +/** Update the stillness detector with a new sample. */ +void gyro_still_det_update(struct gyro_still_det *gyro_still_det, + uint32_t stillness_win_endtime, uint32_t sample_time, + fp_t x, fp_t y, fp_t z); + +/** Calculates and returns the stillness confidence score [0,1]. */ +fp_t gyro_still_det_compute(struct gyro_still_det *gyro_still_det); + +/** + * Resets the stillness detector and initiates a new detection window. + * + * @param reset_stats Determines whether the stillness statistics are reset. + */ +void gyro_still_det_reset(struct gyro_still_det *gyro_still_det, + bool reset_stats); + +#endif /* __CROS_EC_GYRO_STILL_DET_H */ diff --git a/include/math_util.h b/include/math_util.h index a54eaf3618..6b60d4a1d6 100644 --- a/include/math_util.h +++ b/include/math_util.h @@ -22,6 +22,9 @@ typedef float fp_inter_t; /* Fixed-point to float, for unit tests */ #define FP_TO_FLOAT(x) ((float)(x)) +#define FLT_MAX (3.4028234664e+38) +#define FLT_MIN (1.1754943508e-38) + #else /* Fixed-point type */ typedef int32_t fp_t; @@ -39,6 +42,10 @@ typedef int64_t fp_inter_t; #define FLOAT_TO_FP(x) ((fp_t)((x) * (float)(1<<FP_BITS))) /* Fixed-point to float, for unit tests */ #define FP_TO_FLOAT(x) ((float)(x) / (float)(1<<FP_BITS)) + +#define FLT_MAX INT32_MAX +#define FLT_MIN INT32_MIN + #endif /* |