common: Add sensor stillness detector

This change adds a stillness detector for 3d sensors.
This will be needed to filter sensor readings when
calibrating later.

BUG=b:138303429,chromium:1023858
BRANCH=None
TEST=buildall with new unit tests
Change-Id: I919ae7533fd42b0394de66aa0585e58343a662cc
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1833157
Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>

9 files changed, 355 insertions, 5 deletions

diff --git a/common/build.mk b/common/build.mk
index 2a976936d2..51ced0ae42 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -119,6 +119,7 @@ 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_SHA1)+= sha1.o
 common-$(CONFIG_SHA256)+=sha256.o
 common-$(CONFIG_SOFTWARE_CLZ)+=clz.o
diff --git a/common/stillness_detector.c b/common/stillness_detector.c
new file mode 100644
index 0000000000..c33472aa22
--- /dev/null
+++ b/common/stillness_detector.c
@@ -0,0 +1,110 @@
+/* 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.
+ */
+
+#include "common.h"
+#include "stillness_detector.h"
+#include "timer.h"
+#include <string.h>
+
+static void still_det_reset(struct still_det *still_det)
+{
+	still_det->num_samples = 0;
+	still_det->acc_x = FLOAT_TO_FP(0.0f);
+	still_det->acc_y = FLOAT_TO_FP(0.0f);
+	still_det->acc_z = FLOAT_TO_FP(0.0f);
+	still_det->acc_xx = FLOAT_TO_FP(0.0f);
+	still_det->acc_yy = FLOAT_TO_FP(0.0f);
+	still_det->acc_zz = FLOAT_TO_FP(0.0f);
+}
+
+static bool stillness_batch_complete(struct still_det *still_det,
+				     uint32_t sample_time)
+{
+	bool complete = false;
+	uint32_t batch_window = time_until(still_det->window_start_time,
+					   sample_time);
+
+	/* Checking if enough data is accumulated */
+	if (batch_window >= still_det->min_batch_window &&
+	    still_det->num_samples > still_det->min_batch_size) {
+		if (batch_window <= still_det->max_batch_window) {
+			complete = true;
+		} else {
+			/* Checking for too long batch window, reset and start
+			 * over
+			 */
+			still_det_reset(still_det);
+		}
+	} else if (batch_window > still_det->min_batch_window &&
+		   still_det->num_samples < still_det->min_batch_size) {
+		/* Not enough samples collected, reset and start over */
+		still_det_reset(still_det);
+	}
+	return complete;
+}
+
+static inline fp_t compute_variance(fp_t acc_squared, fp_t acc, fp_t inv)
+{
+	/* (acc^2 - (acc * acc * inv)) * inv */
+	return fp_mul((acc_squared - fp_mul(fp_sq(acc), inv)), inv);
+}
+
+bool still_det_update(struct still_det *still_det, uint32_t sample_time,
+		      fp_t x, fp_t y, fp_t z)
+{
+	fp_t inv = FLOAT_TO_FP(0.0f), var_x, var_y, var_z;
+	bool complete = false;
+
+	/* Accumulate for mean and VAR */
+	still_det->acc_x += x;
+	still_det->acc_y += y;
+	still_det->acc_z += z;
+	still_det->acc_xx += fp_mul(x, x);
+	still_det->acc_yy += fp_mul(y, y);
+	still_det->acc_zz += fp_mul(z, z);
+
+	switch (++still_det->num_samples) {
+	case 0:
+		/* If we rolled over, go back. */
+		still_det->num_samples--;
+		break;
+	case 1:
+		/* Set a new start time if new batch. */
+		still_det->window_start_time = sample_time;
+		break;
+	}
+
+	if (stillness_batch_complete(still_det, sample_time)) {
+		/*
+		 * Compute 1/num_samples and check for num_samples == 0 (should
+		 * never happen, but just in case)
+		 */
+		if (still_det->num_samples) {
+			inv = fp_div(1.0f, INT_TO_FP(still_det->num_samples));
+		} else {
+			still_det_reset(still_det);
+			return complete;
+		}
+		/* Calculating the VAR = sum(x^2)/n - sum(x)^2/n^2 */
+		var_x = compute_variance(
+			still_det->acc_xx, still_det->acc_x, inv);
+		var_y = compute_variance(
+			still_det->acc_yy, still_det->acc_y, inv);
+		var_z = compute_variance(
+			still_det->acc_zz, still_det->acc_z, inv);
+		/* Checking if sensor is still */
+		if (var_x < still_det->var_threshold &&
+		    var_y < still_det->var_threshold &&
+		    var_z < still_det->var_threshold) {
+			still_det->mean_x = fp_mul(still_det->acc_x, inv);
+			still_det->mean_y = fp_mul(still_det->acc_y, inv);
+			still_det->mean_z = fp_mul(still_det->acc_z, inv);
+			complete = true;
+		}
+		/* Reset and start over */
+		still_det_reset(still_det);
+	}
+	return complete;
+}
diff --git a/include/config.h b/include/config.h
index a8e70d0ccf..6d34b5e5aa 100644
--- a/include/config.h
+++ b/include/config.h
@@ -2763,6 +2763,9 @@
 /* Need for a math library */
 #undef CONFIG_MATH_UTIL
 
