common: Implement accelerometer calibration

This change implements the hybrid accelerometer calibration
algorithm described in
https://drive.google.com/corp/drive/u/0/folders/13k8AWvVkCg8KUr1HhD2qv6_ob1ixgCbE

1. Waits until the device is still
2. Adds a still sample to an orientation accumulator
   - If the new sample is close to an existing one, they're
   merged.
   - If the new sample is unique, it is added to the list of
   orientations in a FIFO manner (may be evicting an older
   sample).
   - Once enough orientations have been gathered, run the
   kasa algorithm.
   - The kasa algorithm should yield a radius that's near 1g,
   since all the samples were added when still. If this isn't
   the case, we fall back on newton's method (which takes
   longer).

BUG=b:138303429,chromium:1023858
BRANCH=None
TEST=buildall with new unit tests

Change-Id: I98bb0d365017d8a916b008c7c0c263345a9cddac
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1879716
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>

8 files changed, 327 insertions, 2 deletions

diff --git a/common/accel_cal.c b/common/accel_cal.c
new file mode 100644
index 0000000000..cf97ccbe56
--- /dev/null
+++ b/common/accel_cal.c
@@ -0,0 +1,81 @@
+/* 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 "console.h"
+#include "accel_cal.h"
+
+#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args)
+
+#define TEMP_RANGE (CONFIG_ACCEL_CAL_MAX_TEMP - CONFIG_ACCEL_CAL_MIN_TEMP)
+
+void accel_cal_reset(struct accel_cal *cal)
+{
+	int i;
+
+	for (i = 0; i < cal->num_temp_windows; ++i) {
+		kasa_reset(&(cal->algos[i].kasa_fit));
+		newton_fit_reset(&(cal->algos[i].newton_fit));
+	}
+}
+
+static inline int compute_temp_gate(const struct accel_cal *cal, fp_t temp)
+{
+	int gate = (int) fp_div(fp_mul(temp - CONFIG_ACCEL_CAL_MIN_TEMP,
+				       INT_TO_FP(cal->num_temp_windows)),
+				TEMP_RANGE);
+
+	return gate < cal->num_temp_windows
+		? gate : (cal->num_temp_windows - 1);
+}
+
+bool accel_cal_accumulate(struct accel_cal *cal, uint32_t sample_time, fp_t x,
+			  fp_t y, fp_t z, fp_t temp)
+{
+	struct accel_cal_algo *algo;
+
+	/* Test that we're within the temperature range. */
+	if (temp >= CONFIG_ACCEL_CAL_MAX_TEMP ||
+	    temp <= CONFIG_ACCEL_CAL_MIN_TEMP)
+		return false;
+
+	/* Test that we have a still sample. */
+	if (!still_det_update(&cal->still_det, sample_time, x, y, z))
+		return false;
+
+	/* We have a still sample, update x, y, and z to the mean. */
+	x = cal->still_det.mean_x;
+	y = cal->still_det.mean_y;
+	z = cal->still_det.mean_z;
+
+	/* Compute the temp gate. */
+	algo = &cal->algos[compute_temp_gate(cal, temp)];
+
+	kasa_accumulate(&algo->kasa_fit, x, y, z);
+	if (newton_fit_accumulate(&algo->newton_fit, x, y, z)) {
+		fp_t radius;
+
+		kasa_compute(&algo->kasa_fit, cal->bias, &radius);
+		if (ABS(radius - FLOAT_TO_FP(1.0f)) <
+		    CONFIG_ACCEL_CAL_KASA_RADIUS_THRES)
+			goto accel_cal_accumulate_success;
+
+		newton_fit_compute(&algo->newton_fit, cal->bias, &radius);
+		if (ABS(radius - FLOAT_TO_FP(1.0f)) <
+		    CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES)
+			goto accel_cal_accumulate_success;
+	}
+
+	return false;
+
+accel_cal_accumulate_success:
+	cal->bias[X] = cal->bias[X];
+	cal->bias[Y] = cal->bias[Y];
+	cal->bias[Z] = cal->bias[Z];
+
+	accel_cal_reset(cal);
+
+	return true;
+}
diff --git a/common/build.mk b/common/build.mk
index beb01a83a7..cc45be50c0 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -121,7 +121,7 @@ common-$(CONFIG_RWSIG)+=rwsig.o vboot/common.o
 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
