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>

4 files changed, 159 insertions, 1 deletions

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