+/* Include sensor online calibration (requires CONFIG_FPU) */
+#undef CONFIG_ONLINE_CALIB
+
 /* Include code to do online compass calibration */
 #undef CONFIG_MAG_CALIBRATE
 
@@ -5153,4 +5156,8 @@
 #define CONFIG_CRC8
 #endif
 
+#if defined(CONFIG_ONLINE_CALIB) && !defined(CONFIG_FPU)
+#error "Online calibration requires CONFIG_FPU"
+#endif
+
 #endif  /* __CROS_EC_CONFIG_H */
diff --git a/include/stillness_detector.h b/include/stillness_detector.h
new file mode 100644
index 0000000000..65598d4d5c
--- /dev/null
+++ b/include/stillness_detector.h
@@ -0,0 +1,71 @@
+/* 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.
+ */
+
+#ifndef __CROS_EC_STILLNESS_DETECTOR_H
+#define __CROS_EC_STILLNESS_DETECTOR_H
+
+#include "common.h"
+#include "math_util.h"
+#include "stdbool.h"
+#include <stdint.h>
+
+struct still_det {
+	/** Variance threshold for the stillness confidence score. [units]^2 */
+	fp_t var_threshold;
+
+	/** The minimum window duration to consider a still sample. */
+	uint32_t min_batch_window;
+
+	/** The maximum window duration to consider a still sample. */
+	uint32_t max_batch_window;
+
+	/**
+	 * The minimum number of samples in a window to consider a still sample.
+	 */
+	uint16_t min_batch_size;
+
+	/** The timestamp of the first sample in the current batch. */
+	uint32_t window_start_time;
+
+	/** The number of samples in the current batch. */
+	uint16_t num_samples;
+
+	/** Accumulators used for calculating stillness. */
+	fp_t acc_x, acc_y, acc_z, acc_xx, acc_yy, acc_zz, mean_x, mean_y,
+		mean_z;
+};
+
+#define STILL_DET(VAR_THRES, MIN_BATCH_WIN, MAX_BATCH_WIN, MIN_BATCH_SIZE) \
+	((struct still_det){                                               \
+		.var_threshold = VAR_THRES,                                \
+		.min_batch_window = MIN_BATCH_WIN,                         \
+		.max_batch_window = MAX_BATCH_WIN,                         \
+		.min_batch_size = MIN_BATCH_SIZE,                          \
+		.window_start_time = 0,                                    \
+		.acc_x = 0.0f,                                             \
+		.acc_y = 0.0f,                                             \
+		.acc_z = 0.0f,                                             \
+		.acc_xx = 0.0f,                                            \
+		.acc_yy = 0.0f,                                            \
+		.acc_zz = 0.0f,                                            \
+		.mean_x = 0.0f,                                            \
+		.mean_y = 0.0f,                                            \
+		.mean_z = 0.0f,                                            \
+	})
+
+/**
+ * Update a stillness detector with a new sample.
+ *
+ * @param sample_time The timestamp of the sample to add.
+ * @param x The x component of the sample to add.
+ * @param y The y component of the sample to add.
+ * @param z The z component of the sample to add.
+ * @return True if the sample triggered a complete batch and mean_* are now
+ *         valid.
+ */
+bool still_det_update(struct still_det *still_det, uint32_t sample_time, fp_t x,
+		      fp_t y, fp_t z);
+
+#endif /* __CROS_EC_STILLNESS_DETECTOR_H */
diff --git a/include/test_util.h b/include/test_util.h
index 23034f918e..e10dcbe259 100644
--- a/include/test_util.h
+++ b/include/test_util.h
@@ -38,16 +38,22 @@
 		} \
 	} while (0)
 
