common: Implement kasa sphere fit algorithm

Add an implementation of the kasa sphere fit algorithm
adapted from AOSP.

BUG=b:138303429,chromium:1023858
TEST=Added unit tests
BRANCH=None

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

7 files changed, 212 insertions, 1 deletions

diff --git a/common/build.mk b/common/build.mk
index 372cf98eb2..0fc5a7d393 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -119,7 +119,8 @@ common-$(CONFIG_ROLLBACK)+=rollback.o
 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
+common-$(CONFIG_ONLINE_CALIB)+=stillness_detector.o kasa.o math_util.o \
+	mat44.o vec3.o
 common-$(CONFIG_SHA1)+= sha1.o
 common-$(CONFIG_SHA256)+=sha256.o
 common-$(CONFIG_SOFTWARE_CLZ)+=clz.o
diff --git a/common/kasa.c b/common/kasa.c
new file mode 100644
index 0000000000..79c75ad55b
--- /dev/null
+++ b/common/kasa.c
@@ -0,0 +1,74 @@
+/* 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 "kasa.h"
+#include "mat44.h"
+#include <string.h>
+
+void kasa_reset(struct kasa_fit *kasa)
+{
+	memset(kasa, 0, sizeof(struct kasa_fit));
+}
+
+void kasa_accumulate(struct kasa_fit *kasa, fp_t x, fp_t y, fp_t z)
+{
+	fp_t w = fp_sq(x) + fp_sq(y) + fp_sq(z);
+
+	kasa->acc_x += x;
+	kasa->acc_y += y;
+	kasa->acc_z += z;
+	kasa->acc_w += w;
+
+	kasa->acc_xx += fp_sq(x);
+	kasa->acc_xy += fp_mul(x, y);
+	kasa->acc_xz += fp_mul(x, z);
+	kasa->acc_xw += fp_mul(x, w);
+
+	kasa->acc_yy += fp_sq(y);
+	kasa->acc_yz += fp_mul(y, z);
+	kasa->acc_yw += fp_mul(y, w);
+
+	kasa->acc_zz += fp_sq(z);
+	kasa->acc_zw += fp_mul(z, w);
+
+	kasa->nsamples += 1;
+}
+
+void kasa_compute(struct kasa_fit *kasa, fpv3_t bias, fp_t *radius)
+{
+	/*    A    *   out   =    b
+	 * (4 x 4)   (4 x 1)   (4 x 1)
+	 */
+	mat44_fp_t A;
+	fpv4_t b, out;
+	sizev4_t pivot;
+
+	A[0][0] = kasa->nsamples;
+	A[0][1] = A[1][0] = kasa->acc_x;
+	A[0][2] = A[2][0] = kasa->acc_y;
+	A[0][3] = A[3][0] = kasa->acc_z;
+	A[1][1] = kasa->acc_xx;
+	A[1][2] = A[2][1] = kasa->acc_xy;
+	A[1][3] = A[3][1] = kasa->acc_xz;
+	A[2][2] = kasa->acc_yy;
+	A[2][3] = A[3][2] = kasa->acc_yz;
+	A[3][3] = kasa->acc_zz;
+
+	b[0] = -kasa->acc_w;
+	b[1] = -kasa->acc_xw;
+	b[2] = -kasa->acc_yw;
+	b[3] = -kasa->acc_zw;
+
+	mat44_fp_decompose_lup(A, pivot);
+	mat44_fp_solve(A, out, b, pivot);
+
+	bias[0] = fp_mul(out[1], FLOAT_TO_FP(-0.5f));
+	bias[1] = fp_mul(out[2], FLOAT_TO_FP(-0.5f));
+	bias[2] = fp_mul(out[3], FLOAT_TO_FP(-0.5f));
+
+	*radius = fpv3_dot(bias, bias) - out[0];
+	*radius = (*radius > 0) ? fp_sqrtf(*radius) : FLOAT_TO_FP(0.0f);
+}
diff --git a/include/kasa.h b/include/kasa.h
new file mode 100644
index 0000000000..6157b5632d
--- /dev/null
+++ b/include/kasa.h
@@ -0,0 +1,46 @@
+/* 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.
+ */
+
+/* Kasa sphere fit algorithm */
+
+#ifndef __CROS_EC_KASA_H
+#define __CROS_EC_KASA_H
+
+#include "vec3.h"
+
+struct kasa_fit {
+	fp_t acc_x, acc_y, acc_z, acc_w;
+	fp_t acc_xx, acc_xy, acc_xz, acc_xw;
+	fp_t acc_yy, acc_yz, acc_yw;
+	fp_t acc_zz, acc_zw;
+	uint32_t nsamples;
+};
+
+/**
+ * Resets the kasa_fit data structure (sets all variables to zero).
+ *
+ * @param kasa Pointer to the struct that should be reset.
+ */
+void kasa_reset(struct kasa_fit *kasa);
+
+/**
+ * Add a new sample to the kasa_fit structure.
+ *
+ * @param x The X component of the new sample.
+ * @param y The Y component of the new sample.
+ * @param z the Z component of the new sample.
+ */
+void kasa_accumulate(struct kasa_fit *kasa, fp_t x, fp_t y, fp_t z);
+
+/**
+ * Compute the current center/radius from the kasa_fit structure.
+ *
+ * @param kasa Pointer to the struct that should be used for the calculation.
+ * @param bias Pointer to the start of a fp_t[3] to save the computed center.
+ * @param radius Pointer to a fp_t that will hold the computed radius.
+ */
+void kasa_compute(struct kasa_fit *kasa, fpv3_t bias, fp_t *radius);
+
+#endif /* __CROS_EC_KASA_H */
diff --git a/test/build.mk b/test/build.mk
index 319bfc038c..97997ed8b3 100644
--- a/test/build.mk
+++ b/test/build.mk
@@ -39,6 +39,7 @@ test-list-host += inductive_charging
 test-list-host += interrupt
 test-list-host += is_enabled
 test-list-host += is_enabled_error
