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>

1 files changed, 81 insertions, 0 deletions

diff --git a/common/accel_cal.c b/common/accel_cal.c
new file mode 100644
index 0000000000..cf97ccbe56
--- /dev/null
+++ b/common/accel_cal.c
@@ -0,0 +1,81 @@
+/* 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 "console.h"
+#include "accel_cal.h"
+
+#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args)
+
+#define TEMP_RANGE (CONFIG_ACCEL_CAL_MAX_TEMP - CONFIG_ACCEL_CAL_MIN_TEMP)
+
+void accel_cal_reset(struct accel_cal *cal)
+{
+	int i;
+
+	for (i = 0; i < cal->num_temp_windows; ++i) {
+		kasa_reset(&(cal->algos[i].kasa_fit));
+		newton_fit_reset(&(cal->algos[i].newton_fit));
+	}
+}
+
+static inline int compute_temp_gate(const struct accel_cal *cal, fp_t temp)
+{
+	int gate = (int) fp_div(fp_mul(temp - CONFIG_ACCEL_CAL_MIN_TEMP,
+				       INT_TO_FP(cal->num_temp_windows)),
+				TEMP_RANGE);
+
+	return gate < cal->num_temp_windows
+		? gate : (cal->num_temp_windows - 1);
+}
+
+bool accel_cal_accumulate(struct accel_cal *cal, uint32_t sample_time, fp_t x,
+			  fp_t y, fp_t z, fp_t temp)
+{
+	struct accel_cal_algo *algo;
+
+	/* Test that we're within the temperature range. */
+	if (temp >= CONFIG_ACCEL_CAL_MAX_TEMP ||
+	    temp <= CONFIG_ACCEL_CAL_MIN_TEMP)
+		return false;
+
+	/* Test that we have a still sample. */
+	if (!still_det_update(&cal->still_det, sample_time, x, y, z))
+		return false;
+
+	/* We have a still sample, update x, y, and z to the mean. */
+	x = cal->still_det.mean_x;
+	y = cal->still_det.mean_y;
+	z = cal->still_det.mean_z;
+
+	/* Compute the temp gate. */
+	algo = &cal->algos[compute_temp_gate(cal, temp)];
+
+	kasa_accumulate(&algo->kasa_fit, x, y, z);
+	if (newton_fit_accumulate(&algo->newton_fit, x, y, z)) {
+		fp_t radius;
+
+		kasa_compute(&algo->kasa_fit, cal->bias, &radius);
+		if (ABS(radius - FLOAT_TO_FP(1.0f)) <
+		    CONFIG_ACCEL_CAL_KASA_RADIUS_THRES)
+			goto accel_cal_accumulate_success;
+
+		newton_fit_compute(&algo->newton_fit, cal->bias, &radius);
+		if (ABS(radius - FLOAT_TO_FP(1.0f)) <
+		    CONFIG_ACCEL_CAL_NEWTON_RADIUS_THRES)
+			goto accel_cal_accumulate_success;
+	}
+
+	return false;
+
+accel_cal_accumulate_success:
+	cal->bias[X] = cal->bias[X];
+	cal->bias[Y] = cal->bias[Y];
+	cal->bias[Z] = cal->bias[Z];
+
+	accel_cal_reset(cal);
+
+	return true;
+}