common: implement body_detection state machine

Implement body_detection state machine. Detect whether the user is
close to the devices or not.

Use the variance of acceleration from accelerometer to compute the
motion confidence. If motion confidence is high enough, set the motion
state to ON_BODY. If low enough, set it to OFF_BODY.

Also, make gesture_cal() be called only when the gesture sensor data is
updated.

BRANCH=None
BUG=b:123434029
TEST=make buildall, flash the ec, watch the ec console log

Change-Id: If886b7f9fc505ef4db4d7abb08194b8f866121bb
Signed-off-by: Ching-Kang Yen <chingkang@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2230938
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>

7 files changed, 342 insertions, 12 deletions

diff --git a/common/body_detection.c b/common/body_detection.c
new file mode 100644
index 0000000000..020314e71e
--- /dev/null
+++ b/common/body_detection.c
@@ -0,0 +1,226 @@
+/* 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 "accelgyro.h"
+#include "body_detection.h"
+#include "console.h"
+#include "hwtimer.h"
+#include "lid_switch.h"
+#include "math_util.h"
+#include "motion_sense_fifo.h"
+#include "timer.h"
+
+/* Console output macros */
+#define CPUTS(outstr) cputs(CC_ACCEL, outstr)
+#define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args)
+#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
+
+static struct motion_sensor_t *body_sensor =
+	&motion_sensors[CONFIG_BODY_DETECTION_SENSOR];
+
+static int window_size = CONFIG_BODY_DETECTION_MAX_WINDOW_SIZE;
+static uint64_t var_threshold_scaled, confidence_delta_scaled;
+static int stationary_timeframe;
+
+static int history_idx;
+static enum body_detect_states motion_state = BODY_DETECTION_OFF_BODY;
+
+static bool history_initialized;
+static bool body_detect_enable;
+
+static struct body_detect_motion_data
+{
+	int history[CONFIG_BODY_DETECTION_MAX_WINDOW_SIZE]; /* acceleration */
+	int sum;              /* sum(history) */
+	uint64_t n2_variance; /* n^2 * var(history) */
+} data[2]; /* motion data for X-axis and Y-axis */
+
+/*
+ * This function will update new variance and new sum according to incoming
+ * value, previous value, previous sum and previous variance.
+ * In order to prevent inaccuracy, we use integer to calculate instead of float
+ *
+ * n: window size
+ * x: data in the old window
+ * x': data in the new window
+ * x_0: oldest value in the window, will be replaced by x_n
+ * x_n: new coming value
+ *
+ * n^2 * var(x') = n^2 * var(x) + (sum(x') - sum(x))^2 +
+ *                 (n * x_n - sum(x'))^2 / n - (n * x_0 - sum(x'))^2 / n
+ */
+static void update_motion_data(struct body_detect_motion_data *x, int x_n)
+{
+	const int n = window_size;
+	const int x_0 = x->history[history_idx];
+	const int new_sum = x->sum + (x_n - x->history[history_idx]);
+
+	x->n2_variance = x->n2_variance + POW2((int64_t)new_sum - x->sum) +
+			 (POW2((int64_t)x_n * n - new_sum) -
+			  POW2((int64_t)x_0 * n - new_sum)) / n;
+	x->sum = new_sum;
+	x->history[history_idx] = x_n;
+}
+
+/* Update motion data of X, Y with new sensor data. */
+static void update_motion_variance(void)
+{
+	update_motion_data(&data[X], body_sensor->xyz[X]);
+	update_motion_data(&data[Y], body_sensor->xyz[Y]);
+	history_idx = (history_idx + 1 >= window_size) ? 0 : history_idx + 1;
+}
+
+/* return Var(X) + Var(Y) */
+static uint64_t get_motion_variance(void)
+{
+	return (data[X].n2_variance + data[Y].n2_variance)
+		/ window_size / window_size;
+}
+
+static int calculate_motion_confidence(uint64_t var)
+{
+	if (var < var_threshold_scaled - confidence_delta_scaled)
+		return 0;
+	if (var > var_threshold_scaled + confidence_delta_scaled)
+		return 100;
+	return 100 * (var - var_threshold_scaled + confidence_delta_scaled) /
+		(2 * confidence_delta_scaled);
+}
+
+/* Change the motion state and commit the change to AP. */
+void body_detect_change_state(enum body_detect_states state)
+{
+#ifdef CONFIG_GESTURE_HOST_DETECTION
+	struct ec_response_motion_sensor_data vector = {
+		.flags = MOTIONSENSE_SENSOR_FLAG_WAKEUP,
+		.activity = MOTIONSENSE_ACTIVITY_BODY_DETECTION,
+		.state = state,
+		.sensor_num = MOTION_SENSE_ACTIVITY_SENSOR_ID,
+	};
+	motion_sense_fifo_stage_data(&vector, NULL, 0,
+			__hw_clock_source_read());
+	motion_sense_fifo_commit_data();
+#endif
+	/* change the motion state */
+	motion_state = state;
+	if (state == BODY_DETECTION_ON_BODY) {
+		/* reset time counting of stationary */
+		stationary_timeframe = 0;
+	}
+	/* state changing log */
+	CPRINTS("body_detect changed state to: %s body",
+		motion_state ? "on" : "off");
+}
+
+enum body_detect_states body_detect_get_state(void)
+{
+	return motion_state;
+}
+
+/* Determine window size for 1 second by sensor data rate. */
+static void determine_window_size(int odr)
+{
+	window_size = odr / 1000;
+	/* Normally, window_size should not exceed MAX_WINDOW_SIZE. */
+	if (window_size > CONFIG_BODY_DETECTION_MAX_WINDOW_SIZE) {
+		/* This will cause window size not enough for 1 second */
+		CPRINTS("ODR exceeds CONFIG_BODY_DETECTION_MAX_WINDOW_SIZE");
+		window_size = CONFIG_BODY_DETECTION_MAX_WINDOW_SIZE;
+	}
+}
+
+/* Determine variance threshold scale by range and resolution. */
+static void determine_threshold_scale(int range, int resolution, int rms_noise)
+{
+	/*
+	 * range:              g
+	 * resolution:         bits
+	 * data_1g:            LSB/g
+	 * data_1g / 9800:     LSB/(mm/s^2)
+	 * (data_1g / 9800)^2: (LSB^2)/(mm^2/s^4), which number of
+	 *                     var(sensor data) will represents 1 (mm^2/s^4)
+	 * rms_noise:          ug
+	 * var_noise:          mm^2/s^4
+	 */
+	const int data_1g = BIT(resolution - 1) / range;
+	const int multiplier = POW2(data_1g);
+	const int divisor = POW2(9800);
+	const int var_noise = POW2((int64_t)rms_noise) * POW2(98) / POW2(10000);
+
+	var_threshold_scaled = (uint64_t)
+		(CONFIG_BODY_DETECTION_VAR_THRESHOLD + var_noise) *
+		multiplier / divisor;
+	confidence_delta_scaled = (uint64_t)
+		CONFIG_BODY_DETECTION_CONFIDENCE_DELTA *
+		multiplier / divisor;
+}
+
+void body_detect_reset(void)
+{
+	int odr = body_sensor->drv->get_data_rate(body_sensor);
+	int range = body_sensor->drv->get_range(body_sensor);
+	int resolution = body_sensor->drv->get_resolution(body_sensor);
+	int rms_noise = body_sensor->drv->get_rms_noise(body_sensor);
+
+	body_detect_change_state(BODY_DETECTION_ON_BODY);
+	/*
+	 * The sensor is suspended since its ODR is 0,
+	 * there is no need to reset until sensor is up again
+	 */
+	if (odr == 0)
+		return;
+	determine_window_size(odr);
+	determine_threshold_scale(range, resolution, rms_noise);
+	/* initialize motion data and state */
+	memset(data, 0, sizeof(data));
+	history_idx = 0;
+	history_initialized = 0;
+}
+
+void body_detect(void)
+{
+	uint64_t motion_var;
+	int motion_confidence;
+
+	if (!body_detect_enable)
+		return;
+
+	update_motion_variance();
+	if (!history_initialized) {
+		if (history_idx == window_size - 1)
+			history_initialized = 1;
+		return;
+	}
+
+	motion_var = get_motion_variance();
+	motion_confidence = calculate_motion_confidence(motion_var);
+	switch (motion_state) {
+	case BODY_DETECTION_OFF_BODY:
+		if (motion_confidence > CONFIG_BODY_DETECTION_ON_BODY_CON)
+			body_detect_change_state(BODY_DETECTION_ON_BODY);
+		break;
+	case BODY_DETECTION_ON_BODY:
+		stationary_timeframe += 1;
+		/* confidence exceeds the limit, reset time counting */
+		if (motion_confidence >= CONFIG_BODY_DETECTION_OFF_BODY_CON)
+			stationary_timeframe = 0;
+		/* if no motion for enough time, change state to off_body */
+		if (stationary_timeframe >=
+		    CONFIG_BODY_DETECTION_STATIONARY_DURATION * window_size)
+			body_detect_change_state(BODY_DETECTION_OFF_BODY);
+		break;
+	}
+}
+
+void body_detect_set_enable(int enable)
+{
+	body_detect_enable = enable;
+	body_detect_change_state(BODY_DETECTION_ON_BODY);
+}
+
+int body_detect_get_enable(void)
+{
+	return body_detect_enable;
+}
diff --git a/common/build.mk b/common/build.mk
index f54f9ad5a8..4ef78d57f5 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -44,6 +44,7 @@ common-$(CONFIG_BATTERY)+=battery.o
 common-$(CONFIG_BATTERY_FUEL_GAUGE)+=battery_fuel_gauge.o
 common-$(CONFIG_BLUETOOTH_LE)+=bluetooth_le.o
 common-$(CONFIG_BLUETOOTH_LE_STACK)+=btle_hci_controller.o btle_ll.o
