test: body_detection: add unittest for algorithm

Add test to check if the algorithm of body detection is correct.

There are three testcases:
OnBody: always in on-body state, should not change to off-body
Off to On: change state from off-body to on-body
On to Off: change state from on-body to off-body

BRANCH=None
BUG=b:123434029
TEST=make buildall

Signed-off-by: Ching-Kang Yen <chingkang@chromium.org>
Change-Id: Ib027fa3ed659f0d2efd7416888a9ee4ad519edd6
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2329113
Reviewed-by: Heng-ruey Hsu <henryhsu@chromium.org>

test/body_detection.c

@@ -0,0 +1,117 @@
+/* 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.
+ *
+ * Test body_detection algorithm
+ */
+
+#include "accelgyro.h"
+#include "body_detection.h"
+#include "body_detection_test_data.h"
+#include "common.h"
+#include "motion_common.h"
+#include "motion_sense.h"
+#include "test_util.h"
+#include "util.h"
+
+static struct motion_sensor_t *sensor = &motion_sensors[BASE];
+static const int window_size = 50; /* sensor data rate (Hz) */
+
+static int filler(const struct motion_sensor_t *s, const float v)
+{
+	int resolution = s->drv->get_resolution(s);
+	int range = s->drv->get_range(s);
+	int data_1g = BIT(resolution - 1) / range;
+
+	return (int)(v * data_1g / 9.8);
+}
+
+static void feed_body_detect_data(const struct body_detect_test_data *array,
+				  const int idx)
+{
+	sensor->xyz[X] = filler(sensor, array[idx].x);
+	sensor->xyz[Y] = filler(sensor, array[idx].y);
+	sensor->xyz[Z] = filler(sensor, array[idx].z);
+}
+
+static int get_trigger_time(const struct body_detect_test_data *data,
+			    const size_t size,
+			    const enum body_detect_states target_state)
+{
+	int i, action_index = -1, target_index = -1;
+
+	body_detect_reset();
+	/*
+	 * Clear on-body state when the window is initialized, so
+	 * that we do not need to wait for 15 second if the testcase
+	 * is in off-body initially.
+	 */
+	body_detect_change_state(BODY_DETECTION_OFF_BODY);
+	for (i = 0; i < size; ++i) {
+		enum body_detect_states motion_state;
+
+		if (data[i].action == 1 && action_index == -1) {
+			cprints(CC_ACCEL, "action start");
+			action_index = i;
+		}
+		feed_body_detect_data(data, i);
+		/* run the body detect */
+		body_detect();
+		/* skip if action not start yet */
+		if (action_index == -1)
+			continue;
+
+		motion_state = body_detect_get_state();
+		if (target_index == -1 && motion_state == target_state)
+			target_index = i;
+	}
+	if (target_index == -1)
+		return -1;
+	return target_index - action_index;
+}
+
+static int test_body_detect(void)
+{
+	int ret, trigger_time;
+
+	ret = sensor->drv->set_data_rate(sensor, window_size * 1000, 0);
+	TEST_ASSERT(ret == EC_SUCCESS);
+
+	body_detect_set_enable(true);
+	/* Onbody test */
+	cprints(CC_ACCEL, "start OnBody test");
+	trigger_time = get_trigger_time(kBodyDetectOnBodyTestData,
+					kBodyDetectOnBodyTestDataLength,
+					BODY_DETECTION_OFF_BODY);
+	/* It should not enter off-body state ever */
+	TEST_ASSERT(trigger_time == -1);
+
+	/* OffOn test */
+	cprints(CC_ACCEL, "start Off to On test");
+	trigger_time = get_trigger_time(kBodyDetectOffOnTestData,
+					kBodyDetectOffOnTestDataLength,
+					BODY_DETECTION_ON_BODY);
+	/* It should enter on-body state in 3 seconds */
+	TEST_ASSERT(trigger_time >= 0 && trigger_time < 3 * window_size);
+
+	/* OnOff test */
+	cprints(CC_ACCEL, "start On to Off test");
+	trigger_time = get_trigger_time(kBodyDetectOnOffTestData,
+					kBodyDetectOnOffTestDataLength,
+					BODY_DETECTION_OFF_BODY);
+	/* It should enter off-body state between 15 to 20 seconds */
+	TEST_ASSERT(15 * window_size <= trigger_time &&
+		    trigger_time < 20 * window_size);
+
+	return EC_SUCCESS;
+}
+
+
+void run_test(int argc, char **argv)
+{
+	test_reset();
+
+	RUN_TEST(test_body_detect);
+
+	test_print_result();
+}

test/body_detection.tasklist

@@ -0,0 +1,10 @@
+/* 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.
+ */
+
+/**
+ * See CONFIG_TASK_LIST in config.h for details.
+ */
+#define CONFIG_TEST_TASK_LIST  \
+  TASK_TEST(MOTIONSENSE, motion_sense_task, NULL, TASK_STACK_SIZE)

