summaryrefslogtreecommitdiff
path: root/common/accel_cal.c
diff options
context:
space:
mode:
Diffstat (limited to 'common/accel_cal.c')
-rw-r--r--common/accel_cal.c81
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;
+}