common: mag_cal: update magnetometer to leverage kasa

Update magnetometer calibration algorithm to leverage
the new kasa standalone code.

BUG=b:138303429,chromium:1023858
TEST=added unit test
BRANCH=None

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

7 files changed, 161 insertions, 141 deletions

diff --git a/common/build.mk b/common/build.mk
index 0fc5a7d393..04ec98bcd0 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -101,7 +101,8 @@ common-$(CONFIG_LID_ANGLE)+=motion_lid.o math_util.o
 common-$(CONFIG_LID_ANGLE_UPDATE)+=lid_angle.o
 common-$(CONFIG_LID_SWITCH)+=lid_switch.o
 common-$(CONFIG_HOSTCMD_X86)+=acpi.o port80.o ec_features.o
-common-$(CONFIG_MAG_CALIBRATE)+= mag_cal.o math_util.o vec3.o mat33.o mat44.o
+common-$(CONFIG_MAG_CALIBRATE)+= mag_cal.o math_util.o vec3.o mat33.o mat44.o \
+	kasa.o
 common-$(CONFIG_MKBP_EVENT)+=mkbp_event.o
 common-$(CONFIG_ONEWIRE)+=onewire.o
 common-$(CONFIG_PECI_COMMON)+=peci.o
diff --git a/common/mag_cal.c b/common/mag_cal.c
index db09050007..1dc40e34db 100644
--- a/common/mag_cal.c
+++ b/common/mag_cal.c
@@ -26,6 +26,18 @@
 #define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
 #define PRINTF_FLOAT(x)  ((int)((x) * 100.0f))
 
+/**
+ * Compute the covariance element: (avg(ab) - avg(a)*avg(b))
+ *
+ * @param sq The accumulated sum of a*b
+ * @param a The accumulated sum of a
+ * @param b The accumulated sum of b
+ * @return (sq - ((a * b) * inv)) * inv
+ */
+static inline fp_t covariance_element(fp_t sq, fp_t a, fp_t b, fp_t inv)
+{
+	return fp_mul(sq - fp_mul(fp_mul(a, b), inv), inv);
+}
 /*
  * eigen value magnitude and ratio test
  *
@@ -38,18 +50,35 @@ static int moc_eigen_test(struct mag_cal_t *moc)
 	fpv3_t eigenvals;
 	mat33_fp_t eigenvecs;
 	fp_t evmax, evmin, evmag;
+	fp_t inv = fp_div_dbz(FLOAT_TO_FP(1.0f),
+			      INT_TO_FP((int) moc->kasa_fit.nsamples));
 	int eigen_pass;
 
 	/* covariance matrix */
-	S[0][0] = moc->acc[0][0] - fp_sq(moc->acc[0][3]);
-	S[0][1] = S[1][0] =
-		moc->acc[0][1] - fp_mul(moc->acc[0][3], moc->acc[1][3]);
-	S[0][2] = S[2][0] =
-		moc->acc[0][2] - fp_mul(moc->acc[0][3], moc->acc[2][3]);
-	S[1][1] = moc->acc[1][1] - fp_sq(moc->acc[1][3]);
-	S[1][2] = S[2][1] =
-		moc->acc[1][2] - fp_mul(moc->acc[1][3], moc->acc[2][3]);
-	S[2][2] = moc->acc[2][2] - fp_sq(moc->acc[2][3]);
+	S[0][0] = covariance_element(moc->kasa_fit.acc_xx,
+				     moc->kasa_fit.acc_x,
+				     moc->kasa_fit.acc_x,
+				     inv);
+	S[0][1] = S[1][0] = covariance_element(moc->kasa_fit.acc_xy,
+					       moc->kasa_fit.acc_x,
+					       moc->kasa_fit.acc_y,
+					       inv);
+	S[0][2] = S[2][0] = covariance_element(moc->kasa_fit.acc_xz,
+					       moc->kasa_fit.acc_x,
+					       moc->kasa_fit.acc_z,
+					       inv);
+	S[1][1] = covariance_element(moc->kasa_fit.acc_yy,
+				     moc->kasa_fit.acc_y,
+				     moc->kasa_fit.acc_y,
+				     inv);
+	S[1][2] = S[2][1] = covariance_element(moc->kasa_fit.acc_yz,
+					       moc->kasa_fit.acc_y,
+					       moc->kasa_fit.acc_z,
+					       inv);
+	S[2][2] = covariance_element(moc->kasa_fit.acc_zz,
+				     moc->kasa_fit.acc_z,
+				     moc->kasa_fit.acc_z,
+				     inv);
 
 	mat33_fp_get_eigenbasis(S, eigenvals, eigenvecs);
 
@@ -66,88 +95,23 @@ static int moc_eigen_test(struct mag_cal_t *moc)
 		&& (evmag < MAX_EIGEN_MAG);
 
 #if 0