+	mat44.o vec3.o newton_fit.o accel_cal.o
 common-$(CONFIG_SHA1)+= sha1.o
 common-$(CONFIG_SHA256)+=sha256.o
 common-$(CONFIG_SOFTWARE_CLZ)+=clz.o
diff --git a/include/accel_cal.h b/include/accel_cal.h
new file mode 100644
index 0000000000..cfba5b915d
--- /dev/null
+++ b/include/accel_cal.h
@@ -0,0 +1,51 @@
+/* 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.
+ */
+
+/* Online accelerometer calibration */
+
+#ifndef __CROS_EC_ACCEL_CAL_H
+#define __CROS_EC_ACCEL_CAL_H
+
+#include "kasa.h"
+#include "newton_fit.h"
+#include "stdbool.h"
+#include "stillness_detector.h"
+
+struct accel_cal_algo {
+	struct kasa_fit kasa_fit;
+	struct newton_fit newton_fit;
+};
+
+struct accel_cal {
+	struct still_det still_det;
+	struct accel_cal_algo *algos;
+	uint8_t num_temp_windows;
+	fpv3_t bias;
+};
+
+/**
+ * Reset the accelerometer calibration object. This should only be called
+ * once. The struct will reset automatically in accel_cal_accumulate when
+ * a new calibration is computed.
+ *
+ * @param cal Pointer to the accel_cal struct to reset.
+ */
+void accel_cal_reset(struct accel_cal *cal);
+
+/**
+ * Add new reading to the accelerometer calibration.
+ *
+ * @param cal Pointer to the accel_cal struct to update.
+ * @param sample_time The timestamp when the sample was taken.
+ * @param x X component of the new reading.
+ * @param y Y component of the new reading.
+ * @param z Z component of the new reading.
+ * @param temp The sensor's internal temperature in degrees C.
+ * @return True if a new bias is available.
+ */
+bool accel_cal_accumulate(struct accel_cal *cal, uint32_t sample_time, fp_t x,
+			  fp_t y, fp_t z, fp_t temp);
+
+#endif /* __CROS_EC_ACCEL_CAL_H */
diff --git a/include/config.h b/include/config.h
index 3fdc8de174..fcd8978840 100644
--- a/include/config.h
+++ b/include/config.h
@@ -2765,6 +2765,22 @@
  */
 #undef CONFIG_TEMP_CACHE_STALE_THRES
 
+/* Set minimum temperature for accelerometer calibration. */
+#undef CONFIG_ACCEL_CAL_MIN_TEMP
+
+/* Set maximum temperature for accelerometer calibration. */
+#undef CONFIG_ACCEL_CAL_MAX_TEMP
+
+/* Set threshold radius for using the Kasa algorithm in accelerometer bias
+ * calculation (g).
+ */
+#undef CONFIG_ACCEL_CAL_KASA_RADIUS_THRES
+
+/* Set threshold radius for using the Newton fit algorithm in accelerometer
+ * bias calculation (g).
+ */
+#undef CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES
+
 /* Include code to do online compass calibration */
 #undef CONFIG_MAG_CALIBRATE
 
@@ -5251,4 +5267,23 @@
 #error "Online calibration requires CONFIG_FPU"
 #endif
 
+/* Set default values for accelerometer calibration if not defined. */
+#ifdef CONFIG_ONLINE_CALIB
+#ifndef CONFIG_ACCEL_CAL_MIN_TEMP
+#define CONFIG_ACCEL_CAL_MIN_TEMP 0.0f
+#endif
+
+#ifndef CONFIG_ACCEL_CAL_MAX_TEMP
+#define CONFIG_ACCEL_CAL_MAX_TEMP 45.0f
+#endif
+
+#ifndef CONFIG_ACCEL_CAL_KASA_RADIUS_THRES
+#define CONFIG_ACCEL_CAL_KASA_RADIUS_THRES 0.001f
+#endif
+
+#ifndef CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES
+#define CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES 0.001f
+#endif
+#endif /* CONFIG_ONLINE_CALIB */
+
 #endif  /* __CROS_EC_CONFIG_H */
