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