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>

11 files changed, 1904 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;
 }
-
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
 
 /*
diff --git a/test/build.mk b/test/build.mk
index 8aa9059f23..6c69f639c1 100644
--- a/test/build.mk
+++ b/test/build.mk
@@ -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
diff --git a/test/gyro_cal.c b/test/gyro_cal.c
new file mode 100644
index 0000000000..85d53789ef
--- /dev/null
+++ b/test/gyro_cal.c
@@ -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();
+}
diff --git a/test/gyro_cal.tasklist b/test/gyro_cal.tasklist
new file mode 100644
index 0000000000..7d28eb5b64
--- /dev/null
+++ b/test/gyro_cal.tasklist
@@ -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)
diff --git a/test/test_config.h b/test/test_config.h
index 97a8c3e951..ed07f73983 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -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