test/body_detection_test_data.h

@@ -0,0 +1,25 @@
+/* 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_TEST_DATA_H
+#define __CROS_EC_BODY_DETECTION_TEST_DATA_H
+
+#include "body_detection.h"
+#include "motion_sense.h"
+
+struct body_detect_test_data {
+	float x, y, z;
+	int action;
+};
+
+extern const struct body_detect_test_data kBodyDetectOnBodyTestData[];
+extern const size_t kBodyDetectOnBodyTestDataLength;
+
+extern const struct body_detect_test_data kBodyDetectOffOnTestData[];
+extern const size_t kBodyDetectOffOnTestDataLength;
+
+extern const struct body_detect_test_data kBodyDetectOnOffTestData[];
+extern const size_t kBodyDetectOnOffTestDataLength;
+#endif

test/build.mk

@@ -17,6 +17,7 @@ test-list-host += base32
 test-list-host += battery_get_params_smart
 test-list-host += bklight_lid
 test-list-host += bklight_passthru
+test-list-host += body_detection
 test-list-host += button
 test-list-host += cbi
 test-list-host += cec
@@ -124,6 +125,7 @@ base32-y=base32.o
 battery_get_params_smart-y=battery_get_params_smart.o
 bklight_lid-y=bklight_lid.o
 bklight_passthru-y=bklight_passthru.o
+body_detection-y=body_detection.o body_detection_data_literals.o motion_common.o
 button-y=button.o
 cbi-y=cbi.o
 cec-y=cec.o

test/motion_common.c

@@ -6,6 +6,7 @@
  */
 
 #include "accelgyro.h"
+#include "driver/accelgyro_bmi_common.h"
 #include "host_command.h"
 #include "motion_common.h"
 #include "motion_sense.h"
@@ -32,6 +33,10 @@ static int accel_get_range(const struct motion_sensor_t *s)
 
 static int accel_get_resolution(const struct motion_sensor_t *s)
 {
+#ifdef TEST_BODY_DETECTION
+	/* Assume we are using BMI160 */
+	return BMI_RESOLUTION;
+#endif
 	return 0;
 }
 
@@ -50,6 +55,18 @@ static int accel_get_data_rate(const struct motion_sensor_t *s)
 	return test_data_rate[s - motion_sensors];
 }
 
+#ifdef TEST_BODY_DETECTION
+static int accel_get_rms_noise(const struct motion_sensor_t *s)
+{
+	/* Assume we are using BMI160 */
+	fp_t rate = INT_TO_FP(accel_get_data_rate(s) / 1000);
+	fp_t noise_100hz = INT_TO_FP(BMI160_ACCEL_RMS_NOISE_100HZ);
+	fp_t sqrt_rate_ratio = fp_sqrtf(fp_div(rate,
+					       INT_TO_FP(BMI_ACCEL_100HZ)));
+	return FP_TO_INT(fp_mul(noise_100hz, sqrt_rate_ratio));
+}
+#endif
+
 const struct accelgyro_drv test_motion_sense = {
 	.init = accel_init,
 	.read = accel_read,
@@ -57,6 +74,9 @@ const struct accelgyro_drv test_motion_sense = {
 	.get_resolution = accel_get_resolution,
 	.set_data_rate = accel_set_data_rate,
 	.get_data_rate = accel_get_data_rate,
+#ifdef CONFIG_BODY_DETECTION
+	.get_rms_noise = accel_get_rms_noise,
+#endif
 };
 
 struct motion_sensor_t motion_sensors[] = {

test/test_config.h

@@ -150,11 +150,12 @@
 #define CONFIG_TEMP_CACHE_STALE_THRES (1 * SECOND)
 #endif /* CONFIG_ONLINE_CALIB && !CONFIG_TEMP_CACHE_STALE_THRES */
 
-#if defined(TEST_MOTION_LID) || \
+#if defined(CONFIG_ONLINE_CALIB) || \
+	defined(TEST_BODY_DETECTION) || \
 	defined(TEST_MOTION_ANGLE) || \
 	defined(TEST_MOTION_ANGLE_TABLET) || \
-	defined(TEST_MOTION_SENSE_FIFO) || \
-	defined(CONFIG_ONLINE_CALIB)
+	defined(TEST_MOTION_LID) || \
+	defined(TEST_MOTION_SENSE_FIFO)
 enum sensor_id {
 	BASE,
 	LID,
@@ -182,6 +183,11 @@ enum sensor_id {
 	 (1 << CONFIG_LID_ANGLE_SENSOR_LID))
 #endif
 
+#if defined(TEST_BODY_DETECTION)
+#define CONFIG_BODY_DETECTION
+#define CONFIG_BODY_DETECTION_SENSOR BASE
+#endif
+
 #ifdef TEST_RMA_AUTH
 
 /* Test server public and private keys */