-	CPRINTF("mag eigenvalues: (%d %d %d), ",
+	CPRINTF("mag eigenvalues: (%.02d %.02d %.02d), ",
 		PRINTF_FLOAT(eigenvals[X]),
 		PRINTF_FLOAT(eigenvals[Y]),
 		PRINTF_FLOAT(eigenvals[Z]));
 
-	CPRINTF("ratio %d, mag %d: pass %d\r\n",
+	CPRINTF("ratio %.02d, mag %.02d: pass %d\r\n",
 		PRINTF_FLOAT(evmax / evmin),
 		PRINTF_FLOAT(evmag),
-		PRINTF_FLOAT(eigen_pass));
+		eigen_pass);
 #endif
 
 	return eigen_pass;
 }
 
-/*
- * Kasa sphere fitting with normal equation
- */
-static int moc_fit(struct mag_cal_t *moc, fpv3_t bias, fp_t *radius)
-{
-	sizev4_t pivot;
-	fpv4_t out;
-	int success = 0;
-
-	/*
-	 * To reduce stack size, moc->acc is A,
-	 * moc->acc_w is b: we are looking for out, where:
-	 *
-	 *    A    *   out   =    b
-	 * (4 x 4)   (4 x 1)   (4 x 1)
-	 */
-	/* complete the matrix: */
-	moc->acc[1][0] = moc->acc[0][1];
-	moc->acc[2][0] = moc->acc[0][2];
-	moc->acc[2][1] = moc->acc[1][2];
-	moc->acc[3][0] = moc->acc[0][3];
-	moc->acc[3][1] = moc->acc[1][3];
-	moc->acc[3][2] = moc->acc[2][3];
-	moc->acc[3][3] = FLOAT_TO_FP(1.0f);
-
-	moc->acc_w[X] = fp_mul(moc->acc_w[X], FLOAT_TO_FP(-1));
-	moc->acc_w[Y] = fp_mul(moc->acc_w[Y], FLOAT_TO_FP(-1));
-	moc->acc_w[Z] = fp_mul(moc->acc_w[Z], FLOAT_TO_FP(-1));
-	moc->acc_w[W] = fp_mul(moc->acc_w[W], FLOAT_TO_FP(-1));
-
-	mat44_fp_decompose_lup(moc->acc, pivot);
-
-	mat44_fp_solve(moc->acc, out, moc->acc_w, pivot);
-
-	/*
-	 * spherei is defined by:
-	 * (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
-	 *
-	 * Where r is:
-	 * xc = -out[X] / 2, yc = -out[Y] / 2, zc = -out[Z] / 2
-	 * r = sqrt(xc^2 + yc^2 + zc^2 - out[W])
-	 */
-
-	memcpy(bias, out, sizeof(fpv3_t));
-	fpv3_scalar_mul(bias, FLOAT_TO_FP(-0.5f));
-
-	*radius = fp_sqrtf(fpv3_dot(bias, bias) - out[W]);
-
-#if 0
-	CPRINTF("mag cal: bias (%d, %d, %d), R %d uT\n",
-		PRINTF_FLOAT(bias[X] / MAG_CAL_RAW_UT),
-		PRINTF_FLOAT(bias[Y] / MAG_CAL_RAW_UT),
-		PRINTF_FLOAT(bias[Z] / MAG_CAL_RAW_UT),
-		PRINTF_FLOAT(*radius / MAG_CAL_RAW_UT));
-#endif
-
-	/* TODO (menghsuan): bound on bias as well? */
-	if (*radius > MIN_FIT_MAG && *radius < MAX_FIT_MAG)
-		success = 1;
-
-	return success;
-}
-
 void init_mag_cal(struct mag_cal_t *moc)
 {
-	memset(moc->acc, 0, sizeof(moc->acc));
-	memset(moc->acc_w, 0, sizeof(moc->acc_w));
-	moc->nsamples = 0;
+	kasa_reset(&moc->kasa_fit);
 }
 
 int mag_cal_update(struct mag_cal_t *moc, const intv3_t v)
@@ -155,61 +119,22 @@ int mag_cal_update(struct mag_cal_t *moc, const intv3_t v)
 	int new_bias = 0;
 
 	/* 1. run accumulators */
