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>

4 files changed, 1012 insertions, 18 deletions

diff --git a/common/build.mk b/common/build.mk
index 0a7fbe86cc..21a268f507 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -120,7 +120,7 @@ common-$(CONFIG_RWSIG_TYPE_RWSIG)+=vboot/vb21_lib.o
 common-$(CONFIG_MATH_UTIL)+=math_util.o
 common-$(CONFIG_ONLINE_CALIB)+=stillness_detector.o kasa.o math_util.o \
 	mat44.o vec3.o newton_fit.o accel_cal.o online_calibration.o \
-	mkbp_event.o mag_cal.o math_util.o mat33.o
+	mkbp_event.o mag_cal.o math_util.o mat33.o gyro_cal.o gyro_still_det.o
 common-$(CONFIG_SHA1)+= sha1.o
 common-$(CONFIG_SHA256)+=sha256.o
 common-$(CONFIG_SOFTWARE_CLZ)+=clz.o
diff --git a/common/gyro_cal.c b/common/gyro_cal.c
new file mode 100644
index 0000000000..572e401b18
--- /dev/null
+++ b/common/gyro_cal.c
@@ -0,0 +1,630 @@
+/* Copyright 2020 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "gyro_cal.h"
+#include "string.h"
+#include <stdbool.h>
+
+/*
+ * Maximum gyro bias correction (should be set based on expected max bias
+ * of the given sensor). [rad/sec]
+ */
+#define MAX_GYRO_BIAS FLOAT_TO_FP(0.2f)
+
+static void device_stillness_check(struct gyro_cal *gyro_cal,
+				   uint32_t sample_time_us);
+
+static void compute_gyro_cal(struct gyro_cal *gyro_cal,
+			     uint32_t calibration_time_us);
+
+static void check_window(struct gyro_cal *gyro_cal, uint32_t sample_time_us);
+
+/** Data tracker command enumeration. */
+enum gyro_cal_tracker_command {
+	/** Resets the local data used for data tracking. */
+	DO_RESET = 0,
+	/** Updates the local tracking data. */
+	DO_UPDATE_DATA,
+	/** Stores intermediate results for later recall. */
+	DO_STORE_DATA,
+	/** Computes and provides the results of the gate function. */
+	DO_EVALUATE
+};
+
+/**
+ * Reset the gyro_cal's temperature statistics.
+ *
+ * @param gyro_cal Pointer to the gyro_cal data structure.
+ */
+static void gyro_temperature_stats_tracker_reset(struct gyro_cal *gyro_cal);
+
+/**
+ * Updates the temperature min/max and mean during the stillness period.
+ *
+ * @param gyro_cal		Pointer to the gyro_cal data structure.
+ * @param temperature_kelvin	New temperature sample to include.
+ */
+static void gyro_temperature_stats_tracker_update(struct gyro_cal *gyro_cal,
+						  int temperature_kelvin);
+
+/**
+ * Store the tracker data to be used for calculation.
+ *
+ * @param gyro_cal Pointer to the gyro_cal data structure.
+ */
+static void gyro_temperature_stats_tracker_store(struct gyro_cal *gyro_cal);
+
+/**
+ * Compute whether or not the temperature values are in range.
+ *
+ * @param gyro_cal	Pointer to the gyro_cal data structure.
+ * @return		'true' if the min and max temperature values exceed the
+ *			range set by 'temperature_delta_limit_kelvin'.
+ */
+static bool gyro_temperature_stats_tracker_eval(struct gyro_cal *gyro_cal);
+
+/**
+ * Tracks the minimum and maximum gyroscope stillness window means.
+ * Returns
+ *
+ * @param gyro_cal Pointer to the gyro_cal data structure.
+ * @param do_this  Command enumerator that controls function behavior.
+ */
+static void gyro_still_mean_tracker_reset(struct gyro_cal *gyro_cal);
+
+/**
+ * Compute the min/max window mean values according to 'window_mean_tracker'.
+ *
+ * @param gyro_cal Pointer to the gyro_cal data structure.
+ */
+static void gyro_still_mean_tracker_update(struct gyro_cal *gyro_cal);
+
+/**
+ * Store the most recent "stillness" mean data to the gyro_cal data structure.
+ *
+ * @param gyro_cal Pointer to the gyro_cal data structure.
+ */
+static void gyro_still_mean_tracker_store(struct gyro_cal *gyro_cal);
+
+/**
+ * Compute whether or not the gyroscope window range is within the valid range.
+ *
+ * @param gyro_cal	Pointer to the gyro_cal data structure.
+ * @return		'true' when the difference between gyroscope min and max
+ *			window means are outside the range set by
+ *			'stillness_mean_delta_limit'.
+ */
+static bool gyro_still_mean_tracker_eval(struct gyro_cal *gyro_cal);
+
+void init_gyro_cal(struct gyro_cal *gyro_cal)
+{
+	gyro_still_mean_tracker_reset(gyro_cal);
+	gyro_temperature_stats_tracker_reset(gyro_cal);
+}
+
+void gyro_cal_get_bias(struct gyro_cal *gyro_cal, fpv3_t bias,
+		       int *temperature_kelvin, uint32_t *calibration_time_us)
+{
+	bias[X] = gyro_cal->bias_x;
+	bias[Y] = gyro_cal->bias_y;
+	bias[Z] = gyro_cal->bias_z;
+	*calibration_time_us = gyro_cal->calibration_time_us;
+	*temperature_kelvin = gyro_cal->bias_temperature_kelvin;
+}
+
+void gyro_cal_set_bias(struct gyro_cal *gyro_cal, fpv3_t bias,
+		       int temperature_kelvin, uint32_t calibration_time_us)
+{
+	gyro_cal->bias_x = bias[X];
+	gyro_cal->bias_y = bias[Y];
+	gyro_cal->bias_z = bias[Z];
+	gyro_cal->calibration_time_us = calibration_time_us;
+	gyro_cal->bias_temperature_kelvin = temperature_kelvin;
+}
+
+void gyro_cal_remove_bias(struct gyro_cal *gyro_cal, fpv3_t in, fpv3_t out)
+{
+	if (gyro_cal->gyro_calibration_enable) {
+		out[X] = in[X] - gyro_cal->bias_x;
+		out[Y] = in[Y] - gyro_cal->bias_y;
+		out[Z] = in[Z] - gyro_cal->bias_z;
+	}
+}
+
+bool gyro_cal_new_bias_available(struct gyro_cal *gyro_cal)
+{
+	bool new_gyro_cal_available = (gyro_cal->gyro_calibration_enable &&
+				       gyro_cal->new_gyro_cal_available);
+
+	/* Clear the flag. */
+	gyro_cal->new_gyro_cal_available = false;
+
+	return new_gyro_cal_available;
+}
+
+void gyro_cal_update_gyro(struct gyro_cal *gyro_cal, uint32_t sample_time_us,
+			  fp_t x, fp_t y, fp_t z, int temperature_kelvin)
+{
+	/*
+	 * Make sure that a valid window end-time is set, and start the window
+	 * timer.
+	 */
+	if (gyro_cal->stillness_win_endtime_us <= 0) {
+		gyro_cal->stillness_win_endtime_us =
+			sample_time_us + gyro_cal->window_time_duration_us;
+
+		/* Start the window timer. */
+		gyro_cal->gyro_window_start_us = sample_time_us;
+	}
+
+	/* Update the temperature statistics. */
+	gyro_temperature_stats_tracker_update(gyro_cal, temperature_kelvin);
+
+	/* Pass gyro data to stillness detector */
+	gyro_still_det_update(&gyro_cal->gyro_stillness_detect,
+			      gyro_cal->stillness_win_endtime_us,
+			      sample_time_us, x, y, z);
+
+	/*
+	 * Perform a device stillness check, set next window end-time, and
+	 * possibly do a gyro bias calibration and stillness detector reset.
+	 */
+	device_stillness_check(gyro_cal, sample_time_us);
+}
+
+void gyro_cal_update_mag(struct gyro_cal *gyro_cal, uint32_t sample_time_us,
+			 fp_t x, fp_t y, fp_t z)
+{
+	/* Pass magnetometer data to stillness detector. */
+	gyro_still_det_update(&gyro_cal->mag_stillness_detect,
+			      gyro_cal->stillness_win_endtime_us,
+			      sample_time_us, x, y, z);
+
+	/* Received a magnetometer sample; incorporate it into detection. */
+	gyro_cal->using_mag_sensor = true;
+
+	/*
+	 * Perform a device stillness check, set next window end-time, and
+	 * possibly do a gyro bias calibration and stillness detector reset.
+	 */
+	device_stillness_check(gyro_cal, sample_time_us);
+}
+
+void gyro_cal_update_accel(struct gyro_cal *gyro_cal, uint32_t sample_time_us,
+			   fp_t x, fp_t y, fp_t z)
+{
+	/* Pass accelerometer data to stillnesss detector. */
+	gyro_still_det_update(&gyro_cal->accel_stillness_detect,
+			      gyro_cal->stillness_win_endtime_us,
+			      sample_time_us, x, y, z);
+
+	/*
+	 * Perform a device stillness check, set next window end-time, and
+	 * possibly do a gyro bias calibration and stillness detector reset.
+	 */
+	device_stillness_check(gyro_cal, sample_time_us);
+}
+
+/**
+ * Handle the case where the device is found to be still. This function should
+ * be called from device_stillness_check.
+ *
+ * @param gyro_cal Pointer to the gyroscope calibration struct.
+ */
+static void handle_device_is_still(struct gyro_cal *gyro_cal)
+{
+	/*
+	 * Device is "still" logic:
+	 * If not previously still, then record the start time.
+	 * If stillness period is too long, then do a calibration.
+	 * Otherwise, continue collecting stillness data.
+	 */
+	bool stillness_duration_exceeded = false;
+
+	/*
+	 * If device was not previously still, set new start timestamp.
+	 */
+	if (!gyro_cal->prev_still) {
+		/*
+		 * Record the starting timestamp of the current stillness
+		 * window. This enables the calculation of total duration of
+		 * the stillness period.
+		 */
+		gyro_cal->start_still_time_us =
+			gyro_cal->gyro_stillness_detect.window_start_time;
+	}
+
+	/*
+	 * Check to see if current stillness period exceeds the desired limit.
+	 */
+	stillness_duration_exceeded =
+		gyro_cal->gyro_stillness_detect.last_sample_time >=
+		(gyro_cal->start_still_time_us +
+		 gyro_cal->max_still_duration_us);
+
+	/* Track the new stillness mean and temperature data. */
+	gyro_still_mean_tracker_store(gyro_cal);
+	gyro_temperature_stats_tracker_store(gyro_cal);
+
+	if (stillness_duration_exceeded) {
+		/*
+		 * The current stillness has gone too long. Do a calibration
+		 * with the current data and reset.
+		 */
+
+		/*
+		 * Updates the gyro bias estimate with the current window data
+		 * and resets the stats.
+		 */
+		gyro_still_det_reset(&gyro_cal->accel_stillness_detect,
+				     /*reset_stats=*/true);
+		gyro_still_det_reset(&gyro_cal->gyro_stillness_detect,
+				     /*reset_stats=*/true);
+		gyro_still_det_reset(&gyro_cal->mag_stillness_detect,
+				     /*reset_stats=*/true);
+
+		/*
+		 * Resets the local calculations because the stillness
+		 * period is over.
+		 */
+		gyro_still_mean_tracker_reset(gyro_cal);
+		gyro_temperature_stats_tracker_reset(gyro_cal);
+
+		/* Computes a new gyro offset estimate. */
+		compute_gyro_cal(
+			gyro_cal,
+			gyro_cal->gyro_stillness_detect.last_sample_time);
+
+		/*
+		 * Update stillness flag. Force the start of a new
+		 * stillness period.
+		 */
+		gyro_cal->prev_still = false;
+	} else {
+		/* Continue collecting stillness data. */
+
+		/* Extend the stillness period. */
+		gyro_still_det_reset(&gyro_cal->accel_stillness_detect,
+				     /*reset_stats=*/false);
+		gyro_still_det_reset(&gyro_cal->gyro_stillness_detect,
+				     /*reset_stats=*/false);
+		gyro_still_det_reset(&gyro_cal->mag_stillness_detect,
+				     /*reset_stats=*/false);
+
+		/* Update the stillness flag. */
+		gyro_cal->prev_still = true;
+	}
+}
+
+static void handle_device_not_still(struct gyro_cal *gyro_cal)
+{
+	/* Device is NOT still; motion detected. */
+
+	/*
+	 * If device was previously still and the total stillness
+	 * duration is not "too short", then do a calibration with the
+	 * data accumulated thus far.
+	 */
+	bool stillness_duration_too_short =
+		gyro_cal->gyro_stillness_detect.window_start_time <
+		(gyro_cal->start_still_time_us +
+		 gyro_cal->min_still_duration_us);
+
+	if (gyro_cal->prev_still && !stillness_duration_too_short)
+		compute_gyro_cal(
+			gyro_cal,
+			gyro_cal->gyro_stillness_detect.window_start_time);
+
+	/* Reset the stillness detectors and the stats. */
+	gyro_still_det_reset(&gyro_cal->accel_stillness_detect,
+			     /*reset_stats=*/true);
+	gyro_still_det_reset(&gyro_cal->gyro_stillness_detect,
+			     /*reset_stats=*/true);
+	gyro_still_det_reset(&gyro_cal->mag_stillness_detect,
+			     /*reset_stats=*/true);
+
+	/* Resets the temperature and sensor mean data. */
+	gyro_temperature_stats_tracker_reset(gyro_cal);
+	gyro_still_mean_tracker_reset(gyro_cal);
+
+	/* Update stillness flag. */
+	gyro_cal->prev_still = false;
+}
+
+void device_stillness_check(struct gyro_cal *gyro_cal, uint32_t sample_time_us)
+{
+	bool min_max_temp_exceeded = false;
+	bool mean_not_stable = false;
+	bool device_is_still = false;
+	fp_t conf_not_rot = INT_TO_FP(0);
+	fp_t conf_not_accel = INT_TO_FP(0);
+	fp_t conf_still = INT_TO_FP(0);
+
+	/* Check the window timer. */
+	check_window(gyro_cal, sample_time_us);
+
+	/* Is there enough data to do a stillness calculation? */
+	if ((!gyro_cal->mag_stillness_detect.stillness_window_ready &&
+	     gyro_cal->using_mag_sensor) ||
+	    !gyro_cal->accel_stillness_detect.stillness_window_ready ||
+	    !gyro_cal->gyro_stillness_detect.stillness_window_ready)
+		return; /* Not yet, wait for more data. */
+
+	/* Set the next window end-time for the stillness detectors. */
+	gyro_cal->stillness_win_endtime_us =
+		sample_time_us + gyro_cal->window_time_duration_us;
+
+	/* Update the confidence scores for all sensors. */
+	gyro_still_det_compute(&gyro_cal->accel_stillness_detect);
+	gyro_still_det_compute(&gyro_cal->gyro_stillness_detect);
+	if (gyro_cal->using_mag_sensor) {
+		gyro_still_det_compute(&gyro_cal->mag_stillness_detect);
+	} else {
+		/*
+		 * Not using magnetometer, force stillness confidence to 100%.
+		 */
+		gyro_cal->mag_stillness_detect.stillness_confidence =
+			INT_TO_FP(1);
+	}
+
+	/* Updates the mean tracker data. */
+	gyro_still_mean_tracker_update(gyro_cal);
+
+	/*
+	 * Determine motion confidence scores (rotation, accelerating, and
+	 * stillness).
+	 */
+	conf_not_rot =
+		fp_mul(gyro_cal->gyro_stillness_detect.stillness_confidence,
+		       gyro_cal->mag_stillness_detect.stillness_confidence);
+	conf_not_accel = gyro_cal->accel_stillness_detect.stillness_confidence;
+	conf_still = fp_mul(conf_not_rot, conf_not_accel);
+
+	/* Evaluate the mean and temperature gate functions. */
+	mean_not_stable = gyro_still_mean_tracker_eval(gyro_cal);
+	min_max_temp_exceeded = gyro_temperature_stats_tracker_eval(gyro_cal);
+
+	/* Determines if the device is currently still. */
+	device_is_still = (conf_still > gyro_cal->stillness_threshold) &&
+			  !mean_not_stable && !min_max_temp_exceeded;
+
+	if (device_is_still)
+		handle_device_is_still(gyro_cal);
+	else
+		handle_device_not_still(gyro_cal);
+
+	/* Reset the window timer after we have processed data. */
+	gyro_cal->gyro_window_start_us = sample_time_us;
+}
+
+void compute_gyro_cal(struct gyro_cal *gyro_cal, uint32_t calibration_time_us)
+{
+	/* Check to see if new calibration values is within acceptable range. */
+	if (!(gyro_cal->gyro_stillness_detect.prev_mean[X] < MAX_GYRO_BIAS &&
+	      gyro_cal->gyro_stillness_detect.prev_mean[X] > -MAX_GYRO_BIAS &&
+	      gyro_cal->gyro_stillness_detect.prev_mean[Y] < MAX_GYRO_BIAS &&
+	      gyro_cal->gyro_stillness_detect.prev_mean[Y] > -MAX_GYRO_BIAS &&
+	      gyro_cal->gyro_stillness_detect.prev_mean[Z] < MAX_GYRO_BIAS &&
+	      gyro_cal->gyro_stillness_detect.prev_mean[Z] > -MAX_GYRO_BIAS))
+		/* Outside of range. Ignore, reset, and continue. */
+		return;
+
+	/* Record the new gyro bias offset calibration. */
+	gyro_cal->bias_x = gyro_cal->gyro_stillness_detect.prev_mean[X];
+	gyro_cal->bias_y = gyro_cal->gyro_stillness_detect.prev_mean[Y];
+	gyro_cal->bias_z = gyro_cal->gyro_stillness_detect.prev_mean[Z];
+
+	/*
+	 * Store the calibration temperature (using the mean temperature over
+	 * the "stillness" period).
+	 */
+	gyro_cal->bias_temperature_kelvin = gyro_cal->temperature_mean_kelvin;
+
+	/* Store the calibration time stamp. */
+	gyro_cal->calibration_time_us = calibration_time_us;
+
+	/* Record the final stillness confidence. */
+	gyro_cal->stillness_confidence = fp_mul(
+		gyro_cal->gyro_stillness_detect.prev_stillness_confidence,
+		gyro_cal->accel_stillness_detect.prev_stillness_confidence);
+	gyro_cal->stillness_confidence = fp_mul(
+		gyro_cal->stillness_confidence,
+		gyro_cal->mag_stillness_detect.prev_stillness_confidence);
+
+	/* Set flag to indicate a new gyro calibration value is available. */
+	gyro_cal->new_gyro_cal_available = true;
+}
+
+void check_window(struct gyro_cal *gyro_cal, uint32_t sample_time_us)
+{
+	bool window_timeout;
+
+	/* Check for initialization of the window time (=0). */
+	if (gyro_cal->gyro_window_start_us <= 0)
+		return;
+
+	/*
+	 * Checks for the following window timeout conditions:
+	 * i.  The current timestamp has exceeded the allowed window duration.
+	 * ii. A timestamp was received that has jumped backwards by more than
+	 *     the allowed window duration (e.g., timestamp clock roll-over).
+	 */
+	window_timeout =
+		(sample_time_us > gyro_cal->gyro_window_timeout_duration_us +
+					  gyro_cal->gyro_window_start_us) ||
+		(sample_time_us + gyro_cal->gyro_window_timeout_duration_us <
+		 gyro_cal->gyro_window_start_us);
+
+	/* If a timeout occurred then reset to known good state. */
+	if (window_timeout) {
+		/* Reset stillness detectors and restart data capture. */
+		gyro_still_det_reset(&gyro_cal->accel_stillness_detect,
+				     /*reset_stats=*/true);
+		gyro_still_det_reset(&gyro_cal->gyro_stillness_detect,
+				     /*reset_stats=*/true);
+		gyro_still_det_reset(&gyro_cal->mag_stillness_detect,
+				     /*reset_stats=*/true);
+
+		/* Resets the temperature and sensor mean data. */
+		gyro_temperature_stats_tracker_reset(gyro_cal);
+		gyro_still_mean_tracker_reset(gyro_cal);
+
+		/* Resets the stillness window end-time. */
+		gyro_cal->stillness_win_endtime_us = 0;
+
+		/* Force stillness confidence to zero. */
+		gyro_cal->accel_stillness_detect.prev_stillness_confidence = 0;
+		gyro_cal->gyro_stillness_detect.prev_stillness_confidence = 0;
+		gyro_cal->mag_stillness_detect.prev_stillness_confidence = 0;
+		gyro_cal->stillness_confidence = 0;
+		gyro_cal->prev_still = false;
+
+		/*
+		 * If there are no magnetometer samples being received then
+		 * operate the calibration algorithm without this sensor.
+		 */
+		if (!gyro_cal->mag_stillness_detect.stillness_window_ready &&
+		    gyro_cal->using_mag_sensor) {
+			gyro_cal->using_mag_sensor = false;
+		}
+
+		/* Assert window timeout flags. */
+		gyro_cal->gyro_window_start_us = 0;
+	}
+}
+
+void gyro_temperature_stats_tracker_reset(struct gyro_cal *gyro_cal)
+{
+	/* Resets the mean accumulator. */
+	gyro_cal->temperature_mean_tracker.num_points = 0;
+	gyro_cal->temperature_mean_tracker.mean_accumulator = INT_TO_FP(0);
+
+	/* Initializes the min/max temperatures values. */
+	gyro_cal->temperature_mean_tracker.temperature_min_kelvin = 0x7fff;
+	gyro_cal->temperature_mean_tracker.temperature_max_kelvin = 0xffff;
+}
+
+void gyro_temperature_stats_tracker_update(struct gyro_cal *gyro_cal,
+					   int temperature_kelvin)
+{
+	/* Does the mean accumulation. */
+	gyro_cal->temperature_mean_tracker.mean_accumulator +=
+		temperature_kelvin;
+	gyro_cal->temperature_mean_tracker.num_points++;
+
+	/* Tracks the min, max, and latest temperature values. */
+	gyro_cal->temperature_mean_tracker.latest_temperature_kelvin =
+		temperature_kelvin;
+	if (gyro_cal->temperature_mean_tracker.temperature_min_kelvin >
+	    temperature_kelvin) {
+		gyro_cal->temperature_mean_tracker.temperature_min_kelvin =
+			temperature_kelvin;
+	}
+	if (gyro_cal->temperature_mean_tracker.temperature_max_kelvin <
+	    temperature_kelvin) {
+		gyro_cal->temperature_mean_tracker.temperature_max_kelvin =
+			temperature_kelvin;
+	}
+}
+
+void gyro_temperature_stats_tracker_store(struct gyro_cal *gyro_cal)
+{
+	/*
+	 * Store the most recent temperature statistics data to the
+	 * gyro_cal data structure. This functionality allows previous
+	 * results to be recalled when the device suddenly becomes "not
+	 * still".
+	 */
+	if (gyro_cal->temperature_mean_tracker.num_points > 0)
+		gyro_cal->temperature_mean_kelvin =
+			gyro_cal->temperature_mean_tracker.mean_accumulator /
+			gyro_cal->temperature_mean_tracker.num_points;
+	else
+		gyro_cal->temperature_mean_kelvin =
+			gyro_cal->temperature_mean_tracker
+				.latest_temperature_kelvin;
+}
+
+bool gyro_temperature_stats_tracker_eval(struct gyro_cal *gyro_cal)
+{
+	bool min_max_temp_exceeded = false;
+
+	/* Determines if the min/max delta exceeded the set limit. */
+	if (gyro_cal->temperature_mean_tracker.num_points > 0) {
+		min_max_temp_exceeded =
+			(gyro_cal->temperature_mean_tracker
+				 .temperature_max_kelvin -
+			 gyro_cal->temperature_mean_tracker
+				 .temperature_min_kelvin) >
+			gyro_cal->temperature_delta_limit_kelvin;
+	}
+
+	return min_max_temp_exceeded;
+}
+
+void gyro_still_mean_tracker_reset(struct gyro_cal *gyro_cal)
+{
+	size_t i;
+
+	/* Resets the min/max window mean values to a default value. */
+	for (i = 0; i < 3; i++) {
+		gyro_cal->window_mean_tracker.gyro_winmean_min[i] = FLT_MAX;
+		gyro_cal->window_mean_tracker.gyro_winmean_max[i] = -FLT_MAX;
+	}
+}
+
+void gyro_still_mean_tracker_update(struct gyro_cal *gyro_cal)
+{
+	int i;
+
+	/* Computes the min/max window mean values. */
+	for (i = 0; i < 3; ++i) {
+		if (gyro_cal->window_mean_tracker.gyro_winmean_min[i] >
+		    gyro_cal->gyro_stillness_detect.win_mean[i]) {
+			gyro_cal->window_mean_tracker.gyro_winmean_min[i] =
+				gyro_cal->gyro_stillness_detect.win_mean[i];
+		}
+		if (gyro_cal->window_mean_tracker.gyro_winmean_max[i] <
+		    gyro_cal->gyro_stillness_detect.win_mean[i]) {
+			gyro_cal->window_mean_tracker.gyro_winmean_max[i] =
+				gyro_cal->gyro_stillness_detect.win_mean[i];
+		}
+	}
+}
+
+void gyro_still_mean_tracker_store(struct gyro_cal *gyro_cal)
+{
+	/*
+	 * Store the most recent "stillness" mean data to the gyro_cal
+	 * data structure. This functionality allows previous results to
+	 * be recalled when the device suddenly becomes "not still".
+	 */
+	memcpy(gyro_cal->gyro_winmean_min,
+	       gyro_cal->window_mean_tracker.gyro_winmean_min,
+	       sizeof(gyro_cal->window_mean_tracker.gyro_winmean_min));
+	memcpy(gyro_cal->gyro_winmean_max,
+	       gyro_cal->window_mean_tracker.gyro_winmean_max,
+	       sizeof(gyro_cal->window_mean_tracker.gyro_winmean_max));
+}
+
+bool gyro_still_mean_tracker_eval(struct gyro_cal *gyro_cal)
+{
+	bool mean_not_stable = false;
+	size_t i;
+
+	/*
+	 * Performs the stability check and returns the 'true' if the
+	 * difference between min/max window mean value is outside the
+	 * stable range.
+	 */
+	for (i = 0; i < 3 && !mean_not_stable; i++) {
+		mean_not_stable |=
+			(gyro_cal->window_mean_tracker.gyro_winmean_max[i] -
+			 gyro_cal->window_mean_tracker.gyro_winmean_min[i]) >
+			gyro_cal->stillness_mean_delta_limit;
+	}
+
+	return mean_not_stable;
+}
diff --git a/common/gyro_still_det.c b/common/gyro_still_det.c
new file mode 100644
index 0000000000..4574e22e5f
--- /dev/null
+++ b/common/gyro_still_det.c
@@ -0,0 +1,242 @@
+/* Copyright 2020 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "gyro_still_det.h"
+#include "vec3.h"
+
+/* Enforces the limits of an input value [0,1]. */
+static fp_t gyro_still_det_limit(fp_t value);
+
+void gyro_still_det_update(struct gyro_still_det *gyro_still_det,
+			   uint32_t stillness_win_endtime, uint32_t sample_time,
+			   fp_t x, fp_t y, fp_t z)
+{
+	fp_t delta = INT_TO_FP(0);
+
+	/*
+	 * Using the method of the assumed mean to preserve some numerical
+	 * stability while avoiding per-sample divisions that the more
+	 * numerically stable Welford method would afford.
+	 *
+	 * Reference for the numerical method used below to compute the
+	 * online mean and variance statistics:
+	 *   1). en.wikipedia.org/wiki/assumed_mean
+	 */
+
+	/* Increment the number of samples. */
+	gyro_still_det->num_acc_samples++;
+
+	/* Online computation of mean for the running stillness period. */
+	gyro_still_det->mean[X] += x;
+	gyro_still_det->mean[Y] += y;
+	gyro_still_det->mean[Z] += z;
+
+	/* Is this the first sample of a new window? */
+	if (gyro_still_det->start_new_window) {
+		/* Record the window start time. */
+		gyro_still_det->window_start_time = sample_time;
+		gyro_still_det->start_new_window = false;
+
+		/* Update assumed mean values. */
+		gyro_still_det->assumed_mean[X] = x;
+		gyro_still_det->assumed_mean[Y] = y;
+		gyro_still_det->assumed_mean[Z] = z;
+
+		/* Reset current window mean and variance. */
+		gyro_still_det->num_acc_win_samples = 0;
+		gyro_still_det->win_mean[X] = INT_TO_FP(0);
+		gyro_still_det->win_mean[Y] = INT_TO_FP(0);
+		gyro_still_det->win_mean[Z] = INT_TO_FP(0);
+		gyro_still_det->acc_var[X] = INT_TO_FP(0);
+		gyro_still_det->acc_var[Y] = INT_TO_FP(0);
+		gyro_still_det->acc_var[Z] = INT_TO_FP(0);
+	} else {
+		/*
+		 * Check to see if we have enough samples to compute a stillness
+		 * confidence score.
+		 */
+		gyro_still_det->stillness_window_ready =
+			(sample_time >= stillness_win_endtime) &&
+			(gyro_still_det->num_acc_samples > 1);
+	}
+
+	/* Record the most recent sample time stamp. */
+	gyro_still_det->last_sample_time = sample_time;
+
+	/* Online window mean and variance ("one-pass" accumulation). */
+	gyro_still_det->num_acc_win_samples++;
+
+	delta = (x - gyro_still_det->assumed_mean[X]);
+	gyro_still_det->win_mean[X] += delta;
+	gyro_still_det->acc_var[X] += fp_sq(delta);
+
+	delta = (y - gyro_still_det->assumed_mean[Y]);
+	gyro_still_det->win_mean[Y] += delta;
+	gyro_still_det->acc_var[Y] += fp_sq(delta);
+
+	delta = (z - gyro_still_det->assumed_mean[Z]);
+	gyro_still_det->win_mean[Z] += delta;
+	gyro_still_det->acc_var[Z] += fp_sq(delta);
+}
+
+fp_t gyro_still_det_compute(struct gyro_still_det *gyro_still_det)
+{
+	fp_t tmp_denom = INT_TO_FP(1);
+	fp_t tmp_denom_mean = INT_TO_FP(1);
+	fp_t tmp;
+	fp_t upper_var_thresh, lower_var_thresh;
+
+	/* Don't divide by zero (not likely, but a precaution). */
+	if (gyro_still_det->num_acc_win_samples > 1) {
+		tmp_denom = fp_div(
+			tmp_denom,
+			INT_TO_FP(gyro_still_det->num_acc_win_samples - 1));
+		tmp_denom_mean =
+			fp_div(tmp_denom_mean,
+			       INT_TO_FP(gyro_still_det->num_acc_win_samples));
+	} else {
+		/* Return zero stillness confidence. */
+		gyro_still_det->stillness_confidence = 0;
+		return gyro_still_det->stillness_confidence;
+	}
+
+	/* Update the final calculation of window mean and variance. */
+	tmp = gyro_still_det->win_mean[X];
+	gyro_still_det->win_mean[X] =
+		fp_mul(gyro_still_det->win_mean[X], tmp_denom_mean);
+	gyro_still_det->win_var[X] =
+		fp_mul((gyro_still_det->acc_var[X] -
+			fp_mul(gyro_still_det->win_mean[X], tmp)),
+		       tmp_denom);
+
+	tmp = gyro_still_det->win_mean[Y];
+	gyro_still_det->win_mean[Y] =
+		fp_mul(gyro_still_det->win_mean[Y], tmp_denom_mean);
+	gyro_still_det->win_var[Y] =
+		fp_mul((gyro_still_det->acc_var[Y] -
+			fp_mul(gyro_still_det->win_mean[Y], tmp)),
+		       tmp_denom);
+
+	tmp = gyro_still_det->win_mean[Z];
+	gyro_still_det->win_mean[Z] =
+		fp_mul(gyro_still_det->win_mean[Z], tmp_denom_mean);
+	gyro_still_det->win_var[Z] =
+		fp_mul((gyro_still_det->acc_var[Z] -
+			fp_mul(gyro_still_det->win_mean[Z], tmp)),
+		       tmp_denom);
+
+	/* Adds the assumed mean value back to the total mean calculation. */
+	gyro_still_det->win_mean[X] += gyro_still_det->assumed_mean[X];
+	gyro_still_det->win_mean[Y] += gyro_still_det->assumed_mean[Y];
+	gyro_still_det->win_mean[Z] += gyro_still_det->assumed_mean[Z];
+
+	/* Define the variance thresholds. */
+	upper_var_thresh = gyro_still_det->var_threshold +
+			   gyro_still_det->confidence_delta;
+
+	lower_var_thresh = gyro_still_det->var_threshold -
+			   gyro_still_det->confidence_delta;
+
+	/* Compute the stillness confidence score. */
+	if ((gyro_still_det->win_var[X] > upper_var_thresh) ||
+	    (gyro_still_det->win_var[Y] > upper_var_thresh) ||
+	    (gyro_still_det->win_var[Z] > upper_var_thresh)) {
+		/*
+		 * Sensor variance exceeds the upper threshold (i.e., motion
+		 * detected). Set stillness confidence equal to 0.
+		 */
+		gyro_still_det->stillness_confidence = 0;
+	} else if ((gyro_still_det->win_var[X] <= lower_var_thresh) &&
+		   (gyro_still_det->win_var[Y] <= lower_var_thresh) &&
+		   (gyro_still_det->win_var[Z] <= lower_var_thresh)) {
+		/*
+		 * Sensor variance is below the lower threshold (i.e.
+		 * stillness detected).
+		 * Set stillness confidence equal to 1.
+		 */
+		gyro_still_det->stillness_confidence = INT_TO_FP(1);
+	} else {
+		/*
+		 * Motion detection thresholds not exceeded. Compute the
+		 * stillness confidence score.
+		 */
+		fp_t var_thresh = gyro_still_det->var_threshold;
+		fpv3_t limit;
+
+		/*
+		 * Compute the stillness confidence score.
+		 * Each axis score is limited [0,1].
+		 */
+		tmp_denom = fp_div(INT_TO_FP(1),
+				   (upper_var_thresh - lower_var_thresh));
+		limit[X] = gyro_still_det_limit(
+			FLOAT_TO_FP(0.5f) -
+			fp_mul(gyro_still_det->win_var[X] - var_thresh,
+			       tmp_denom));
+		limit[Y] = gyro_still_det_limit(
+			FLOAT_TO_FP(0.5f) -
+			fp_mul(gyro_still_det->win_var[Y] - var_thresh,
+			       tmp_denom));
+		limit[Z] = gyro_still_det_limit(
+			FLOAT_TO_FP(0.5f) -
+			fp_mul(gyro_still_det->win_var[Z] - var_thresh,
+			       tmp_denom));
+
+		gyro_still_det->stillness_confidence =
+			fp_mul(limit[X], fp_mul(limit[Y], limit[Z]));
+	}
+
+	/* Return the stillness confidence. */
+	return gyro_still_det->stillness_confidence;
+}
+
+void gyro_still_det_reset(struct gyro_still_det *gyro_still_det,
+			  bool reset_stats)
+{
+	fp_t tmp_denom = INT_TO_FP(1);
+
+	/* Reset the stillness data ready flag. */
+	gyro_still_det->stillness_window_ready = false;
+
+	/* Signal to start capture of next stillness data window. */
+	gyro_still_det->start_new_window = true;
+
+	/* Track the stillness confidence (current->previous). */
+	gyro_still_det->prev_stillness_confidence =
+		gyro_still_det->stillness_confidence;
+
+	/* Track changes in the mean estimate. */
+	if (gyro_still_det->num_acc_samples > INT_TO_FP(1))
+		tmp_denom =
+			fp_div(INT_TO_FP(1), gyro_still_det->num_acc_samples);
+
+	gyro_still_det->prev_mean[X] =
+		fp_mul(gyro_still_det->mean[X], tmp_denom);
+	gyro_still_det->prev_mean[Y] =
+		fp_mul(gyro_still_det->mean[Y], tmp_denom);
+	gyro_still_det->prev_mean[Z] =
+		fp_mul(gyro_still_det->mean[Z], tmp_denom);
+
+	/* Reset the current statistics to zero. */
+	if (reset_stats) {
+		gyro_still_det->num_acc_samples = 0;
+		gyro_still_det->mean[X] = INT_TO_FP(0);
+		gyro_still_det->mean[Y] = INT_TO_FP(0);
+		gyro_still_det->mean[Z] = INT_TO_FP(0);
+		gyro_still_det->acc_var[X] = INT_TO_FP(0);
+		gyro_still_det->acc_var[Y] = INT_TO_FP(0);
+		gyro_still_det->acc_var[Z] = INT_TO_FP(0);
+	}
+}
+
+fp_t gyro_still_det_limit(fp_t value)
+{
+	if (value < INT_TO_FP(0))
+		value = INT_TO_FP(0);
+	else if (value > INT_TO_FP(1))
+		value = INT_TO_FP(1);
+
+	return value;
+}
diff --git a/common/online_calibration.c b/common/online_calibration.c
index 6c090a6abc..ca7a5a620d 100644
--- a/common/online_calibration.c
+++ b/common/online_calibration.c
@@ -15,8 +15,9 @@
 #include "ec_commands.h"
 #include "accel_cal.h"
 #include "mkbp_event.h"
