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>

1 files changed, 163 insertions, 0 deletions

diff --git a/include/gyro_cal.h b/include/gyro_cal.h
new file mode 100644
index 0000000000..fb69464aec
--- /dev/null
+++ b/include/gyro_cal.h
@@ -0,0 +1,163 @@
+/* Copyright 2020 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#ifndef __CROS_EC_GYRO_CAL_H
+#define __CROS_EC_GYRO_CAL_H
+
+#include "common.h"
+#include "gyro_still_det.h"
+#include "math_util.h"
+#include "stdbool.h"
+#include "stddef.h"
+#include "vec3.h"
+
+struct temperature_mean_data {
+	int16_t temperature_min_kelvin;
+	int16_t temperature_max_kelvin;
+	int16_t latest_temperature_kelvin;
+	int mean_accumulator;
+	size_t num_points;
+};
+
+/** Data structure for tracking min/max window mean during device stillness. */
+struct min_max_window_mean_data {
+	fpv3_t gyro_winmean_min;
+	fpv3_t gyro_winmean_max;
+};
+
+struct gyro_cal {
+	/** Stillness detector for accelerometer. */
+	struct gyro_still_det accel_stillness_detect;
+	/** Stillness detector for magnetometer. */
+	struct gyro_still_det mag_stillness_detect;
+	/** Stillness detector for gyroscope. */
+	struct gyro_still_det gyro_stillness_detect;
+
+	/**
+	 * Data for tracking temperature mean during periods of device
+	 * stillness.
+	 */
+	struct temperature_mean_data temperature_mean_tracker;
+
+	/** Data for tracking gyro mean during periods of device stillness. */
+	struct min_max_window_mean_data window_mean_tracker;
+
+	/**
+	 * Aggregated sensor stillness threshold required for gyro bias
+	 * calibration.
+	 */
+	fp_t stillness_threshold;
+
+	/** Min and max durations for gyro bias calibration. */
+	uint32_t min_still_duration_us;
+	uint32_t max_still_duration_us;
+
+	/** Duration of the stillness processing windows. */
+	uint32_t window_time_duration_us;
+
+	/** Timestamp when device started a still period. */
+	uint64_t start_still_time_us;
+
+	/**
+	 * Gyro offset estimate, and the associated calibration temperature,
+	 * timestamp, and stillness confidence values.
+	 * [rad/sec]
+	 */
+	fp_t bias_x, bias_y, bias_z;
+	int bias_temperature_kelvin;
+	fp_t stillness_confidence;
+	uint32_t calibration_time_us;
+
+	/**
+	 * Current window end-time for all sensors. Used to assist in keeping
+	 * sensor data collection in sync. On initialization this will be set to
+	 * zero indicating that sensor data will be dropped until a valid
+	 * end-time is set from the first gyro timestamp received.
+	 */
+	uint32_t stillness_win_endtime_us;
+
+	/**
+	 * Watchdog timer to reset to a known good state when data capture
+	 * stalls.
+	 */
+	uint32_t gyro_window_start_us;
+	uint32_t gyro_window_timeout_duration_us;
+
+	/** Flag is "true" when the magnetometer is used. */
+	bool using_mag_sensor;
+
+	/** Flag set by user to control whether calibrations are used. */
+	bool gyro_calibration_enable;
+
+	/** Flag is 'true' when a new calibration update is ready. */
+	bool new_gyro_cal_available;
+
+	/** Flag to indicate if device was previously still. */
+	bool prev_still;
+
+	/**
+	 * Min and maximum stillness window mean. This is used to check the
+	 * stability of the mean values computed for the gyroscope (i.e.
+	 * provides further validation for stillness).
+	 */
+	fpv3_t gyro_winmean_min;
+	fpv3_t gyro_winmean_max;
+	fp_t stillness_mean_delta_limit;
+
+	/**
+	 * The mean temperature over the stillness period. The limit is used to
+	 * check for temperature stability and provide a gate for when
+	 * temperature is rapidly changing.
+	 */
+	fp_t temperature_mean_kelvin;
+	fp_t temperature_delta_limit_kelvin;
+};
+
+/**
+ * Data structure used to configure the gyroscope calibration in individual
+ * sensors.
+ */
+struct gyro_cal_data {
+	/** The gyro_cal struct to use. */
+	struct gyro_cal gyro_cal;
+	/** The sensor ID of the accelerometer to use. */
+	uint8_t accel_sensor_id;
+	/**
+	 * The sensor ID of the accelerometer to use (use a number greater than
+	 * SENSOR_COUNT to skip).
+	 */
+	uint8_t mag_sensor_id;
+};
+
+/** Reset trackers. */
+void init_gyro_cal(struct gyro_cal *gyro_cal);
+
+/** Get the most recent bias calibration value. */
+void gyro_cal_get_bias(struct gyro_cal *gyro_cal, fpv3_t bias,
+		       int *temperature_kelvin, uint32_t *calibration_time_us);
+
+/** Set an initial bias calibration value. */
+void gyro_cal_set_bias(struct gyro_cal *gyro_cal, fpv3_t bias,
+		       int temperature_kelvin, uint32_t calibration_time_us);
+
+/** Remove gyro bias from the calibration [rad/sec]. */
+void gyro_cal_remove_bias(struct gyro_cal *gyro_cal, fpv3_t in, fpv3_t out);
+
+/** Returns true when a new gyro calibration is available. */
+bool gyro_cal_new_bias_available(struct gyro_cal *gyro_cal);
+
+/** Update the gyro calibration with gyro data [rad/sec]. */
+void gyro_cal_update_gyro(struct gyro_cal *gyro_cal, uint32_t sample_time_us,
+			  fp_t x, fp_t y, fp_t z, int temperature_kelvin);
+
+/** Update the gyro calibration with mag data [micro Tesla]. */
+void gyro_cal_update_mag(struct gyro_cal *gyro_cal, uint32_t sample_time_us,
+			 fp_t x, fp_t y, fp_t z);
+
+/** Update the gyro calibration with accel data [m/sec^2]. */
+void gyro_cal_update_accel(struct gyro_cal *gyro_cal, uint32_t sample_time_us,
+			   fp_t x, fp_t y, fp_t z);
+
+#endif /* __CROS_EC_GYRO_CAL_H */