-	fp_t w = fp_sq(v[X]) + fp_sq(v[Y]) + fp_sq(v[Z]);
-
-	moc->acc[0][3] += v[X];
-	moc->acc[1][3] += v[Y];
-	moc->acc[2][3] += v[Z];
-	moc->acc_w[W] += w;
-
-	moc->acc[0][0] += fp_sq(v[X]);
-	moc->acc[0][1] += fp_mul(v[X], v[Y]);
-	moc->acc[0][2] += fp_mul(v[X], v[Z]);
-	moc->acc_w[X] += fp_mul(v[X], w);
-
-	moc->acc[1][1] += fp_sq(v[Y]);
-	moc->acc[1][2] += fp_mul(v[Y], v[Z]);
-	moc->acc_w[Y] += fp_mul(v[Y], w);
-
-	moc->acc[2][2] += fp_sq(v[Z]);
-	moc->acc_w[Z] += fp_mul(v[Z], w);
-
-	if (moc->nsamples < MAG_CAL_MAX_SAMPLES)
-		moc->nsamples++;
+	kasa_accumulate(&moc->kasa_fit, INT_TO_FP(v[X]), INT_TO_FP(v[Y]),
+			INT_TO_FP(v[Z]));
 
 	/* 2. batch has enough samples? */
-	if (moc->batch_size > 0 && moc->nsamples >= moc->batch_size) {
-		fp_t inv = fp_div_dbz(FLOAT_TO_FP(1.0f),
-				  INT_TO_FP((int)moc->nsamples));
-
-		moc->acc[0][3] = fp_mul(moc->acc[0][3], inv);
-		moc->acc[1][3] = fp_mul(moc->acc[1][3], inv);
-		moc->acc[2][3] = fp_mul(moc->acc[2][3], inv);
-		moc->acc_w[W] = fp_mul(moc->acc_w[W], inv);
-
-		moc->acc[0][0] = fp_mul(moc->acc[0][0], inv);
-		moc->acc[0][1] = fp_mul(moc->acc[0][1], inv);
-		moc->acc[0][2] = fp_mul(moc->acc[0][2], inv);
-		moc->acc_w[X] = fp_mul(moc->acc_w[X], inv);
-
-		moc->acc[1][1] = fp_mul(moc->acc[1][1], inv);
-		moc->acc[1][2] = fp_mul(moc->acc[1][2], inv);
-		moc->acc_w[Y] = fp_mul(moc->acc_w[Y], inv);
-
-		moc->acc[2][2] = fp_mul(moc->acc[2][2], inv);
-		moc->acc_w[Z] = fp_mul(moc->acc_w[Z], inv);
-
+	if (moc->batch_size > 0 && moc->kasa_fit.nsamples >= moc->batch_size) {
 		/* 3. eigen test */
 		if (moc_eigen_test(moc)) {
 			fpv3_t bias;
 			fp_t radius;
 
 			/* 4. Kasa sphere fitting */
-			if (moc_fit(moc, bias, &radius)) {
-
-				moc->bias[X] = fp_mul(bias[X], FLOAT_TO_FP(-1));
-				moc->bias[Y] = fp_mul(bias[Y], FLOAT_TO_FP(-1));
-				moc->bias[Z] = fp_mul(bias[Z], FLOAT_TO_FP(-1));
+			kasa_compute(&moc->kasa_fit, bias, &radius);
+			if (radius > MIN_FIT_MAG && radius < MAX_FIT_MAG) {
+				moc->bias[X] = FP_TO_INT(bias[X]);
+				moc->bias[Y] = FP_TO_INT(bias[Y]);
+				moc->bias[Z] = FP_TO_INT(bias[Z]);
 
 				moc->radius = radius;
 
diff --git a/include/mag_cal.h b/include/mag_cal.h
index b0b35ded6f..5c12e00be7 100644
--- a/include/mag_cal.h
+++ b/include/mag_cal.h
@@ -11,33 +11,20 @@
 #include "math_util.h"
 #include "mat44.h"
 #include "vec4.h"
+#include "kasa.h"
 
 #define MAG_CAL_MAX_SAMPLES 0xffff
 #define MAG_CAL_MIN_BATCH_WINDOW_US    SECOND
 #define MAG_CAL_MIN_BATCH_SIZE      25      /* samples */
 
 struct mag_cal_t {
-	/*
-	 * Matric for sphere fitting:
-	 * +----+----+----+----+
-	 * | xx | xy | xz | x  |
-	 * +----+----+----+----+
-	 * | xy | yy | yz | y  |
-	 * +----+----+----+----+
-	 * | xz | yz | zz | z  |
-	 * +----+----+----+----+
-	 * | x  | y  | z  | 1  |
-	 * +----+----+----+----+
-	 */
-	mat44_fp_t acc;
-	fpv4_t acc_w;
+	struct kasa_fit kasa_fit;
 	fp_t radius;
 
 	intv3_t bias;
 
 	/* number of samples needed to calibrate */
 	uint16_t batch_size;
-	uint16_t nsamples;
 };
 
 void init_mag_cal(struct mag_cal_t *moc);
diff --git a/test/build.mk b/test/build.mk
index 97997ed8b3..c577226fdb 100644
--- a/test/build.mk
+++ b/test/build.mk
@@ -45,6 +45,7 @@ test-list-host += kb_mkbp
 #test-list-host += kb_scan	# crbug.com/976974
 test-list-host += lid_sw
 test-list-host += lightbar
+test-list-host += mag_cal
 test-list-host += math_util
 test-list-host += motion_angle
 test-list-host += motion_angle_tablet
@@ -123,6 +124,7 @@ kb_mkbp-y=kb_mkbp.o
 kb_scan-y=kb_scan.o
 lid_sw-y=lid_sw.o
 lightbar-y=lightbar.o
+mag_cal-y=mag_cal.o
 math_util-y=math_util.o
 motion_angle-y=motion_angle.o motion_angle_data_literals.o motion_common.o
 motion_angle_tablet-y=motion_angle_tablet.o motion_angle_data_literals_tablet.o motion_common.o
diff --git a/test/mag_cal.c b/test/mag_cal.c
new file mode 100644
index 0000000000..e1931c352a
--- /dev/null
+++ b/test/mag_cal.c
@@ -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.
+ */
+
+#include "common.h"
+#include "mag_cal.h"
+#include "test_util.h"
+#include <stdio.h>
+
+/**
+ * Various samples that might be seen in the wild. Normal range for magnetic
+ * fields is around 80 uT. This translates to roughly +/-525 units for the
+ * lis2mdl sensor.
+ *
+ * Random numbers were generated using the range of [518,532] (+- 2.14 uT) for
+ * the high values and [-5,5] (+- 1.53 uT) for the low values.
+ */
+static intv3_t samples[] = {
+	{ -522, 5, -5 },
+	{ -528, -3, 1 },
+	{ -531, -2, 0 },
+	{ -525, -1, 3 },
+
+	{ 527, 3, -2 },
+	{ 523, -5, 1 },
+	{ 520, -3, 2 },
+	{ 522, 0, -4 },
+
+	{ -3, -519, -2 },
+	{ 1, -521, 5 },
+	{ 2, -526, 4 },
+	{ 0, -532, -5 },
+
+	{ -5, 528, 4 },
+	{ -2, 531, -4 },
+	{ 1, 522, 2 },
+	{ 5, 532, 3 },
+
+	{ -5, 0, -524 },
+	{ -1, -2, -527 },
+	{ -3, 4, -532 },
+	{ 5, 3, -531 },
+
+	{ 4, -2, 524 },
+	{ 1, 3, 520 },
+	{ 5, -5, 528 },
+	{ 0, 2, 521 },
+};
+
+static int test_mag_cal_computes_bias(void)
+{
+	struct mag_cal_t cal;
+	int i;
+
+	init_mag_cal(&cal);
+	cal.batch_size = ARRAY_SIZE(samples);
+
+	/* Test that we don't calibrate until we added the final sample. */
+	for (i = 0; i < cal.batch_size - 1; ++i)
+		TEST_EQ(0, mag_cal_update(&cal, samples[i]), "%d");
+	/* Add the final sample and check calibration. */
+	TEST_EQ(1, mag_cal_update(&cal, samples[cal.batch_size - 1]), "%d");
+	TEST_EQ(525, FP_TO_INT(cal.radius), "%d");
+	TEST_EQ(-1, cal.bias[0], "%d");
+	TEST_EQ(1, cal.bias[1], "%d");
+	TEST_EQ(-2, cal.bias[2], "%d");
+
+	/*
+	 * State should have reset, run the same code again to verify that
+	 * we get the same calibration.
+	 */
+	for (i = 0; i < cal.batch_size - 1; ++i)
+		TEST_EQ(0, mag_cal_update(&cal, samples[i]), "%d");
+	TEST_EQ(1, mag_cal_update(&cal, samples[cal.batch_size - 1]), "%d");
+	TEST_EQ(525, FP_TO_INT(cal.radius), "%d");
+	TEST_EQ(-1, cal.bias[0], "%d");
+	TEST_EQ(1, cal.bias[1], "%d");
+	TEST_EQ(-2, cal.bias[2], "%d");
+
+	return EC_SUCCESS;
+}
+
+void run_test(void)
+{
+	test_reset();
+
+	RUN_TEST(test_mag_cal_computes_bias);
+
+	test_print_result();
+}
diff --git a/test/mag_cal.tasklist b/test/mag_cal.tasklist
new file mode 100644
index 0000000000..ff715f69cd
--- /dev/null
+++ b/test/mag_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  /* No test task */
+
diff --git a/test/test_config.h b/test/test_config.h
index 9dbbd4e1d0..3c269c4946 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -65,6 +65,10 @@
 #define CONFIG_MATH_UTIL
 #endif
 
+#ifdef TEST_MAG_CAL
+#define CONFIG_MAG_CALIBRATE
+#endif
+
 #ifdef TEST_STILLNESS_DETECTOR
 #define CONFIG_FPU
 #define CONFIG_ONLINE_CALIB