+common-$(CONFIG_BODY_DETECTION)+=body_detection.o
 common-$(CONFIG_CAPSENSE)+=capsense.o
 common-$(CONFIG_CEC)+=cec.o
 common-$(CONFIG_CROS_BOARD_INFO)+=cbi.o
diff --git a/common/math_util.c b/common/math_util.c
index 450dfbe5e7..dc52729c65 100644
--- a/common/math_util.c
+++ b/common/math_util.c
@@ -10,9 +10,6 @@
 #include "math_util.h"
 #include "util.h"
 
-/* Some useful math functions.  Use with integers only! */
-#define SQ(x) ((x) * (x))
-
 /* For cosine lookup table, define the increment and the size of the table. */
 #define COSINE_LUT_INCR_DEG	5
 #define COSINE_LUT_SIZE		((180 / COSINE_LUT_INCR_DEG) + 1)
diff --git a/common/motion_sense.c b/common/motion_sense.c
index 85cce294c9..c1bf6b8930 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -7,6 +7,7 @@
 
 #include "accelgyro.h"
 #include "atomic.h"
+#include "body_detection.h"
 #include "chipset.h"
 #include "common.h"
 #include "console.h"
@@ -187,6 +188,10 @@ int motion_sense_set_data_rate(struct motion_sensor_t *sensor)
 	sensor->next_collection = ts.le.lo + sensor->collection_rate;
 	sensor->oversampling = 0;
 	mutex_unlock(&g_sensor_mutex);