+#include "gyro_cal.h"
 
-#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args)
 
 #ifndef CONFIG_MKBP_EVENT
 #error "Must use CONFIG_MKBP_EVENT for online calibration"
@@ -40,7 +41,7 @@ static int get_temperature(struct motion_sensor_t *sensor, int *temp)
 	now = __hw_clock_source_read();
 	if (entry->last_temperature < 0 ||
 	    time_until(entry->last_temperature_timestamp, now) >
-	    CONFIG_TEMP_CACHE_STALE_THRES) {
+		    CONFIG_TEMP_CACHE_STALE_THRES) {
 		int t;
 		int rc = sensor->drv->read_temp(sensor, &t);
 
@@ -72,8 +73,8 @@ static void data_int16_to_fp(const struct motion_sensor_t *s,
 	}
 }
 
-static void data_fp_to_int16(const struct motion_sensor_t *s,
-			     const fpv3_t data, int16_t *out)
+static void data_fp_to_int16(const struct motion_sensor_t *s, const fpv3_t data,
+			     int16_t *out)
 {
 	int i;
 	fp_t range = INT_TO_FP(s->drv->get_range(s));
@@ -82,14 +83,102 @@ static void data_fp_to_int16(const struct motion_sensor_t *s,
 		int32_t iv;
 		fp_t v = fp_div(data[i], range);
 
-		v = fp_mul(v, INT_TO_FP(
-			(data[i] >= INT_TO_FP(0)) ? 0x7fff : 0x8000));
+		v = fp_mul(v, INT_TO_FP((data[i] >= INT_TO_FP(0)) ? 0x7fff :
+								    0x8000));
 		iv = FP_TO_INT(v);
 		/* Check for overflow */
 		out[i] = CLAMP(iv, (int32_t)0xffff8000, (int32_t)0x00007fff);
 	}
 }
 
+/**
+ * Check a gyroscope for new bias. This function checks a given sensor (must be
+ * a gyroscope) for new bias values. If found, it will update the appropriate
+ * caches and notify the AP.
+ *
+ * @param sensor Pointer to the gyroscope sensor to check.
+ */
+static void check_gyro_cal_new_bias(struct motion_sensor_t *sensor)
+{
+	struct online_calib_data *calib_data =
+		(struct online_calib_data *)sensor->online_calib_data;
+	struct gyro_cal_data *data =
+		(struct gyro_cal_data *)calib_data->type_specific_data;
+	size_t sensor_num = motion_sensors - sensor;
+	int temp_out;
+	fpv3_t bias_out;
+	uint32_t timestamp_out;
+
+	/* Check that we have a new bias. */
+	if (data == NULL || calib_data == NULL ||
+	    !gyro_cal_new_bias_available(&data->gyro_cal))
+		return;
+
+	/* Read the calibration values. */
+	gyro_cal_get_bias(&data->gyro_cal, bias_out, &temp_out, &timestamp_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;
 }
-