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>

1 files changed, 46 insertions, 0 deletions

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 */