+#ifdef CONFIG_BODY_DETECTION
+	if (sensor - motion_sensors == CONFIG_BODY_DETECTION_SENSOR)
+		body_detect_reset();
+#endif
 	return 0;
 }
 
@@ -444,6 +449,10 @@ static void motion_sense_shutdown(void)
 				MOTIONSENSE_ACTIVITY_DOUBLE_TAP, 1, NULL);
 	}
 #endif
+#ifdef CONFIG_BODY_DETECTION
+	/* disable the body detection since motion sensor is down */
+	body_detect_set_enable(false);
+#endif
 }
 DECLARE_HOOK(HOOK_CHIPSET_SHUTDOWN, motion_sense_shutdown,
 	     MOTION_SENSE_HOOK_PRIO);
@@ -459,6 +468,10 @@ static void motion_sense_suspend(void)
 
 	sensor_active = SENSOR_ACTIVE_S3;
 
+#ifdef CONFIG_BODY_DETECTION
+	/* disable the body detection since motion sensor is suspended */
+	body_detect_set_enable(false);
+#endif
 	/*
 	 * During shutdown sequence sensor rails can be powered down
 	 * asynchronously to the EC hence EC cannot interlock the sensor
@@ -659,9 +672,11 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 {
 	int ret = EC_SUCCESS;
 	int is_odr_pending = 0;
+	int has_data_read = 0;
+	int sensor_num = sensor - motion_sensors;
 
 	if (*event & TASK_EVENT_MOTION_ODR_CHANGE) {
-		const int sensor_bit = 1 << (sensor - motion_sensors);
+		const int sensor_bit = 1 << sensor_num;
 		int odr_pending = atomic_read_clear(&odr_event_required);
 
 		is_odr_pending = odr_pending & sensor_bit;
@@ -673,8 +688,10 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 	if ((*event & TASK_EVENT_MOTION_INTERRUPT_MASK || is_odr_pending) &&
 	    (sensor->drv->irq_handler != NULL)) {
 		ret = sensor->drv->irq_handler(sensor, event);
+		if (ret == EC_SUCCESS)
+			has_data_read = 1;
 	}
-#endif
+#endif /* CONFIG_ACCEL_INTERRUPTS */
 	if (motion_sensor_in_forced_mode(sensor)) {
 		if (motion_sensor_time_to_read(ts, sensor)) {
 			ret = motion_sense_read(sensor);
@@ -683,8 +700,10 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 			ret = EC_ERROR_BUSY;
 		}
 
-		if (ret == EC_SUCCESS)
+		if (ret == EC_SUCCESS) {
 			motion_sense_push_raw_xyz(sensor);
+			has_data_read = 1;
+		}
 	}
 	if (IS_ENABLED(CONFIG_ACCEL_FIFO) &&
 	    *event & TASK_EVENT_MOTION_FLUSH_PENDING) {
@@ -704,6 +723,17 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 			motion_sense_fifo_insert_async_event(
 				sensor, ASYNC_EVENT_ODR);
 	}