diff --git a/test/accel_cal.c b/test/accel_cal.c
new file mode 100644
index 0000000000..e7dd876995
--- /dev/null
+++ b/test/accel_cal.c
@@ -0,0 +1,137 @@
+/* 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 "accel_cal.h"
+#include "test_util.h"
+#include "motion_sense.h"
+#include <math.h>
+
+struct motion_sensor_t motion_sensors[] = {};
+const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
+
+struct accel_cal_algo algos[2] = {
+	{
+		.newton_fit = NEWTON_FIT(8, 1, 0.01f, 0.25f, 1.0e-8f, 100),
+	},
+	{
+		.newton_fit = NEWTON_FIT(8, 1, 0.01f, 0.25f, 1.0e-8f, 100),
+	}
+};
+
+struct accel_cal cal = {
+	.still_det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5),
+	.algos = algos,
+	.num_temp_windows = ARRAY_SIZE(algos),
+};
+
+static bool accumulate(float x, float y, float z, float temperature)
+{
+	return accel_cal_accumulate(&cal, 0, x, y, z, temperature)
+		| accel_cal_accumulate(&cal, 200 * MSEC, x, y, z, temperature)
+		| accel_cal_accumulate(&cal, 400 * MSEC, x, y, z, temperature)
+		| accel_cal_accumulate(&cal, 600 * MSEC, x, y, z, temperature)
+		| accel_cal_accumulate(&cal, 800 * MSEC, x, y, z, temperature)
+		| accel_cal_accumulate(&cal, 1000 * MSEC, x, y, z, temperature);
+}
+
+static int test_calibrated_correctly_with_kasa(void)
+{
+	bool has_bias;
+
+	accumulate(1.01f, 0.01f, 0.01f, 21.0f);
+	accumulate(-0.99f, 0.01f, 0.01f, 21.0f);
+	accumulate(0.01f, 1.01f, 0.01f, 21.0f);
+	accumulate(0.01f, -0.99f, 0.01f, 21.0f);
+	accumulate(0.01f, 0.01f, 1.01f, 21.0f);
+	accumulate(0.01f, 0.01f, -0.99f, 21.0f);
+	accumulate(0.7171f, 0.7171f, 0.7171f, 21.0f);
+	has_bias = accumulate(-0.6971f, -0.6971f, -0.6971f, 21.0f);
+
+	TEST_EQ(has_bias, true, "%d");
+	TEST_NEAR(cal.bias[X], 0.01f, 0.0001f, "%f");
+	TEST_NEAR(cal.bias[Y], 0.01f, 0.0001f, "%f");
+	TEST_NEAR(cal.bias[Z], 0.01f, 0.0001f, "%f");
+
+	return EC_SUCCESS;
+}
+
+static int test_calibrated_correctly_with_newton(void)
+{
+	bool has_bias = false;
+	struct kasa_fit kasa;
+	fpv3_t kasa_bias;
+	float kasa_radius;
+	int i;
+	float data[] = {
+		1.00290f, 0.09170f, 0.09649f,
+		0.95183f, 0.23626f, 0.25853f,
+		0.95023f, 0.15387f, 0.31865f,
+		0.97374f, 0.01639f, 0.27675f,
+		0.88521f, 0.30212f, 0.39558f,
+		0.92787f, 0.35157f, 0.21209f,
+		0.95162f, 0.33173f, 0.10924f,
+		0.98397f, 0.22644f, 0.07737f,
+	};
+
+	kasa_reset(&kasa);
+	for (i = 0; i < ARRAY_SIZE(data); i += 3) {
+		TEST_EQ(has_bias, false, "%d");
+		kasa_accumulate(&kasa,  data[i], data[i + 1], data[i + 2]);
+		has_bias = accumulate(data[i], data[i + 1], data[i + 2], 21.0f);
+	}
+
+	kasa_compute(&kasa, kasa_bias, &kasa_radius);
+	TEST_EQ(has_bias, true, "%d");
+	/* Check that the bias is right */
+	TEST_NEAR(cal.bias[X], 0.01f, 0.001f, "%f");
+	TEST_NEAR(cal.bias[Y], 0.01f, 0.001f, "%f");
+	TEST_NEAR(cal.bias[Z], 0.01f, 0.001f, "%f");
+	/* Demonstrate that we got a better bias compared to kasa */
+	TEST_LT(sqrtf(powf(cal.bias[X] - 0.01f, 2.0f) +
+		      powf(cal.bias[Y] - 0.01f, 2.0f) +
+		      powf(cal.bias[Z] - 0.01f, 2.0f)),
+		sqrtf(powf(kasa_bias[X] - 0.01f, 2.0f) +
+		      powf(kasa_bias[Y] - 0.01f, 2.0f) +
+		      powf(kasa_bias[Z] - 0.01f, 2.0f)),
+		"%f");
+
+	return EC_SUCCESS;
+}
+
+static int test_temperature_gates(void)
+{
+	bool has_bias;
+
+	accumulate(1.01f, 0.01f, 0.01f, 21.0f);
+	accumulate(-0.99f, 0.01f, 0.01f, 21.0f);
+	accumulate(0.01f, 1.01f, 0.01f, 21.0f);
+	accumulate(0.01f, -0.99f, 0.01f, 21.0f);
+	accumulate(0.01f, 0.01f, 1.01f, 21.0f);
+	accumulate(0.01f, 0.01f, -0.99f, 21.0f);
+	accumulate(0.7171f, 0.7171f, 0.7171f, 21.0f);
+	has_bias = accumulate(-0.6971f, -0.6971f, -0.6971f, 31.0f);
+
+	TEST_EQ(has_bias, false, "%d");
+
+	return EC_SUCCESS;
+}
+
+void before_test(void)
+{
+	cal.still_det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
+	accel_cal_reset(&cal);
+}
+
+void run_test(void)
+{
+	test_reset();
+
+	RUN_TEST(test_calibrated_correctly_with_kasa);
+	RUN_TEST(test_calibrated_correctly_with_newton);
+	RUN_TEST(test_temperature_gates);
+
+	test_print_result();
+}
diff --git a/test/accel_cal.tasklist b/test/accel_cal.tasklist
new file mode 100644
index 0000000000..0e3696c3f0
--- /dev/null
+++ b/test/accel_cal.tasklist
@@ -0,0 +1,10 @@
+/* Copyright 2019 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/build.mk b/test/build.mk
index bcb1278ae7..c7462da559 100644
--- a/test/build.mk
+++ b/test/build.mk
@@ -11,7 +11,8 @@ test-list-y ?= pingpong timer_calib timer_dos timer_jump mutex utils utils_str
 ifneq ($(TEST_LIST_HOST),)
 test-list-host=$(TEST_LIST_HOST)
 else
-test-list-host = aes
+test-list-host = accel_cal
+test-list-host += aes
 test-list-host += base32
 test-list-host += battery_get_params_smart
 test-list-host += bklight_lid
@@ -97,6 +98,7 @@ test-list-host += x25519
 test-list-host += stillness_detector
 endif
 
+accel_cal-y=accel_cal.o
 aes-y=aes.o
 base32-y=base32.o
 battery_get_params_smart-y=battery_get_params_smart.o
diff --git a/test/test_config.h b/test/test_config.h
index 75e68f853d..51275afb30 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -105,6 +105,15 @@
 #define CONFIG_ONLINE_CALIB
 #endif
 
+#ifdef TEST_ACCEL_CAL
+#define CONFIG_FPU
+#define CONFIG_ONLINE_CALIB
+#define CONFIG_ACCEL_CAL_MIN_TEMP 20.0f
+#define CONFIG_ACCEL_CAL_MAX_TEMP 40.0f
+#define CONFIG_ACCEL_CAL_KASA_RADIUS_THRES 0.1f
+#define CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES 0.1f
+#endif
+
 #ifdef TEST_NEWTON_FIT
 #define CONFIG_FPU
 #define CONFIG_ONLINE_CALIB