+#if defined(__cplusplus) && !defined(__auto_type)
+#define __auto_type auto
+#endif
+
 #define TEST_OPERATOR(a, b, op, fmt) \
 	do { \
-		if (!((a) op (b))) { \
+		__auto_type _a = (a); \
+		__auto_type _b = (b); \
+		if (!(_a op _b)) { \
 			ccprintf("%d: ASSERSION failed: %s " #op " %s\n", \
 				 __LINE__, #a, #b); \
 			ccprintf("\t\tEVAL: " fmt " " #op " " fmt "\n", \
-				 (a), (b)); \
-			task_dump_trace(); \
-			return EC_ERROR_UNKNOWN; \
-		} \
+				 _a, _b); \
+			task_dump_trace();                                  \
+			return EC_ERROR_UNKNOWN;                            \
+		}                                                           \
 	} while (0)
 
 #define TEST_EQ(a, b, fmt) TEST_OPERATOR(a, b, ==, fmt)
diff --git a/test/build.mk b/test/build.mk
index b9e54574e4..4011fa455b 100644
--- a/test/build.mk
+++ b/test/build.mk
@@ -88,6 +88,7 @@ test-list-host += utils
 test-list-host += utils_str
 test-list-host += vboot
 test-list-host += x25519
+test-list-host += stillness_detector
 endif
 
 aes-y=aes.o
@@ -171,6 +172,7 @@ vboot-y=vboot.o
 float-y=fp.o
 fp-y=fp.o
 x25519-y=x25519.o
+stillness_detector-y=stillness_detector.o
 
 TPM2_ROOT := $(CROS_WORKON_SRCROOT)/src/third_party/tpm2
 $(out)/RO/common/new_nvmem.o: CFLAGS += -I$(TPM2_ROOT) -I chip/g