+	if (has_data_read) {
+#ifdef CONFIG_GESTURE_SW_DETECTION
+		/* Run gesture recognition engine */
+		if (sensor_num == CONFIG_GESTURE_SENSOR_DOUBLE_TAP)
+			gesture_calc(event);
+#endif
+#ifdef CONFIG_BODY_DETECTION
+		if (sensor_num == CONFIG_BODY_DETECTION_SENSOR)
+			body_detect();
+#endif
+	}
 	return ret;
 }
 
@@ -734,10 +764,6 @@ static void check_and_queue_gestures(uint32_t *event)
 	const struct motion_sensor_t *sensor;
 #endif
 
-#ifdef CONFIG_GESTURE_SW_DETECTION
-	/* Run gesture recognition engine */
-	gesture_calc(event);
-#endif
 #ifdef CONFIG_GESTURE_SENSOR_DOUBLE_TAP
 	if (*event & TASK_EVENT_MOTION_ACTIVITY_INTERRUPT(
 				MOTIONSENSE_ACTIVITY_DOUBLE_TAP)) {
@@ -1168,8 +1194,8 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args)
 				return EC_RES_INVALID_COMMAND;
 
 			if (sensor->drv->set_range(sensor,
-						in->sensor_range.data,
-						in->sensor_range.roundup)
+						   in->sensor_range.data,
+						   in->sensor_range.roundup)
 					!= EC_SUCCESS) {
 				return EC_RES_INVALID_PARAM;
 			}
@@ -1350,6 +1376,15 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args)
 				out->list_activities.disabled |= disabled;
 			}
 		}
+#ifdef CONFIG_BODY_DETECTION
+		if (body_detect_get_enable()) {
+			out->list_activities.enabled |=
+				BIT(MOTIONSENSE_ACTIVITY_BODY_DETECTION);
+		} else {
+			out->list_activities.disabled |=
+				BIT(MOTIONSENSE_ACTIVITY_BODY_DETECTION);
+		}
+#endif
 		if (ret != EC_RES_SUCCESS)
 			return ret;
 		args->response_size = sizeof(out->list_activities);
