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>

4 files changed, 90 insertions, 0 deletions

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 {