diff options
Diffstat (limited to 'common/gyro_cal.c')
| -rw-r--r-- | common/gyro_cal.c | 630 |
1 files changed, 0 insertions, 630 deletions
diff --git a/common/gyro_cal.c b/common/gyro_cal.c deleted file mode 100644 index 572e401b18..0000000000 --- a/common/gyro_cal.c +++ /dev/null @@ -1,630 +0,0 @@ -/* 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; -} |