@@ -1372,11 +1407,33 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args)
 						in->set_activity.enable,
 						&in->set_activity);
 		}
+#ifdef CONFIG_BODY_DETECTION
+		if (in->set_activity.activity ==
+		    MOTIONSENSE_ACTIVITY_BODY_DETECTION)
+			body_detect_set_enable(in->set_activity.enable);
+#endif
 		if (ret != EC_RES_SUCCESS)
 			return ret;
 		args->response_size = 0;
 		break;
 	}
+	case MOTIONSENSE_CMD_GET_ACTIVITY: {
+		switch (in->get_activity.activity) {
+#ifdef CONFIG_BODY_DETECTION
+		case MOTIONSENSE_ACTIVITY_BODY_DETECTION:
+			out->get_activity.state = (uint8_t)
+					body_detect_get_state();
+			ret = EC_RES_SUCCESS;
+			break;
+#endif
+		default:
+			ret = EC_RES_INVALID_PARAM;
+		}
+		if (ret != EC_RES_SUCCESS)
+			return ret;
+		args->response_size = sizeof(out->get_activity);
+		break;
+	}
 #endif /* defined(CONFIG_GESTURE_HOST_DETECTION) */
 
 #ifdef CONFIG_ACCEL_SPOOF_MODE
diff --git a/include/body_detection.h b/include/body_detection.h
new file mode 100644
index 0000000000..d3c1865e36
--- /dev/null
+++ b/include/body_detection.h
@@ -0,0 +1,33 @@
+/* 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.
+ */
+
+#ifndef __CROS_EC_BODY_DETECTION_H
+#define __CROS_EC_BODY_DETECTION_H
+
+#include <stdint.h>
+
+enum body_detect_states {
+	BODY_DETECTION_OFF_BODY,
+	BODY_DETECTION_ON_BODY
+};
+
+/* get/set the state of body detection */
+enum body_detect_states body_detect_get_state(void);
+void body_detect_change_state(enum body_detect_states state);
+
+/* Reset the data. This should be called when ODR is changed*/
+void body_detect_reset(void);
+
+/* Body detect main function. This should be called when new sensor data come */
+void body_detect(void);
+
+/* enable/disable body detection */
+void body_detect_set_enable(int enable);
+
+/* get enable state of body detection */
+int body_detect_get_enable(void);
+
+
+#endif /* __CROS_EC_BODY_DETECTION_H */
diff --git a/include/math_util.h b/include/math_util.h
index 6b60d4a1d6..266bde6743 100644
--- a/include/math_util.h
+++ b/include/math_util.h
@@ -48,6 +48,9 @@ typedef int64_t fp_inter_t;
 
 #endif
 
+/* Some useful math functions.  Use with integers only! */
+#define POW2(x) ((x) * (x))
+
 /*
  * Fixed-point addition and subtraction can be done directly, because they
  * work identically.
diff --git a/util/ectool.c b/util/ectool.c
index be8ceae1db..963a04f2d6 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -5541,6 +5541,19 @@ static int cmd_motionsense(int argc, char **argv)
 			return rv;
 		return 0;
 	}
+	if (argc == 4 && !strcasecmp(argv[1], "get_activity")) {
+		param.cmd = MOTIONSENSE_CMD_GET_ACTIVITY;
+		param.get_activity.sensor_num = strtol(argv[2], &e, 0);
+		param.get_activity.activity = strtol(argv[3], &e, 0);
+
+		rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 2,
+				&param, ms_command_sizes[param.cmd].outsize,
+				resp, ms_command_sizes[param.cmd].insize);
+		if (rv < 0)
+			return rv;
+		printf("State: %d\n", resp->get_activity.state);
+		return 0;
+	}
 
 	if (argc == 2 && !strcasecmp(argv[1], "lid_angle")) {
 		param.cmd = MOTIONSENSE_CMD_LID_ANGLE;