+test-list-host += kasa
 test-list-host += kb_8042
 test-list-host += kb_mkbp
 #test-list-host += kb_scan	# crbug.com/976974
@@ -127,6 +128,7 @@ 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
 motion_lid-y=motion_lid.o
 motion_sense_fifo-y=motion_sense_fifo.o
+kasa-y=kasa.o
 mutex-y=mutex.o
 nvmem-y=nvmem.o nvmem_tpm2_mock.o
 pingpong-y=pingpong.o
diff --git a/test/kasa.c b/test/kasa.c
new file mode 100644
index 0000000000..8221db2ae5
--- /dev/null
+++ b/test/kasa.c
@@ -0,0 +1,73 @@
+/* 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 "kasa.h"
+#include "test_util.h"
+#include "motion_sense.h"
+#include <stdio.h>
+
+struct motion_sensor_t motion_sensors[] = {};
+const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
+
+static int test_kasa_reset(void)
+{
+	struct kasa_fit kasa;
+
+	kasa_reset(&kasa);
+
+	TEST_EQ(kasa.nsamples, 0, "%u");
+	TEST_NEAR(kasa.acc_x, 0.0f, 0.000001f, "%f");
+	TEST_NEAR(kasa.acc_y, 0.0f, 0.000001f, "%f");
+	TEST_NEAR(kasa.acc_z, 0.0f, 0.000001f, "%f");
+	TEST_NEAR(kasa.acc_w, 0.0f, 0.000001f, "%f");
+
+	TEST_NEAR(kasa.acc_xx, 0.0f, 0.000001f, "%f");
+	TEST_NEAR(kasa.acc_xy, 0.0f, 0.000001f, "%f");
+	TEST_NEAR(kasa.acc_xz, 0.0f, 0.000001f, "%f");
+	TEST_NEAR(kasa.acc_xw, 0.0f, 0.000001f, "%f");
+
+	TEST_NEAR(kasa.acc_yy, 0.0f, 0.000001f, "%f");
+	TEST_NEAR(kasa.acc_yz, 0.0f, 0.000001f, "%f");
+	TEST_NEAR(kasa.acc_yw, 0.0f, 0.000001f, "%f");
+
+	TEST_NEAR(kasa.acc_zz, 0.0f, 0.000001f, "%f");
+	TEST_NEAR(kasa.acc_zw, 0.0f, 0.000001f, "%f");
+
+	return EC_SUCCESS;
+}
+
+static int test_kasa_calculate(void)
+{
+	struct kasa_fit kasa;
+	fpv3_t bias;
+	float radius;
+
+	kasa_reset(&kasa);
+	kasa_accumulate(&kasa, 1.01f, 0.01f, 0.01f);
+	kasa_accumulate(&kasa, -0.99f, 0.01f, 0.01f);
+	kasa_accumulate(&kasa, 0.01f, 1.01f, 0.01f);
+	kasa_accumulate(&kasa, 0.01f, -0.99f, 0.01f);
+	kasa_accumulate(&kasa, 0.01f, 0.01f, 1.01f);
+	kasa_accumulate(&kasa, 0.01f, 0.01f, -0.99f);
+	kasa_compute(&kasa, bias, &radius);
+
+	TEST_NEAR(bias[0], 0.01f, 0.0001f, "%f");
+	TEST_NEAR(bias[1], 0.01f, 0.0001f, "%f");
+	TEST_NEAR(bias[2], 0.01f, 0.0001f, "%f");
+	TEST_NEAR(radius, 1.0f, 0.0001f, "%f");
+
+	return EC_SUCCESS;
+}
+
+void run_test(void)
+{
+	test_reset();
+
+	RUN_TEST(test_kasa_reset);
+	RUN_TEST(test_kasa_calculate);
+
+	test_print_result();
+}
diff --git a/test/kasa.tasklist b/test/kasa.tasklist
new file mode 100644
index 0000000000..0e3696c3f0
--- /dev/null
+++ b/test/kasa.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/test_config.h b/test/test_config.h
index 9ab25f4e7e..9dbbd4e1d0 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -95,6 +95,11 @@
 #define CONFIG_TEMP_CACHE_STALE_THRES (1 * SECOND)
 #endif
 
+#ifdef TEST_KASA
+#define CONFIG_FPU
+#define CONFIG_ONLINE_CALIB
+#endif
+
 #if defined(TEST_MOTION_LID) || defined(TEST_MOTION_ANGLE) || \
 	defined(TEST_MOTION_ANGLE_TABLET) || defined(TEST_MOTION_SENSE_FIFO)
 enum sensor_id {