diff --git a/test/stillness_detector.c b/test/stillness_detector.c
new file mode 100644
index 0000000000..04e2e55d7e
--- /dev/null
+++ b/test/stillness_detector.c
@@ -0,0 +1,139 @@
+/* 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.
+ */
+
+#include "stillness_detector.h"
+#include "motion_sense.h"
+#include "test_util.h"
+#include "timer.h"
+#include <stdio.h>
+
+/*****************************************************************************/
+/*
+ * Need to define motion sensor globals just to compile.
+ * We include motion task to force the inclusion of math_util.c
+ */
+struct motion_sensor_t motion_sensors[] = {};
+const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
+
+static int test_build_still_det_struct(void)
+{
+	struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
+
+	TEST_NEAR(det.var_threshold, 0.00025f, 0.000001f, "%f");
+	TEST_EQ(det.min_batch_window, 800 * MSEC, "%u");
+	TEST_EQ(det.max_batch_window, 1200 * MSEC, "%u");
+	TEST_EQ(det.min_batch_size, 5, "%u");
+
+	return EC_SUCCESS;
+}
+
+static int test_not_still_short_window(void)
+{
+	struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
+	int i;
+
+	for (i = 0; i < 6; ++i)
+		TEST_ASSERT(!still_det_update(&det, i * 100 * MSEC,
+					      0.0f, 0.0f, 0.0f));
+
+	return EC_SUCCESS;
+}
+
+static int test_not_still_long_window(void)
+{
+	struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
+	int i;
+
+	for (i = 0; i < 5; ++i)
+		TEST_ASSERT(!still_det_update(&det, i * 300 * MSEC,
+					      0.0f, 0.0f, 0.0f));
+
+	return EC_SUCCESS;
+}
+
+static int test_not_still_not_enough_samples(void)
+{
+	struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
+	int i;
+
+	for (i = 0; i < 4; ++i)
+		TEST_ASSERT(!still_det_update(&det, i * 200 * MSEC,
+					      0.0f, 0.0f, 0.0f));
+
+	return EC_SUCCESS;
+}
+
+static int test_is_still_all_axes(void)
+{
+	struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
+	int i;
+
+	for (i = 0; i < 9; ++i) {
+		int result = still_det_update(&det, i * 100 * MSEC,
+					      i * 0.001f, i * 0.001f,
+					      i * 0.001f);
+
+		TEST_EQ(result, i == 8 ? 1 : 0, "%d");
+	}
+	TEST_NEAR(det.mean_x, 0.004f, 0.0001f, "%f");
+	TEST_NEAR(det.mean_y, 0.004f, 0.0001f, "%f");
+	TEST_NEAR(det.mean_z, 0.004f, 0.0001f, "%f");
+
+	return EC_SUCCESS;
+}
+
+static int test_not_still_one_axis(void)
+{
+	struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
+	int i;
+
+	for (i = 0; i < 9; ++i) {
+		TEST_ASSERT(!still_det_update(&det, i * 100 * MSEC,
+					      i * 0.001f, i * 0.001f,
+					      i * 0.01f));
+	}
+
+	return EC_SUCCESS;
+}
+
+static int test_resets(void)
+{
+	struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
+	int i;
+
+	for (i = 0; i < 9; ++i) {
+		TEST_ASSERT(!still_det_update(&det, i * 100 * MSEC,
+					      i * 0.001f, i * 0.001f,
+					      i * 0.01f));
+	}
+
+	for (i = 0; i < 9; ++i) {
+		int result = still_det_update(&det, i * 100 * MSEC,
+					      i * 0.001f, i * 0.001f,
+					      i * 0.001f);
+
+		TEST_EQ(result, i == 8 ? 1 : 0, "%d");
+	}
+	TEST_NEAR(det.mean_x, 0.004f, 0.0001f, "%f");
+	TEST_NEAR(det.mean_y, 0.004f, 0.0001f, "%f");
+	TEST_NEAR(det.mean_z, 0.004f, 0.0001f, "%f");
+
+	return EC_SUCCESS;
+}
+
+void run_test(void)
+{
+	test_reset();
+
+	RUN_TEST(test_build_still_det_struct);
+	RUN_TEST(test_not_still_short_window);
+	RUN_TEST(test_not_still_long_window);
+	RUN_TEST(test_not_still_not_enough_samples);
+	RUN_TEST(test_is_still_all_axes);
+	RUN_TEST(test_not_still_one_axis);
+	RUN_TEST(test_resets);
+
+	test_print_result();
+}
diff --git a/test/stillness_detector.tasklist b/test/stillness_detector.tasklist
new file mode 100644
index 0000000000..5ffe662d01
--- /dev/null
+++ b/test/stillness_detector.tasklist
@@ -0,0 +1,9 @@
+/* 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  /* No test task */
diff --git a/test/test_config.h b/test/test_config.h
index fc5731bac8..c757c233d3 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -65,6 +65,11 @@
 #define CONFIG_MATH_UTIL
 #endif
 
+#ifdef TEST_STILLNESS_DETECTOR
+#define CONFIG_FPU
+#define CONFIG_ONLINE_CALIB
+#endif
+
 #ifdef TEST_FLOAT
 #define CONFIG_FPU
 #define CONFIG_MAG_CALIBRATE