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>
This commit is contained in:
Yuval Peress
Commit Bot
parent
bc932aea92
commit
474c6a5321
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue