1 files changed, 110 insertions, 0 deletions
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;
+}