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/gyro_cal.h | |
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/gyro_cal.h')
-rw-r--r-- | include/gyro_cal.h | 163 |
1 files changed, 163 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 */ |