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 | |
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>
-rw-r--r-- | common/build.mk | 2 | ||||
-rw-r--r-- | common/gyro_cal.c | 630 | ||||
-rw-r--r-- | common/gyro_still_det.c | 242 | ||||
-rw-r--r-- | common/online_calibration.c | 156 | ||||
-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 | ||||
-rw-r--r-- | test/build.mk | 2 | ||||
-rw-r--r-- | test/gyro_cal.c | 611 | ||||
-rw-r--r-- | test/gyro_cal.tasklist | 10 | ||||
-rw-r--r-- | test/test_config.h | 8 |
11 files changed, 1904 insertions, 18 deletions
diff --git a/common/build.mk b/common/build.mk index 0a7fbe86cc..21a268f507 100644 --- a/common/build.mk +++ b/common/build.mk @@ -120,7 +120,7 @@ common-$(CONFIG_RWSIG_TYPE_RWSIG)+=vboot/vb21_lib.o common-$(CONFIG_MATH_UTIL)+=math_util.o common-$(CONFIG_ONLINE_CALIB)+=stillness_detector.o kasa.o math_util.o \ mat44.o vec3.o newton_fit.o accel_cal.o online_calibration.o \ - mkbp_event.o mag_cal.o math_util.o mat33.o + mkbp_event.o mag_cal.o math_util.o mat33.o gyro_cal.o gyro_still_det.o common-$(CONFIG_SHA1)+= sha1.o common-$(CONFIG_SHA256)+=sha256.o common-$(CONFIG_SOFTWARE_CLZ)+=clz.o diff --git a/common/gyro_cal.c b/common/gyro_cal.c new file mode 100644 index 0000000000..572e401b18 --- /dev/null +++ b/common/gyro_cal.c @@ -0,0 +1,630 @@ +/* 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. + */ + +#include "gyro_cal.h" +#include "string.h" +#include <stdbool.h> + +/* + * Maximum gyro bias correction (should be set based on expected max bias + * of the given sensor). [rad/sec] + */ +#define MAX_GYRO_BIAS FLOAT_TO_FP(0.2f) + +static void device_stillness_check(struct gyro_cal *gyro_cal, + uint32_t sample_time_us); + +static void compute_gyro_cal(struct gyro_cal *gyro_cal, + uint32_t calibration_time_us); + +static void check_window(struct gyro_cal *gyro_cal, uint32_t sample_time_us); + +/** Data tracker command enumeration. */ +enum gyro_cal_tracker_command { + /** Resets the local data used for data tracking. */ + DO_RESET = 0, + /** Updates the local tracking data. */ + DO_UPDATE_DATA, + /** Stores intermediate results for later recall. */ + DO_STORE_DATA, + /** Computes and provides the results of the gate function. */ + DO_EVALUATE +}; + +/** + * Reset the gyro_cal's temperature statistics. + * + * @param gyro_cal Pointer to the gyro_cal data structure. + */ +static void gyro_temperature_stats_tracker_reset(struct gyro_cal *gyro_cal); + +/** + * Updates the temperature min/max and mean during the stillness period. + * + * @param gyro_cal Pointer to the gyro_cal data structure. + * @param temperature_kelvin New temperature sample to include. + */ +static void gyro_temperature_stats_tracker_update(struct gyro_cal *gyro_cal, + int temperature_kelvin); + +/** + * Store the tracker data to be used for calculation. + * + * @param gyro_cal Pointer to the gyro_cal data structure. + */ +static void gyro_temperature_stats_tracker_store(struct gyro_cal *gyro_cal); + +/** + * Compute whether or not the temperature values are in range. + * + * @param gyro_cal Pointer to the gyro_cal data structure. + * @return 'true' if the min and max temperature values exceed the + * range set by 'temperature_delta_limit_kelvin'. + */ +static bool gyro_temperature_stats_tracker_eval(struct gyro_cal *gyro_cal); + +/** + * Tracks the minimum and maximum gyroscope stillness window means. + * Returns + * + * @param gyro_cal Pointer to the gyro_cal data structure. + * @param do_this Command enumerator that controls function behavior. + */ +static void gyro_still_mean_tracker_reset(struct gyro_cal *gyro_cal); + +/** + * Compute the min/max window mean values according to 'window_mean_tracker'. + * + * @param gyro_cal Pointer to the gyro_cal data structure. + */ +static void gyro_still_mean_tracker_update(struct gyro_cal *gyro_cal); + +/** + * Store the most recent "stillness" mean data to the gyro_cal data structure. + * + * @param gyro_cal Pointer to the gyro_cal data structure. + */ +static void gyro_still_mean_tracker_store(struct gyro_cal *gyro_cal); + +/** + * Compute whether or not the gyroscope window range is within the valid range. + * + * @param gyro_cal Pointer to the gyro_cal data structure. + * @return 'true' when the difference between gyroscope min and max + * window means are outside the range set by + * 'stillness_mean_delta_limit'. + */ +static bool gyro_still_mean_tracker_eval(struct gyro_cal *gyro_cal); + +void init_gyro_cal(struct gyro_cal *gyro_cal) +{ + gyro_still_mean_tracker_reset(gyro_cal); + gyro_temperature_stats_tracker_reset(gyro_cal); +} + +void gyro_cal_get_bias(struct gyro_cal *gyro_cal, fpv3_t bias, + int *temperature_kelvin, uint32_t *calibration_time_us) +{ + bias[X] = gyro_cal->bias_x; + bias[Y] = gyro_cal->bias_y; + bias[Z] = gyro_cal->bias_z; + *calibration_time_us = gyro_cal->calibration_time_us; + *temperature_kelvin = gyro_cal->bias_temperature_kelvin; +} + +void gyro_cal_set_bias(struct gyro_cal *gyro_cal, fpv3_t bias, + int temperature_kelvin, uint32_t calibration_time_us) +{ + gyro_cal->bias_x = bias[X]; + gyro_cal->bias_y = bias[Y]; + gyro_cal->bias_z = bias[Z]; + gyro_cal->calibration_time_us = calibration_time_us; + gyro_cal->bias_temperature_kelvin = temperature_kelvin; +} + +void gyro_cal_remove_bias(struct gyro_cal *gyro_cal, fpv3_t in, fpv3_t out) +{ + if (gyro_cal->gyro_calibration_enable) { + out[X] = in[X] - gyro_cal->bias_x; + out[Y] = in[Y] - gyro_cal->bias_y; + out[Z] = in[Z] - gyro_cal->bias_z; + } +} + +bool gyro_cal_new_bias_available(struct gyro_cal *gyro_cal) +{ + bool new_gyro_cal_available = (gyro_cal->gyro_calibration_enable && + gyro_cal->new_gyro_cal_available); + + /* Clear the flag. */ + gyro_cal->new_gyro_cal_available = false; + + return new_gyro_cal_available; +} + +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) +{ + /* + * Make sure that a valid window end-time is set, and start the window + * timer. + */ + if (gyro_cal->stillness_win_endtime_us <= 0) { + gyro_cal->stillness_win_endtime_us = + sample_time_us + gyro_cal->window_time_duration_us; + + /* Start the window timer. */ + gyro_cal->gyro_window_start_us = sample_time_us; + } + + /* Update the temperature statistics. */ + gyro_temperature_stats_tracker_update(gyro_cal, temperature_kelvin); + + /* Pass gyro data to stillness detector */ + gyro_still_det_update(&gyro_cal->gyro_stillness_detect, + gyro_cal->stillness_win_endtime_us, + sample_time_us, x, y, z); + + /* + * Perform a device stillness check, set next window end-time, and + * possibly do a gyro bias calibration and stillness detector reset. + */ + device_stillness_check(gyro_cal, sample_time_us); +} + +void gyro_cal_update_mag(struct gyro_cal *gyro_cal, uint32_t sample_time_us, + fp_t x, fp_t y, fp_t z) +{ + /* Pass magnetometer data to stillness detector. */ + gyro_still_det_update(&gyro_cal->mag_stillness_detect, + gyro_cal->stillness_win_endtime_us, + sample_time_us, x, y, z); + + /* Received a magnetometer sample; incorporate it into detection. */ + gyro_cal->using_mag_sensor = true; + + /* + * Perform a device stillness check, set next window end-time, and + * possibly do a gyro bias calibration and stillness detector reset. + */ + device_stillness_check(gyro_cal, sample_time_us); +} + +void gyro_cal_update_accel(struct gyro_cal *gyro_cal, uint32_t sample_time_us, + fp_t x, fp_t y, fp_t z) +{ + /* Pass accelerometer data to stillnesss detector. */ + gyro_still_det_update(&gyro_cal->accel_stillness_detect, + gyro_cal->stillness_win_endtime_us, + sample_time_us, x, y, z); + + /* + * Perform a device stillness check, set next window end-time, and + * possibly do a gyro bias calibration and stillness detector reset. + */ + device_stillness_check(gyro_cal, sample_time_us); +} + +/** + * Handle the case where the device is found to be still. This function should + * be called from device_stillness_check. + * + * @param gyro_cal Pointer to the gyroscope calibration struct. + */ +static void handle_device_is_still(struct gyro_cal *gyro_cal) +{ + /* + * Device is "still" logic: + * If not previously still, then record the start time. + * If stillness period is too long, then do a calibration. + * Otherwise, continue collecting stillness data. + */ + bool stillness_duration_exceeded = false; + + /* + * If device was not previously still, set new start timestamp. + */ + if (!gyro_cal->prev_still) { + /* + * Record the starting timestamp of the current stillness + * window. This enables the calculation of total duration of + * the stillness period. + */ + gyro_cal->start_still_time_us = + gyro_cal->gyro_stillness_detect.window_start_time; + } + + /* + * Check to see if current stillness period exceeds the desired limit. + */ + stillness_duration_exceeded = + gyro_cal->gyro_stillness_detect.last_sample_time >= + (gyro_cal->start_still_time_us + + gyro_cal->max_still_duration_us); + + /* Track the new stillness mean and temperature data. */ + gyro_still_mean_tracker_store(gyro_cal); + gyro_temperature_stats_tracker_store(gyro_cal); + + if (stillness_duration_exceeded) { + /* + * The current stillness has gone too long. Do a calibration + * with the current data and reset. + */ + + /* + * Updates the gyro bias estimate with the current window data + * and resets the stats. + */ + gyro_still_det_reset(&gyro_cal->accel_stillness_detect, + /*reset_stats=*/true); + gyro_still_det_reset(&gyro_cal->gyro_stillness_detect, + /*reset_stats=*/true); + gyro_still_det_reset(&gyro_cal->mag_stillness_detect, + /*reset_stats=*/true); + + /* + * Resets the local calculations because the stillness + * period is over. + */ + gyro_still_mean_tracker_reset(gyro_cal); + gyro_temperature_stats_tracker_reset(gyro_cal); + + /* Computes a new gyro offset estimate. */ + compute_gyro_cal( + gyro_cal, + gyro_cal->gyro_stillness_detect.last_sample_time); + + /* + * Update stillness flag. Force the start of a new + * stillness period. + */ + gyro_cal->prev_still = false; + } else { + /* Continue collecting stillness data. */ + + /* Extend the stillness period. */ + gyro_still_det_reset(&gyro_cal->accel_stillness_detect, + /*reset_stats=*/false); + gyro_still_det_reset(&gyro_cal->gyro_stillness_detect, + /*reset_stats=*/false); + gyro_still_det_reset(&gyro_cal->mag_stillness_detect, + /*reset_stats=*/false); + + /* Update the stillness flag. */ + gyro_cal->prev_still = true; + } +} + +static void handle_device_not_still(struct gyro_cal *gyro_cal) +{ + /* Device is NOT still; motion detected. */ + + /* + * If device was previously still and the total stillness + * duration is not "too short", then do a calibration with the + * data accumulated thus far. + */ + bool stillness_duration_too_short = + gyro_cal->gyro_stillness_detect.window_start_time < + (gyro_cal->start_still_time_us + + gyro_cal->min_still_duration_us); + + if (gyro_cal->prev_still && !stillness_duration_too_short) + compute_gyro_cal( + gyro_cal, + gyro_cal->gyro_stillness_detect.window_start_time); + + /* Reset the stillness detectors and the stats. */ + gyro_still_det_reset(&gyro_cal->accel_stillness_detect, + /*reset_stats=*/true); + gyro_still_det_reset(&gyro_cal->gyro_stillness_detect, + /*reset_stats=*/true); + gyro_still_det_reset(&gyro_cal->mag_stillness_detect, + /*reset_stats=*/true); + + /* Resets the temperature and sensor mean data. */ + gyro_temperature_stats_tracker_reset(gyro_cal); + gyro_still_mean_tracker_reset(gyro_cal); + + /* Update stillness flag. */ + gyro_cal->prev_still = false; +} + +void device_stillness_check(struct gyro_cal *gyro_cal, uint32_t sample_time_us) +{ + bool min_max_temp_exceeded = false; + bool mean_not_stable = false; + bool device_is_still = false; + fp_t conf_not_rot = INT_TO_FP(0); + fp_t conf_not_accel = INT_TO_FP(0); + fp_t conf_still = INT_TO_FP(0); + + /* Check the window timer. */ + check_window(gyro_cal, sample_time_us); + + /* Is there enough data to do a stillness calculation? */ + if ((!gyro_cal->mag_stillness_detect.stillness_window_ready && + gyro_cal->using_mag_sensor) || + !gyro_cal->accel_stillness_detect.stillness_window_ready || + !gyro_cal->gyro_stillness_detect.stillness_window_ready) + return; /* Not yet, wait for more data. */ + + /* Set the next window end-time for the stillness detectors. */ + gyro_cal->stillness_win_endtime_us = + sample_time_us + gyro_cal->window_time_duration_us; + + /* Update the confidence scores for all sensors. */ + gyro_still_det_compute(&gyro_cal->accel_stillness_detect); + gyro_still_det_compute(&gyro_cal->gyro_stillness_detect); + if (gyro_cal->using_mag_sensor) { + gyro_still_det_compute(&gyro_cal->mag_stillness_detect); + } else { + /* + * Not using magnetometer, force stillness confidence to 100%. + */ + gyro_cal->mag_stillness_detect.stillness_confidence = + INT_TO_FP(1); + } + + /* Updates the mean tracker data. */ + gyro_still_mean_tracker_update(gyro_cal); + + /* + * Determine motion confidence scores (rotation, accelerating, and + * stillness). + */ + conf_not_rot = + fp_mul(gyro_cal->gyro_stillness_detect.stillness_confidence, + gyro_cal->mag_stillness_detect.stillness_confidence); + conf_not_accel = gyro_cal->accel_stillness_detect.stillness_confidence; + conf_still = fp_mul(conf_not_rot, conf_not_accel); + + /* Evaluate the mean and temperature gate functions. */ + mean_not_stable = gyro_still_mean_tracker_eval(gyro_cal); + min_max_temp_exceeded = gyro_temperature_stats_tracker_eval(gyro_cal); + + /* Determines if the device is currently still. */ + device_is_still = (conf_still > gyro_cal->stillness_threshold) && + !mean_not_stable && !min_max_temp_exceeded; + + if (device_is_still) + handle_device_is_still(gyro_cal); + else + handle_device_not_still(gyro_cal); + + /* Reset the window timer after we have processed data. */ + gyro_cal->gyro_window_start_us = sample_time_us; +} + +void compute_gyro_cal(struct gyro_cal *gyro_cal, uint32_t calibration_time_us) +{ + /* Check to see if new calibration values is within acceptable range. */ + if (!(gyro_cal->gyro_stillness_detect.prev_mean[X] < MAX_GYRO_BIAS && + gyro_cal->gyro_stillness_detect.prev_mean[X] > -MAX_GYRO_BIAS && + gyro_cal->gyro_stillness_detect.prev_mean[Y] < MAX_GYRO_BIAS && + gyro_cal->gyro_stillness_detect.prev_mean[Y] > -MAX_GYRO_BIAS && + gyro_cal->gyro_stillness_detect.prev_mean[Z] < MAX_GYRO_BIAS && + gyro_cal->gyro_stillness_detect.prev_mean[Z] > -MAX_GYRO_BIAS)) + /* Outside of range. Ignore, reset, and continue. */ + return; + + /* Record the new gyro bias offset calibration. */ + gyro_cal->bias_x = gyro_cal->gyro_stillness_detect.prev_mean[X]; + gyro_cal->bias_y = gyro_cal->gyro_stillness_detect.prev_mean[Y]; + gyro_cal->bias_z = gyro_cal->gyro_stillness_detect.prev_mean[Z]; + + /* + * Store the calibration temperature (using the mean temperature over + * the "stillness" period). + */ + gyro_cal->bias_temperature_kelvin = gyro_cal->temperature_mean_kelvin; + + /* Store the calibration time stamp. */ + gyro_cal->calibration_time_us = calibration_time_us; + + /* Record the final stillness confidence. */ + gyro_cal->stillness_confidence = fp_mul( + gyro_cal->gyro_stillness_detect.prev_stillness_confidence, + gyro_cal->accel_stillness_detect.prev_stillness_confidence); + gyro_cal->stillness_confidence = fp_mul( + gyro_cal->stillness_confidence, + gyro_cal->mag_stillness_detect.prev_stillness_confidence); + + /* Set flag to indicate a new gyro calibration value is available. */ + gyro_cal->new_gyro_cal_available = true; +} + +void check_window(struct gyro_cal *gyro_cal, uint32_t sample_time_us) +{ + bool window_timeout; + + /* Check for initialization of the window time (=0). */ + if (gyro_cal->gyro_window_start_us <= 0) + return; + + /* + * Checks for the following window timeout conditions: + * i. The current timestamp has exceeded the allowed window duration. + * ii. A timestamp was received that has jumped backwards by more than + * the allowed window duration (e.g., timestamp clock roll-over). + */ + window_timeout = + (sample_time_us > gyro_cal->gyro_window_timeout_duration_us + + gyro_cal->gyro_window_start_us) || + (sample_time_us + gyro_cal->gyro_window_timeout_duration_us < + gyro_cal->gyro_window_start_us); + + /* If a timeout occurred then reset to known good state. */ + if (window_timeout) { + /* Reset stillness detectors and restart data capture. */ + gyro_still_det_reset(&gyro_cal->accel_stillness_detect, + /*reset_stats=*/true); + gyro_still_det_reset(&gyro_cal->gyro_stillness_detect, + /*reset_stats=*/true); + gyro_still_det_reset(&gyro_cal->mag_stillness_detect, + /*reset_stats=*/true); + + /* Resets the temperature and sensor mean data. */ + gyro_temperature_stats_tracker_reset(gyro_cal); + gyro_still_mean_tracker_reset(gyro_cal); + + /* Resets the stillness window end-time. */ + gyro_cal->stillness_win_endtime_us = 0; + + /* Force stillness confidence to zero. */ + gyro_cal->accel_stillness_detect.prev_stillness_confidence = 0; + gyro_cal->gyro_stillness_detect.prev_stillness_confidence = 0; + gyro_cal->mag_stillness_detect.prev_stillness_confidence = 0; + gyro_cal->stillness_confidence = 0; + gyro_cal->prev_still = false; + + /* + * If there are no magnetometer samples being received then + * operate the calibration algorithm without this sensor. + */ + if (!gyro_cal->mag_stillness_detect.stillness_window_ready && + gyro_cal->using_mag_sensor) { + gyro_cal->using_mag_sensor = false; + } + + /* Assert window timeout flags. */ + gyro_cal->gyro_window_start_us = 0; + } +} + +void gyro_temperature_stats_tracker_reset(struct gyro_cal *gyro_cal) +{ + /* Resets the mean accumulator. */ + gyro_cal->temperature_mean_tracker.num_points = 0; + gyro_cal->temperature_mean_tracker.mean_accumulator = INT_TO_FP(0); + + /* Initializes the min/max temperatures values. */ + gyro_cal->temperature_mean_tracker.temperature_min_kelvin = 0x7fff; + gyro_cal->temperature_mean_tracker.temperature_max_kelvin = 0xffff; +} + +void gyro_temperature_stats_tracker_update(struct gyro_cal *gyro_cal, + int temperature_kelvin) +{ + /* Does the mean accumulation. */ + gyro_cal->temperature_mean_tracker.mean_accumulator += + temperature_kelvin; + gyro_cal->temperature_mean_tracker.num_points++; + + /* Tracks the min, max, and latest temperature values. */ + gyro_cal->temperature_mean_tracker.latest_temperature_kelvin = + temperature_kelvin; + if (gyro_cal->temperature_mean_tracker.temperature_min_kelvin > + temperature_kelvin) { + gyro_cal->temperature_mean_tracker.temperature_min_kelvin = + temperature_kelvin; + } + if (gyro_cal->temperature_mean_tracker.temperature_max_kelvin < + temperature_kelvin) { + gyro_cal->temperature_mean_tracker.temperature_max_kelvin = + temperature_kelvin; + } +} + +void gyro_temperature_stats_tracker_store(struct gyro_cal *gyro_cal) +{ + /* + * Store the most recent temperature statistics data to the + * gyro_cal data structure. This functionality allows previous + * results to be recalled when the device suddenly becomes "not + * still". + */ + if (gyro_cal->temperature_mean_tracker.num_points > 0) + gyro_cal->temperature_mean_kelvin = + gyro_cal->temperature_mean_tracker.mean_accumulator / + gyro_cal->temperature_mean_tracker.num_points; + else + gyro_cal->temperature_mean_kelvin = + gyro_cal->temperature_mean_tracker + .latest_temperature_kelvin; +} + +bool gyro_temperature_stats_tracker_eval(struct gyro_cal *gyro_cal) +{ + bool min_max_temp_exceeded = false; + + /* Determines if the min/max delta exceeded the set limit. */ + if (gyro_cal->temperature_mean_tracker.num_points > 0) { + min_max_temp_exceeded = + (gyro_cal->temperature_mean_tracker + .temperature_max_kelvin - + gyro_cal->temperature_mean_tracker + .temperature_min_kelvin) > + gyro_cal->temperature_delta_limit_kelvin; + } + + return min_max_temp_exceeded; +} + +void gyro_still_mean_tracker_reset(struct gyro_cal *gyro_cal) +{ + size_t i; + + /* Resets the min/max window mean values to a default value. */ + for (i = 0; i < 3; i++) { + gyro_cal->window_mean_tracker.gyro_winmean_min[i] = FLT_MAX; + gyro_cal->window_mean_tracker.gyro_winmean_max[i] = -FLT_MAX; + } +} + +void gyro_still_mean_tracker_update(struct gyro_cal *gyro_cal) +{ + int i; + + /* Computes the min/max window mean values. */ + for (i = 0; i < 3; ++i) { + if (gyro_cal->window_mean_tracker.gyro_winmean_min[i] > + gyro_cal->gyro_stillness_detect.win_mean[i]) { + gyro_cal->window_mean_tracker.gyro_winmean_min[i] = + gyro_cal->gyro_stillness_detect.win_mean[i]; + } + if (gyro_cal->window_mean_tracker.gyro_winmean_max[i] < + gyro_cal->gyro_stillness_detect.win_mean[i]) { + gyro_cal->window_mean_tracker.gyro_winmean_max[i] = + gyro_cal->gyro_stillness_detect.win_mean[i]; + } + } +} + +void gyro_still_mean_tracker_store(struct gyro_cal *gyro_cal) +{ + /* + * Store the most recent "stillness" mean data to the gyro_cal + * data structure. This functionality allows previous results to + * be recalled when the device suddenly becomes "not still". + */ + memcpy(gyro_cal->gyro_winmean_min, + gyro_cal->window_mean_tracker.gyro_winmean_min, + sizeof(gyro_cal->window_mean_tracker.gyro_winmean_min)); + memcpy(gyro_cal->gyro_winmean_max, + gyro_cal->window_mean_tracker.gyro_winmean_max, + sizeof(gyro_cal->window_mean_tracker.gyro_winmean_max)); +} + +bool gyro_still_mean_tracker_eval(struct gyro_cal *gyro_cal) +{ + bool mean_not_stable = false; + size_t i; + + /* + * Performs the stability check and returns the 'true' if the + * difference between min/max window mean value is outside the + * stable range. + */ + for (i = 0; i < 3 && !mean_not_stable; i++) { + mean_not_stable |= + (gyro_cal->window_mean_tracker.gyro_winmean_max[i] - + gyro_cal->window_mean_tracker.gyro_winmean_min[i]) > + gyro_cal->stillness_mean_delta_limit; + } + + return mean_not_stable; +} diff --git a/common/gyro_still_det.c b/common/gyro_still_det.c new file mode 100644 index 0000000000..4574e22e5f --- /dev/null +++ b/common/gyro_still_det.c @@ -0,0 +1,242 @@ +/* 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. + */ + +#include "gyro_still_det.h" +#include "vec3.h" + +/* Enforces the limits of an input value [0,1]. */ +static fp_t gyro_still_det_limit(fp_t value); + +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) +{ + fp_t delta = INT_TO_FP(0); + + /* + * Using the method of the assumed mean to preserve some numerical + * stability while avoiding per-sample divisions that the more + * numerically stable Welford method would afford. + * + * Reference for the numerical method used below to compute the + * online mean and variance statistics: + * 1). en.wikipedia.org/wiki/assumed_mean + */ + + /* Increment the number of samples. */ + gyro_still_det->num_acc_samples++; + + /* Online computation of mean for the running stillness period. */ + gyro_still_det->mean[X] += x; + gyro_still_det->mean[Y] += y; + gyro_still_det->mean[Z] += z; + + /* Is this the first sample of a new window? */ + if (gyro_still_det->start_new_window) { + /* Record the window start time. */ + gyro_still_det->window_start_time = sample_time; + gyro_still_det->start_new_window = false; + + /* Update assumed mean values. */ + gyro_still_det->assumed_mean[X] = x; + gyro_still_det->assumed_mean[Y] = y; + gyro_still_det->assumed_mean[Z] = z; + + /* Reset current window mean and variance. */ + gyro_still_det->num_acc_win_samples = 0; + gyro_still_det->win_mean[X] = INT_TO_FP(0); + gyro_still_det->win_mean[Y] = INT_TO_FP(0); + gyro_still_det->win_mean[Z] = INT_TO_FP(0); + gyro_still_det->acc_var[X] = INT_TO_FP(0); + gyro_still_det->acc_var[Y] = INT_TO_FP(0); + gyro_still_det->acc_var[Z] = INT_TO_FP(0); + } else { + /* + * Check to see if we have enough samples to compute a stillness + * confidence score. + */ + gyro_still_det->stillness_window_ready = + (sample_time >= stillness_win_endtime) && + (gyro_still_det->num_acc_samples > 1); + } + + /* Record the most recent sample time stamp. */ + gyro_still_det->last_sample_time = sample_time; + + /* Online window mean and variance ("one-pass" accumulation). */ + gyro_still_det->num_acc_win_samples++; + + delta = (x - gyro_still_det->assumed_mean[X]); + gyro_still_det->win_mean[X] += delta; + gyro_still_det->acc_var[X] += fp_sq(delta); + + delta = (y - gyro_still_det->assumed_mean[Y]); + gyro_still_det->win_mean[Y] += delta; + gyro_still_det->acc_var[Y] += fp_sq(delta); + + delta = (z - gyro_still_det->assumed_mean[Z]); + gyro_still_det->win_mean[Z] += delta; + gyro_still_det->acc_var[Z] += fp_sq(delta); +} + +fp_t gyro_still_det_compute(struct gyro_still_det *gyro_still_det) +{ + fp_t tmp_denom = INT_TO_FP(1); + fp_t tmp_denom_mean = INT_TO_FP(1); + fp_t tmp; + fp_t upper_var_thresh, lower_var_thresh; + + /* Don't divide by zero (not likely, but a precaution). */ + if (gyro_still_det->num_acc_win_samples > 1) { + tmp_denom = fp_div( + tmp_denom, + INT_TO_FP(gyro_still_det->num_acc_win_samples - 1)); + tmp_denom_mean = + fp_div(tmp_denom_mean, + INT_TO_FP(gyro_still_det->num_acc_win_samples)); + } else { + /* Return zero stillness confidence. */ + gyro_still_det->stillness_confidence = 0; + return gyro_still_det->stillness_confidence; + } + + /* Update the final calculation of window mean and variance. */ + tmp = gyro_still_det->win_mean[X]; + gyro_still_det->win_mean[X] = + fp_mul(gyro_still_det->win_mean[X], tmp_denom_mean); + gyro_still_det->win_var[X] = + fp_mul((gyro_still_det->acc_var[X] - + fp_mul(gyro_still_det->win_mean[X], tmp)), + tmp_denom); + + tmp = gyro_still_det->win_mean[Y]; + gyro_still_det->win_mean[Y] = + fp_mul(gyro_still_det->win_mean[Y], tmp_denom_mean); + gyro_still_det->win_var[Y] = + fp_mul((gyro_still_det->acc_var[Y] - + fp_mul(gyro_still_det->win_mean[Y], tmp)), + tmp_denom); + + tmp = gyro_still_det->win_mean[Z]; + gyro_still_det->win_mean[Z] = + fp_mul(gyro_still_det->win_mean[Z], tmp_denom_mean); + gyro_still_det->win_var[Z] = + fp_mul((gyro_still_det->acc_var[Z] - + fp_mul(gyro_still_det->win_mean[Z], tmp)), + tmp_denom); + + /* Adds the assumed mean value back to the total mean calculation. */ + gyro_still_det->win_mean[X] += gyro_still_det->assumed_mean[X]; + gyro_still_det->win_mean[Y] += gyro_still_det->assumed_mean[Y]; + gyro_still_det->win_mean[Z] += gyro_still_det->assumed_mean[Z]; + + /* Define the variance thresholds. */ + upper_var_thresh = gyro_still_det->var_threshold + + gyro_still_det->confidence_delta; + + lower_var_thresh = gyro_still_det->var_threshold - + gyro_still_det->confidence_delta; + + /* Compute the stillness confidence score. */ + if ((gyro_still_det->win_var[X] > upper_var_thresh) || + (gyro_still_det->win_var[Y] > upper_var_thresh) || + (gyro_still_det->win_var[Z] > upper_var_thresh)) { + /* + * Sensor variance exceeds the upper threshold (i.e., motion + * detected). Set stillness confidence equal to 0. + */ + gyro_still_det->stillness_confidence = 0; + } else if ((gyro_still_det->win_var[X] <= lower_var_thresh) && + (gyro_still_det->win_var[Y] <= lower_var_thresh) && + (gyro_still_det->win_var[Z] <= lower_var_thresh)) { + /* + * Sensor variance is below the lower threshold (i.e. + * stillness detected). + * Set stillness confidence equal to 1. + */ + gyro_still_det->stillness_confidence = INT_TO_FP(1); + } else { + /* + * Motion detection thresholds not exceeded. Compute the + * stillness confidence score. + */ + fp_t var_thresh = gyro_still_det->var_threshold; + fpv3_t limit; + + /* + * Compute the stillness confidence score. + * Each axis score is limited [0,1]. + */ + tmp_denom = fp_div(INT_TO_FP(1), + (upper_var_thresh - lower_var_thresh)); + limit[X] = gyro_still_det_limit( + FLOAT_TO_FP(0.5f) - + fp_mul(gyro_still_det->win_var[X] - var_thresh, + tmp_denom)); + limit[Y] = gyro_still_det_limit( + FLOAT_TO_FP(0.5f) - + fp_mul(gyro_still_det->win_var[Y] - var_thresh, + tmp_denom)); + limit[Z] = gyro_still_det_limit( + FLOAT_TO_FP(0.5f) - + fp_mul(gyro_still_det->win_var[Z] - var_thresh, + tmp_denom)); + + gyro_still_det->stillness_confidence = + fp_mul(limit[X], fp_mul(limit[Y], limit[Z])); + } + + /* Return the stillness confidence. */ + return gyro_still_det->stillness_confidence; +} + +void gyro_still_det_reset(struct gyro_still_det *gyro_still_det, + bool reset_stats) +{ + fp_t tmp_denom = INT_TO_FP(1); + + /* Reset the stillness data ready flag. */ + gyro_still_det->stillness_window_ready = false; + + /* Signal to start capture of next stillness data window. */ + gyro_still_det->start_new_window = true; + + /* Track the stillness confidence (current->previous). */ + gyro_still_det->prev_stillness_confidence = + gyro_still_det->stillness_confidence; + + /* Track changes in the mean estimate. */ + if (gyro_still_det->num_acc_samples > INT_TO_FP(1)) + tmp_denom = + fp_div(INT_TO_FP(1), gyro_still_det->num_acc_samples); + + gyro_still_det->prev_mean[X] = + fp_mul(gyro_still_det->mean[X], tmp_denom); + gyro_still_det->prev_mean[Y] = + fp_mul(gyro_still_det->mean[Y], tmp_denom); + gyro_still_det->prev_mean[Z] = + fp_mul(gyro_still_det->mean[Z], tmp_denom); + + /* Reset the current statistics to zero. */ + if (reset_stats) { + gyro_still_det->num_acc_samples = 0; + gyro_still_det->mean[X] = INT_TO_FP(0); + gyro_still_det->mean[Y] = INT_TO_FP(0); + gyro_still_det->mean[Z] = INT_TO_FP(0); + gyro_still_det->acc_var[X] = INT_TO_FP(0); + gyro_still_det->acc_var[Y] = INT_TO_FP(0); + gyro_still_det->acc_var[Z] = INT_TO_FP(0); + } +} + +fp_t gyro_still_det_limit(fp_t value) +{ + if (value < INT_TO_FP(0)) + value = INT_TO_FP(0); + else if (value > INT_TO_FP(1)) + value = INT_TO_FP(1); + + return value; +} diff --git a/common/online_calibration.c b/common/online_calibration.c index 6c090a6abc..ca7a5a620d 100644 --- a/common/online_calibration.c +++ b/common/online_calibration.c @@ -15,8 +15,9 @@ #include "ec_commands.h" #include "accel_cal.h" #include "mkbp_event.h" +#include "gyro_cal.h" -#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args) +#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args) #ifndef CONFIG_MKBP_EVENT #error "Must use CONFIG_MKBP_EVENT for online calibration" @@ -40,7 +41,7 @@ static int get_temperature(struct motion_sensor_t *sensor, int *temp) now = __hw_clock_source_read(); if (entry->last_temperature < 0 || time_until(entry->last_temperature_timestamp, now) > - CONFIG_TEMP_CACHE_STALE_THRES) { + CONFIG_TEMP_CACHE_STALE_THRES) { int t; int rc = sensor->drv->read_temp(sensor, &t); @@ -72,8 +73,8 @@ static void data_int16_to_fp(const struct motion_sensor_t *s, } } -static void data_fp_to_int16(const struct motion_sensor_t *s, - const fpv3_t data, int16_t *out) +static void data_fp_to_int16(const struct motion_sensor_t *s, const fpv3_t data, + int16_t *out) { int i; fp_t range = INT_TO_FP(s->drv->get_range(s)); @@ -82,14 +83,102 @@ static void data_fp_to_int16(const struct motion_sensor_t *s, int32_t iv; fp_t v = fp_div(data[i], range); - v = fp_mul(v, INT_TO_FP( - (data[i] >= INT_TO_FP(0)) ? 0x7fff : 0x8000)); + v = fp_mul(v, INT_TO_FP((data[i] >= INT_TO_FP(0)) ? 0x7fff : + 0x8000)); iv = FP_TO_INT(v); /* Check for overflow */ out[i] = CLAMP(iv, (int32_t)0xffff8000, (int32_t)0x00007fff); } } +/** + * Check a gyroscope for new bias. This function checks a given sensor (must be + * a gyroscope) for new bias values. If found, it will update the appropriate + * caches and notify the AP. + * + * @param sensor Pointer to the gyroscope sensor to check. + */ +static void check_gyro_cal_new_bias(struct motion_sensor_t *sensor) +{ + struct online_calib_data *calib_data = + (struct online_calib_data *)sensor->online_calib_data; + struct gyro_cal_data *data = + (struct gyro_cal_data *)calib_data->type_specific_data; + size_t sensor_num = motion_sensors - sensor; + int temp_out; + fpv3_t bias_out; + uint32_t timestamp_out; + + /* Check that we have a new bias. */ + if (data == NULL || calib_data == NULL || + !gyro_cal_new_bias_available(&data->gyro_cal)) + return; + + /* Read the calibration values. */ + gyro_cal_get_bias(&data->gyro_cal, bias_out, &temp_out, ×tamp_out); + + mutex_lock(&g_calib_cache_mutex); + /* Convert result to the right scale. */ + data_fp_to_int16(sensor, bias_out, calib_data->cache); + /* Set valid and dirty. */ + sensor_calib_cache_valid_map |= BIT(sensor_num); + sensor_calib_cache_dirty_map |= BIT(sensor_num); + mutex_unlock(&g_calib_cache_mutex); + /* Notify the AP. */ + mkbp_send_event(EC_MKBP_EVENT_ONLINE_CALIBRATION); +} + +/** + * Update the data stream (accel/mag) for a given sensor and data in all + * gyroscopes that are interested. + * + * @param sensor Pointer to the sensor that generated the data. + * @param data 3 floats/fixed point data points generated by the sensor. + * @param timestamp The timestamp at which the data was generated. + */ +static void update_gyro_cal(struct motion_sensor_t *sensor, fpv3_t data, + uint32_t timestamp) +{ + int i; + + /* + * Find gyroscopes, while we don't currently have instance where more + * than one are present in a board, this loop will work with any number + * of them. + */ + for (i = 0; i < SENSOR_COUNT; ++i) { + struct motion_sensor_t *s = motion_sensors + i; + struct gyro_cal_data *gyro_cal_data = + (struct gyro_cal_data *) + s->online_calib_data->type_specific_data; + + /* + * If we're not looking at a gyroscope OR if the calibration + * data is NULL, skip this sensor. + */ + if (s->type != MOTIONSENSE_TYPE_GYRO || gyro_cal_data == NULL) + continue; + + /* + * Update the appropriate data stream (accel/mag) depending on + * which sensors the gyroscope is tracking. + */ + if (sensor->type == MOTIONSENSE_TYPE_ACCEL && + gyro_cal_data->accel_sensor_id == sensor - motion_sensors) { + gyro_cal_update_accel(&gyro_cal_data->gyro_cal, + timestamp, data[X], data[Y], + data[Z]); + check_gyro_cal_new_bias(s); + } else if (sensor->type == MOTIONSENSE_TYPE_MAG && + gyro_cal_data->mag_sensor_id == + sensor - motion_sensors) { + gyro_cal_update_mag(&gyro_cal_data->gyro_cal, timestamp, + data[X], data[Y], data[Z]); + check_gyro_cal_new_bias(s); + } + } +} + void online_calibration_init(void) { size_t i; @@ -116,6 +205,12 @@ void online_calibration_init(void) init_mag_cal((struct mag_cal_t *)type_specific_data); break; } + case MOTIONSENSE_TYPE_GYRO: { + init_gyro_cal( + &((struct gyro_cal_data *)type_specific_data) + ->gyro_cal); + break; + } default: break; } @@ -141,8 +236,7 @@ bool online_calibration_read(int sensor_num, int16_t out[3]) has_valid = (sensor_calib_cache_valid_map & BIT(sensor_num)) != 0; if (has_valid) { /* Update data in out */ - memcpy(out, - motion_sensors[sensor_num].online_calib_data->cache, + memcpy(out, motion_sensors[sensor_num].online_calib_data->cache, sizeof(out)); /* Clear dirty bit */ sensor_calib_cache_dirty_map &= ~(1 << sensor_num); @@ -152,10 +246,9 @@ bool online_calibration_read(int sensor_num, int16_t out[3]) return has_valid; } -int online_calibration_process_data( - struct ec_response_motion_sensor_data *data, - struct motion_sensor_t *sensor, - uint32_t timestamp) +int online_calibration_process_data(struct ec_response_motion_sensor_data *data, + struct motion_sensor_t *sensor, + uint32_t timestamp) { size_t sensor_num = motion_sensors - sensor; int rc; @@ -169,20 +262,25 @@ int online_calibration_process_data( (struct accel_cal *)(calib_data->type_specific_data); fpv3_t fdata; + /* Convert data to fp. */ + data_int16_to_fp(sensor, data->data, fdata); + + /* Possibly update the gyroscope calibration. */ + update_gyro_cal(sensor, fdata, timestamp); + /* Temperature is required for accelerometer calibration. */ rc = get_temperature(sensor, &temperature); if (rc != EC_SUCCESS) return rc; - data_int16_to_fp(sensor, data->data, fdata); if (accel_cal_accumulate(cal, timestamp, fdata[X], fdata[Y], fdata[Z], temperature)) { mutex_lock(&g_calib_cache_mutex); /* Convert result to the right scale. */ data_fp_to_int16(sensor, cal->bias, calib_data->cache); /* Set valid and dirty. */ - sensor_calib_cache_valid_map |= BIT(sensor_num); - sensor_calib_cache_dirty_map |= BIT(sensor_num); + sensor_calib_cache_valid_map |= BIT(sensor_num); + sensor_calib_cache_dirty_map |= BIT(sensor_num); mutex_unlock(&g_calib_cache_mutex); /* Notify the AP. */ mkbp_send_event(EC_MKBP_EVENT_ONLINE_CALIBRATION); @@ -191,12 +289,19 @@ int online_calibration_process_data( } case MOTIONSENSE_TYPE_MAG: { struct mag_cal_t *cal = - (struct mag_cal_t *) (calib_data->type_specific_data); + (struct mag_cal_t *)(calib_data->type_specific_data); int idata[] = { (int)data->data[X], (int)data->data[Y], (int)data->data[Z], }; + fpv3_t fdata; + + /* Convert data to fp. */ + data_int16_to_fp(sensor, data->data, fdata); + + /* Possibly update the gyroscope calibration. */ + update_gyro_cal(sensor, fdata, timestamp); if (mag_cal_update(cal, idata)) { mutex_lock(&g_calib_cache_mutex); @@ -213,10 +318,27 @@ int online_calibration_process_data( } break; } + case MOTIONSENSE_TYPE_GYRO: { + fpv3_t fdata; + + /* Temperature is required for gyro calibration. */ + rc = get_temperature(sensor, &temperature); + if (rc != EC_SUCCESS) + return rc; + + /* Convert data to fp. */ + data_int16_to_fp(sensor, data->data, fdata); + + /* Update gyroscope calibration. */ + gyro_cal_update_gyro( + &((struct gyro_cal_data *)calib_data->type_specific_data)->gyro_cal, + timestamp, fdata[X], fdata[Y], fdata[Z], temperature); + check_gyro_cal_new_bias(sensor); + break; + } default: break; } return EC_SUCCESS; } - 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 /* diff --git a/test/build.mk b/test/build.mk index 8aa9059f23..6c69f639c1 100644 --- a/test/build.mk +++ b/test/build.mk @@ -35,6 +35,7 @@ test-list-host += fp test-list-host += fpsensor test-list-host += fpsensor_crypto test-list-host += fpsensor_state +test-list-host += gyro_cal test-list-host += hooks test-list-host += host_command test-list-host += i2c_bitbang @@ -139,6 +140,7 @@ flash_write_protect-y=flash_write_protect.o fpsensor-y=fpsensor.o fpsensor_crypto-y=fpsensor_crypto.o fpsensor_state-y=fpsensor_state.o +gyro_cal-y=gyro_cal.o hooks-y=hooks.o host_command-y=host_command.o i2c_bitbang-y=i2c_bitbang.o diff --git a/test/gyro_cal.c b/test/gyro_cal.c new file mode 100644 index 0000000000..85d53789ef --- /dev/null +++ b/test/gyro_cal.c @@ -0,0 +1,611 @@ +/* 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. + */ + +#include "common.h" +#include "gyro_cal.h" +#include "gyro_still_det.h" +#include "motion_sense.h" +#include "test_util.h" +#include <string.h> +#include <stdlib.h> +#include <math.h> +#include <stdio.h> + +float kToleranceGyroRps = 1e-6f; +float kDefaultGravityMps2 = 9.81f; +int kDefaultTemperatureKelvin = 298; + +#define NANOS_TO_SEC (1.0e-9f) +#define NANO_PI (3.14159265359f) +/** Unit conversion: milli-degrees to radians. */ +#define MDEG_TO_RAD (NANO_PI / 180.0e3f) + +#define MSEC_TO_NANOS(x) ((uint64_t)((x) * (uint64_t)(1000000))) +#define SEC_TO_NANOS(x) MSEC_TO_NANOS((x) * (uint64_t)(1000)) +#define HZ_TO_PERIOD_NANOS(hz) (SEC_TO_NANOS(1024) / ((uint64_t)((hz)*1024))) + +struct motion_sensor_t motion_sensors[] = { + [BASE] = {}, + [LID] = {}, +}; + +const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); + +/** + * This function will return a uniformly distributed random value in the range + * of (0,1). This is important that 0 and 1 are excluded because of how the + * value is used in normal_random. For references: + * - rand() / RAND_MAX yields the range [0,1] + * - rand() / (RAND_MAX + 1) yields the range [0,1) + * - (rand() + 1) / (RAND_MAX + 1) yields the range (0, 1) + * + * @return A uniformly distributed random value. + */ +static double rand_gen(void) +{ + return ((double)(rand()) + 1.0) / ((double)(RAND_MAX) + 1.0); +} + +/** + * @return A normally distributed random value + */ +static float normal_random(void) +{ + double v1 = rand_gen(); + double v2 = rand_gen(); + + return (float)(cos(2 * 3.14 * v2) * sqrt(-2.0 * log(v1))); +} + +/** + * @param mean The mean to use for the normal distribution. + * @param stddev The standard deviation to use for the normal distribution. + * @return A normally distributed random value based on mean and stddev. + */ +static float normal_random2(float mean, float stddev) +{ + return normal_random() * stddev + mean; +} + +/** + * + * @param det Pointer to the stillness detector + * @param var_threshold The variance threshold in units^2 + * @param confidence_delta The confidence delta in units^2 + */ +static void gyro_still_det_initialization_for_test(struct gyro_still_det *det, + float var_threshold, + float confidence_delta) +{ + /* Clear all data structure variables to 0. */ + memset(det, 0, sizeof(struct gyro_still_det)); + + /* + * Set the delta about the variance threshold for calculation + * of the stillness confidence score. + */ + if (confidence_delta < var_threshold) + det->confidence_delta = confidence_delta; + else + det->confidence_delta = var_threshold; + + /* + * Set the variance threshold parameter for the stillness + * confidence score. + */ + det->var_threshold = var_threshold; + + /* Signal to start capture of next stillness data window. */ + det->start_new_window = true; +} + +static void gyro_cal_initialization_for_test(struct gyro_cal *gyro_cal) +{ + /* GyroCal initialization. */ + memset(gyro_cal, 0, sizeof(struct gyro_cal)); + + /* + * Initialize the stillness detectors. + * Gyro parameter input units are [rad/sec]. + * Accel parameter input units are [m/sec^2]. + * Magnetometer parameter input units are [uT]. + */ + gyro_still_det_initialization_for_test(&gyro_cal->gyro_stillness_detect, + /* var_threshold */ 5e-5f, + /* confidence_delta */ 1e-5f); + gyro_still_det_initialization_for_test( + &gyro_cal->accel_stillness_detect, + /* var_threshold */ 8e-3f, + /* confidence_delta */ 1.6e-3f); + gyro_still_det_initialization_for_test(&gyro_cal->mag_stillness_detect, + /* var_threshold */ 1.4f, + /* confidence_delta */ 0.25f); + + /* Reset stillness flag and start timestamp. */ + gyro_cal->prev_still = false; + gyro_cal->start_still_time_us = 0; + + /* Set the min and max window stillness duration. */ + gyro_cal->min_still_duration_us = 5 * SECOND; + gyro_cal->max_still_duration_us = 6 * SECOND; + + /* Sets the duration of the stillness processing windows. */ + gyro_cal->window_time_duration_us = 1500000; + + /* Set the window timeout duration. */ + gyro_cal->gyro_window_timeout_duration_us = 5 * SECOND; + + /* Load the last valid cal from system memory. */ + gyro_cal->bias_x = 0.0f; /* [rad/sec] */ + gyro_cal->bias_y = 0.0f; /* [rad/sec] */ + gyro_cal->bias_z = 0.0f; /* [rad/sec] */ + gyro_cal->calibration_time_us = 0; + + /* Set the stillness threshold required for gyro bias calibration. */ + gyro_cal->stillness_threshold = 0.95f; + + /* + * Current window end-time used to assist in keeping sensor data + * collection in sync. Setting this to zero signals that sensor data + * will be dropped until a valid end-time is set from the first gyro + * timestamp received. + */ + gyro_cal->stillness_win_endtime_us = 0; + + /* Gyro calibrations will be applied (see, gyro_cal_remove_bias()). */ + gyro_cal->gyro_calibration_enable = true; + + /* + * Sets the stability limit for the stillness window mean acceptable + * delta. + */ + gyro_cal->stillness_mean_delta_limit = 50.0f * MDEG_TO_RAD; + + /* Sets the min/max temperature delta limit for the stillness period. */ + gyro_cal->temperature_delta_limit_kelvin = 1.5f; + + /* Ensures that the data tracking functionality is reset. */ + init_gyro_cal(gyro_cal); +} + +/** + * Tests that a calibration is updated after a period where the IMU device is + * stationary. Accelerometer and gyroscope measurements are simulated with data + * sheet specs for the BMI160 at their respective noise floors. A magnetometer + * sensor is also included in this test. + * + * @return EC_SUCCESS on success. + */ +static int test_gyro_cal_calibration(void) +{ + int i; + struct gyro_cal gyro_cal; + + /* + * Statistics for simulated gyroscope data. + * RMS noise = 70mDPS, offset = 150mDPS. + */ + /* [Hz] */ + const float sample_rate = 400.0f; + /* [rad/sec] */ + const float gyro_bias = MDEG_TO_RAD * 150.0f; + /* [rad/sec] */ + const float gyro_rms_noise = MDEG_TO_RAD * 70.0f; + const uint64_t sample_interval_nanos = HZ_TO_PERIOD_NANOS(sample_rate); + + /* + * Statistics for simulated accelerometer data. + * noise density = 200ug/rtHz, offset = 50mg. + */ + /* [m/sec^2] */ + const float accel_bias = 0.05f * kDefaultGravityMps2; + /* [m/sec^2] */ + const float accel_rms_noise = + 0.0002f * kDefaultGravityMps2 * fp_sqrtf(0.5f * sample_rate); + + /* + * Statistics for simulated magnetometer data. + * RMS noise = 0.4 micro Tesla (uT), offset = 0.2uT. + */ + const float mag_bias = 0.2f; /* [uT] */ + const float mag_rms_noise = 0.4f; /* [uT] */ + + float bias[3]; + float bias_residual[3]; + int temperature_kelvin; + uint32_t calibration_time_us = 0; + + bool calibration_received = false; + + gyro_cal_initialization_for_test(&gyro_cal); + + /* No calibration should be available yet. */ + TEST_EQ(gyro_cal_new_bias_available(&gyro_cal), false, "%d"); + + /* + * Simulate up to 20 seconds of sensor data (zero mean, additive white + * Gaussian noise). + */ + for (i = 0; i < (int)(20.0f * sample_rate); ++i) { + const uint32_t timestamp_us = + (i * sample_interval_nanos) / 1000; + + /* Generate and add an accelerometer sample. */ + gyro_cal_update_accel( + &gyro_cal, timestamp_us, + normal_random2(accel_bias, accel_rms_noise), + normal_random2(accel_bias, accel_rms_noise), + normal_random2(accel_bias, accel_rms_noise)); + + /* Generate and add a gyroscrope sample. */ + gyro_cal_update_gyro(&gyro_cal, timestamp_us, + normal_random2(gyro_bias, gyro_rms_noise), + normal_random2(gyro_bias, gyro_rms_noise), + normal_random2(gyro_bias, gyro_rms_noise), + kDefaultTemperatureKelvin); + + /* + * The simulated magnetometer here has a sampling rate that is + * 4x slower than the accel/gyro + */ + if (i % 4 == 0) { + gyro_cal_update_mag( + &gyro_cal, timestamp_us, + normal_random2(mag_bias, mag_rms_noise), + normal_random2(mag_bias, mag_rms_noise), + normal_random2(mag_bias, mag_rms_noise)); + } + calibration_received = gyro_cal_new_bias_available(&gyro_cal); + if (calibration_received) + break; + } + + TEST_EQ(calibration_received, true, "%d"); + + gyro_cal_get_bias(&gyro_cal, bias, &temperature_kelvin, + &calibration_time_us); + bias_residual[0] = gyro_bias - bias[0]; + bias_residual[1] = gyro_bias - bias[1]; + bias_residual[2] = gyro_bias - bias[2]; + + /* + * Make sure that the bias estimate is within 20 milli-degrees per + * second. + */ + TEST_LT(bias_residual[0], 20.f * MDEG_TO_RAD, "%f"); + TEST_LT(bias_residual[1], 20.f * MDEG_TO_RAD, "%f"); + TEST_LT(bias_residual[2], 20.f * MDEG_TO_RAD, "%f"); + + TEST_NEAR(gyro_cal.stillness_confidence, 1.0f, 0.0001f, "%f"); + + TEST_EQ(temperature_kelvin, kDefaultTemperatureKelvin, "%d"); + + return EC_SUCCESS; +} + +/** + * Tests that calibration does not falsely occur for low-level motion. + * + * @return EC_SUCCESS on success. + */ +static int test_gyro_cal_no_calibration(void) +{ + int i; + struct gyro_cal gyro_cal; + + /* Statistics for simulated gyroscope data. */ + /* RMS noise = 70mDPS, offset = 150mDPS. */ + const float sample_rate = 400.0f; /* [Hz] */ + const float gyro_bias = MDEG_TO_RAD * 150.0f; /* [rad/sec] */ + const float gyro_rms_noise = MDEG_TO_RAD * 70.0f; /* [rad/sec] */ + const uint64_t sample_interval_nanos = HZ_TO_PERIOD_NANOS(sample_rate); + + /* Statistics for simulated accelerometer data. */ + /* noise density = 200ug/rtHz, offset = 50mg. */ + /* [m/sec^2] */ + const float accel_bias = 0.05f * kDefaultGravityMps2; + /* [m/sec^2] */ + const float accel_rms_noise = + 200.0e-6f * kDefaultGravityMps2 * fp_sqrtf(0.5f * sample_rate); + + /* Define sinusoidal gyroscope motion parameters. */ + const float omega_dt = + 2.0f * NANO_PI * sample_interval_nanos * NANOS_TO_SEC; + const float amplitude = MDEG_TO_RAD * 550.0f; /* [rad/sec] */ + + bool calibration_received = false; + + gyro_cal_initialization_for_test(&gyro_cal); + + for (i = 0; i < (int)(20.0f * sample_rate); ++i) { + const uint32_t timestamp_us = + (i * sample_interval_nanos) / 1000; + + /* Generate and add an accelerometer sample. */ + gyro_cal_update_accel( + &gyro_cal, timestamp_us, + normal_random2(accel_bias, accel_rms_noise), + normal_random2(accel_bias, accel_rms_noise), + normal_random2(accel_bias, accel_rms_noise)); + + /* Generate and add a gyroscope sample. */ + gyro_cal_update_gyro( + &gyro_cal, timestamp_us, + normal_random2(gyro_bias, gyro_rms_noise) + + amplitude * sin(2.0f * omega_dt * i), + normal_random2(gyro_bias, gyro_rms_noise) - + amplitude * sin(2.1f * omega_dt * i), + normal_random2(gyro_bias, gyro_rms_noise) + + amplitude * cos(4.3f * omega_dt * i), + kDefaultTemperatureKelvin); + + /* Check for calibration update. Break after first one. */ + calibration_received = gyro_cal_new_bias_available(&gyro_cal); + if (calibration_received) + break; + } + + /* Determine that NO calibration had occurred. */ + TEST_EQ(calibration_received, false, "%d"); + + /* Make sure that the device was NOT classified as "still". */ + TEST_GT(1.0f, gyro_cal.stillness_confidence, "%f"); + + return EC_SUCCESS; +} + +/** + * Tests that a shift in a stillness window mean does not trigger a calibration. + * + * @return EC_SUCCESS on success. + */ +static int test_gyro_cal_win_mean_shift(void) +{ + struct gyro_cal gyro_cal; + int i; + + /* Statistics for simulated gyroscope data. */ + const float sample_rate = 400.0f; /* [Hz] */ + const float gyro_bias = MDEG_TO_RAD * 150.0f; /* [rad/sec] */ + const float gyro_bias_shift = MDEG_TO_RAD * 60.0f; /* [rad/sec] */ + const uint64_t sample_interval_nanos = HZ_TO_PERIOD_NANOS(sample_rate); + + /* Initialize the gyro calibration. */ + gyro_cal_initialization_for_test(&gyro_cal); + + /* + * Simulates 8 seconds of sensor data (no noise, just a gyro mean shift + * after 4 seconds). + * Assumptions: The max stillness period is 6 seconds, and the mean + * delta limit is 50mDPS. The mean shift should be detected and exceed + * the 50mDPS limit, and no calibration should be triggered. NOTE: This + * step is not large enough to trip the variance checking within the + * stillness detectors. + */ + for (i = 0; i < (int)(8.0f * sample_rate); i++) { + const uint32_t timestamp_us = + (i * sample_interval_nanos) / 1000; + + /* Generate and add a accelerometer sample. */ + gyro_cal_update_accel(&gyro_cal, timestamp_us, 0.0f, 0.0f, + 9.81f); + + /* Generate and add a gyroscope sample. */ + if (timestamp_us > 4 * SECOND) { + gyro_cal_update_gyro(&gyro_cal, timestamp_us, + gyro_bias + gyro_bias_shift, + gyro_bias + gyro_bias_shift, + gyro_bias + gyro_bias_shift, + kDefaultTemperatureKelvin); + } else { + gyro_cal_update_gyro(&gyro_cal, timestamp_us, gyro_bias, + gyro_bias, gyro_bias, + kDefaultTemperatureKelvin); + } + } + + /* Determine that NO calibration had occurred. */ + TEST_EQ(gyro_cal_new_bias_available(&gyro_cal), false, "%d"); + + return EC_SUCCESS; +} + +/** + * Tests that a temperature variation outside the acceptable range prevents a + * calibration. + * + * @return EC_SUCCESS on success. + */ +static int test_gyro_cal_temperature_shift(void) +{ + int i; + struct gyro_cal gyro_cal; + + /* Statistics for simulated gyroscope data. */ + const float sample_rate = 400.0f; /* [Hz] */ + const float gyro_bias = MDEG_TO_RAD * 150.0f; /* [rad/sec] */ + const float temperature_shift_kelvin = 2.6f; + const uint64_t sample_interval_nanos = HZ_TO_PERIOD_NANOS(sample_rate); + + gyro_cal_initialization_for_test(&gyro_cal); + + /* + * Simulates 8 seconds of sensor data (no noise, just a temperature + * shift after 4 seconds). + * Assumptions: The max stillness period is 6 seconds, and the + * temperature delta limit is 1.5C. The shift should be detected and + * exceed the limit, and no calibration should be triggered. + */ + for (i = 0; i < (int)(8.0f * sample_rate); i++) { + const uint32_t timestamp_us = + (i * sample_interval_nanos) / 1000; + float temperature_kelvin = kDefaultTemperatureKelvin; + + /* Generate and add a accelerometer sample. */ + gyro_cal_update_accel(&gyro_cal, timestamp_us, 0.0f, 0.0f, + 9.81f); + + /* Sets the temperature value. */ + if (timestamp_us > 4 * SECOND) + temperature_kelvin += temperature_shift_kelvin; + + /* Generate and add a gyroscope sample. */ + gyro_cal_update_gyro(&gyro_cal, timestamp_us, gyro_bias, + gyro_bias, gyro_bias, + (int)temperature_kelvin); + } + + /* Determine that NO calibration had occurred. */ + TEST_EQ(gyro_cal_new_bias_available(&gyro_cal), false, "%d"); + + return EC_SUCCESS; +} + +/** + * Verifies that complete sensor stillness results in the correct bias estimate + * and produces the correct timestamp. + * + * @return EC_SUCCESS on success; + */ +static int test_gyro_cal_stillness_timestamp(void) +{ + struct gyro_cal gyro_cal; + int64_t time_us; + + /* + * 10Hz update rate for 11 seconds should trigger the in-situ + * algorithms. + */ + const float gyro_bias_x = 0.09f; + const float gyro_bias_y = -0.04f; + const float gyro_bias_z = 0.05f; + + float bias[3]; + int temperature_kelvin = 273; + uint32_t calibration_time_us = 0; + + gyro_cal_initialization_for_test(&gyro_cal); + for (time_us = 0; time_us < 11 * SECOND; time_us += 100 * MSEC) { + /* Generate and add a accelerometer sample. */ + gyro_cal_update_accel(&gyro_cal, time_us, 0.0f, 0.0f, 9.81f); + + /* Generate and add a gyroscope sample. */ + gyro_cal_update_gyro(&gyro_cal, time_us, gyro_bias_x, + gyro_bias_y, gyro_bias_z, + kDefaultTemperatureKelvin); + } + + /* Determine if there is a new calibration. Get the calibration value. + */ + TEST_EQ(gyro_cal_new_bias_available(&gyro_cal), 1, "%d"); + + gyro_cal_get_bias(&gyro_cal, bias, &temperature_kelvin, + &calibration_time_us); + + /* Make sure that the bias estimate is within kToleranceGyroRps. */ + TEST_NEAR(gyro_bias_x - bias[0], 0.0f, 0.0001f, "%f"); + TEST_NEAR(gyro_bias_y - bias[1], 0.0f, 0.0001f, "%f"); + TEST_NEAR(gyro_bias_z - bias[2], 0.0f, 0.0001f, "%f"); + + /* Checks that the calibration occurred at the expected time. */ + TEST_EQ(6 * SECOND, gyro_cal.calibration_time_us, "%u"); + + /* Make sure that the device was classified as 100% "still". */ + TEST_NEAR(1.0f, gyro_cal.stillness_confidence, 0.0001f, "%f"); + + /* Make sure that the calibration temperature is correct. */ + TEST_EQ(kDefaultTemperatureKelvin, temperature_kelvin, "%d"); + + return EC_SUCCESS; +} + +/** + * Verifies that setting an initial bias works. + * + * @return EC_SUCCESS on success. + */ +static int test_gyro_cal_set_bias(void) +{ + struct gyro_cal gyro_cal; + + /* Get the initialized bias value; should be zero. */ + float bias[3] = { 0.0f, 0.0f, 0.0f }; + int temperature_kelvin = 273; + uint32_t calibration_time_us = 10; + + /* Initialize the gyro calibration. */ + gyro_cal_initialization_for_test(&gyro_cal); + gyro_cal_get_bias(&gyro_cal, bias, &temperature_kelvin, + &calibration_time_us); + TEST_NEAR(0.0f, bias[0], 0.0001f, "%f"); + TEST_NEAR(0.0f, bias[1], 0.0001f, "%f"); + TEST_NEAR(0.0f, bias[2], 0.0001f, "%f"); + TEST_EQ(0, temperature_kelvin, "%d"); + TEST_EQ(0, calibration_time_us, "%d"); + + /* Set the calibration bias estimate. */ + bias[0] = 1.0f; + bias[1] = 2.0f; + bias[2] = 3.0f; + gyro_cal_set_bias(&gyro_cal, bias, 31, 3 * 60 * SECOND); + + bias[0] = 0.0f; + bias[1] = 0.0f; + bias[2] = 0.0f; + /* Check that it was set correctly. */ + gyro_cal_get_bias(&gyro_cal, bias, &temperature_kelvin, + &calibration_time_us); + TEST_NEAR(1.0f, bias[0], 0.0001f, "%f"); + TEST_NEAR(2.0f, bias[1], 0.0001f, "%f"); + TEST_NEAR(3.0f, bias[2], 0.0001f, "%f"); + TEST_EQ(31, temperature_kelvin, "%d"); + TEST_EQ(3 * 60 * SECOND, calibration_time_us, "%u"); + + return EC_SUCCESS; +} + +/** + * Verifies that the gyroCalRemoveBias function works as intended. + * + * @return EC_SUCCESS on success + */ +static int test_gyro_cal_remove_bias(void) +{ + struct gyro_cal gyro_cal; + float bias[3] = { 1.0f, 2.0f, 3.0f }; + float bias_out[3]; + + /* Initialize the gyro calibration. */ + gyro_cal_initialization_for_test(&gyro_cal); + + /* Set an calibration bias estimate. */ + gyro_cal_set_bias(&gyro_cal, bias, kDefaultTemperatureKelvin, + 5 * 60 * SECOND); + + /* Correct the bias, and check that it has been adequately removed. */ + gyro_cal_remove_bias(&gyro_cal, bias, bias_out); + + /* Make sure that the bias estimate is within kToleranceGyroRps. */ + TEST_NEAR(0.0f, bias_out[0], 0.0001f, "%f"); + TEST_NEAR(0.0f, bias_out[1], 0.0001f, "%f"); + TEST_NEAR(0.0f, bias_out[2], 0.0001f, "%f"); + + return EC_SUCCESS; +} + +void run_test(int argc, char **argv) +{ + test_reset(); + + RUN_TEST(test_gyro_cal_calibration); + RUN_TEST(test_gyro_cal_no_calibration); + RUN_TEST(test_gyro_cal_win_mean_shift); + RUN_TEST(test_gyro_cal_temperature_shift); + RUN_TEST(test_gyro_cal_stillness_timestamp); + RUN_TEST(test_gyro_cal_set_bias); + RUN_TEST(test_gyro_cal_remove_bias); + + test_print_result(); +} diff --git a/test/gyro_cal.tasklist b/test/gyro_cal.tasklist new file mode 100644 index 0000000000..7d28eb5b64 --- /dev/null +++ b/test/gyro_cal.tasklist @@ -0,0 +1,10 @@ +/* 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. + */ + +/** + * See CONFIG_TASK_LIST in config.h for details. + */ +#define CONFIG_TEST_TASK_LIST \ + TASK_TEST(MOTIONSENSE, motion_sense_task, NULL, TASK_STACK_SIZE) diff --git a/test/test_config.h b/test/test_config.h index 97a8c3e951..ed07f73983 100644 --- a/test/test_config.h +++ b/test/test_config.h @@ -132,6 +132,14 @@ #endif #ifdef TEST_ONLINE_CALIBRATION +#define CONFIG_FPU +#define CONFIG_ONLINE_CALIB +#define CONFIG_MKBP_EVENT +#define CONFIG_MKBP_USE_GPIO +#endif + +#ifdef TEST_GYRO_CAL +#define CONFIG_FPU #define CONFIG_ONLINE_CALIB #define CONFIG_MKBP_EVENT #define CONFIG_MKBP_USE_GPIO |