motion sense: Calculate loop time based on sensor needs

Currently the motion sense loop bases its sleep time based on the
fastest active sensor. This method has several flaws:

1. It does not take into account any task switching overhead
2. With a mix of interrupt driven and forced sensors the sleep time gets
   recalculated every time there is an interrupt causing the loop to
   oversleep
3. If multiple sensors do not have rates that are in sync the timing of
   the slower sensor will be off. For example if there was a sensor running
   at 50 Hz and one running at 20 Hz the slower sensor would end up being
   sampled at about 16 Hz instead of 20 Hz

This change calculates an ideal read time for every forced mode sensor
and calculates the sleep time based on the nearest read time. Every time
a sensor is read the next read time is calculated based on the ideal read
time not the actual read time so that reading does not drift because of
system load or other overhead.

BUG=b:129159505
TEST=Ran sensor CTS tests on arcada, without this change the
     magnetometer was failing 50 Hz tests at about 38 Hz with 30% jitter
     with this change in place 50 Hz was spot on with about 10% jitter
BRANCH=none

Change-Id: Ia4fccb083713b490518d45e7398eb3be3b957eae
Signed-off-by: Mathew King <mathewk@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1574786
Reviewed-by: Jett Rink <jettrink@chromium.org>
(cherry picked from commit 0c71c4748699f5f2cb1423ffc07d4c852d04b3fc)
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3314256
Reviewed-by: Michael5 Chen <michael5_chen1@pegatron.corp-partner.google.com>
Reviewed-by: Henry Sun <henrysun@google.com>
Tested-by: Michael5 Chen <michael5_chen1@pegatron.corp-partner.google.com>
Commit-Queue: Henry Sun <henrysun@google.com>

4 files changed, 291 insertions, 2 deletions

diff --git a/test/motion_angle.c b/test/motion_angle.c
new file mode 100644
index 0000000000..efa6522c8d
--- /dev/null
+++ b/test/motion_angle.c
@@ -0,0 +1,104 @@
+<<<<<<< HEAD   (67a275 TCPMv1, TCPMv2: add pd event for receiving hard reset)
+=======
+/* 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.
+ *
+ * Test motion sense code: Check lid angle calculation and tablet mode
+ * transition.
+ */
+
+#include <math.h>
+#include <stdio.h>
+
+#include "accelgyro.h"
+#include "common.h"
+#include "gpio.h"
+#include "hooks.h"
+#include "motion_common.h"
+#include "motion_lid.h"
+#include "motion_sense.h"
+#include "tablet_mode.h"
+#include "test_util.h"
+#include "util.h"
+
+/*****************************************************************************/
+/* Test utilities */
+
+/* Array units is in m/s^2 - old matrix format. */
+int filler(const struct motion_sensor_t *s, const float v)
+{
+	return (v * MOTION_SCALING_FACTOR) / s->drv->get_range(s);
+}
+
+static int test_lid_angle_less180(void)
+{
+	int index = 0, lid_angle;
+	struct motion_sensor_t *lid = &motion_sensors[
+		CONFIG_LID_ANGLE_SENSOR_LID];
+	struct motion_sensor_t *base = &motion_sensors[
+		CONFIG_LID_ANGLE_SENSOR_BASE];
+
+	/* We don't have TASK_CHIP so simulate init ourselves */
+	hook_notify(HOOK_CHIPSET_SHUTDOWN);
+	TEST_ASSERT(sensor_active == SENSOR_ACTIVE_S5);
+	TEST_ASSERT(lid->drv->get_data_rate(lid) == 0);
+
+	/* Go to S0 state */
+	hook_notify(HOOK_CHIPSET_SUSPEND);
+	hook_notify(HOOK_CHIPSET_RESUME);
+	msleep(1000);
+	TEST_ASSERT(sensor_active == SENSOR_ACTIVE_S0);
+	TEST_ASSERT(lid->drv->get_data_rate(lid) == TEST_LID_FREQUENCY);
+
+	/* Open lid, testing close to 180 degree. */
+	gpio_set_level(GPIO_LID_OPEN, 1);
+	msleep(1000);
+
+	cprints(CC_ACCEL, "start loop");
+	/* Check we will never enter tablet mode. */
+	while (index < kAccelerometerLaptopModeTestDataLength) {
+		feed_accel_data(kAccelerometerLaptopModeTestData,
+				&index, filler);
+		wait_for_valid_sample();
+		lid_angle = motion_lid_get_angle();
+		cprints(CC_ACCEL, "%d : LID(%d, %d, %d)/BASE(%d, %d, %d): %d",
+				index / TEST_LID_SAMPLE_SIZE,
+				lid->xyz[X], lid->xyz[Y], lid->xyz[Z],
+				base->xyz[X], base->xyz[Y], base->xyz[Z],
+				lid_angle);
+		/* We need few sample to debounce and enter laptop mode. */
+		TEST_ASSERT(index < TEST_LID_SAMPLE_SIZE *
+				(TABLET_MODE_DEBOUNCE_COUNT + 2) ||
+			    !tablet_get_mode());
+	}
+
+	/* Check we will never exit tablet mode. */
+	index = 0;
+	while (index < kAccelerometerFullyOpenTestDataLength) {
+		feed_accel_data(kAccelerometerFullyOpenTestData,
+				&index, filler);
+		wait_for_valid_sample();
+		lid_angle = motion_lid_get_angle();
+		cprints(CC_ACCEL, "%d : LID(%d, %d, %d)/BASE(%d, %d, %d): %d",
+				index / TEST_LID_SAMPLE_SIZE,
+				lid->xyz[X], lid->xyz[Y], lid->xyz[Z],
+				base->xyz[X], base->xyz[Y], base->xyz[Z],
+				lid_angle);
+		TEST_ASSERT(index < TEST_LID_SAMPLE_SIZE *
+				(TABLET_MODE_DEBOUNCE_COUNT + 2) ||
+			    tablet_get_mode());
+	}
+	return EC_SUCCESS;
+}
+
+
+void run_test(void)
+{
+	test_reset();
+
+	RUN_TEST(test_lid_angle_less180);
+
+	test_print_result();
+}
+>>>>>>> CHANGE (0c71c4 motion sense: Calculate loop time based on sensor needs)
diff --git a/test/motion_angle_tablet.c b/test/motion_angle_tablet.c
new file mode 100644
index 0000000000..a5d3813d07
--- /dev/null
+++ b/test/motion_angle_tablet.c
@@ -0,0 +1,112 @@
+<<<<<<< HEAD   (67a275 TCPMv1, TCPMv2: add pd event for receiving hard reset)
+=======
+/* Copyright 2018 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 motion sense code, when in tablet mode.
+ */
+
+#include <math.h>
+#include <stdio.h>
+
+#include "accelgyro.h"
+#include "common.h"
+#include "gpio.h"
+#include "hooks.h"
+#include "motion_common.h"
+#include "motion_lid.h"
+#include "motion_sense.h"
+#include "tablet_mode.h"
+#include "test_util.h"
+#include "util.h"
+
+
+/*****************************************************************************/
+/* Test utilities */
+
+/* convert array value from g to m.s^2. */
+int filler(const struct motion_sensor_t *s, const float v)
+{
+	return FP_TO_INT( fp_div(
+		FLOAT_TO_FP(v) * MOTION_SCALING_FACTOR,
+		fp_mul(INT_TO_FP(s->drv->get_range(s)), MOTION_ONE_G)));
+}
+
+static int test_lid_angle_less180(void)
+{
+	int index = 0, lid_angle;
+	struct motion_sensor_t *lid = &motion_sensors[
+		CONFIG_LID_ANGLE_SENSOR_LID];
+	struct motion_sensor_t *base = &motion_sensors[
+		CONFIG_LID_ANGLE_SENSOR_BASE];
+
+	/* We don't have TASK_CHIP so simulate init ourselves */
+	hook_notify(HOOK_CHIPSET_SHUTDOWN);
+	TEST_ASSERT(sensor_active == SENSOR_ACTIVE_S5);
+	TEST_ASSERT(lid->drv->get_data_rate(lid) == 0);
+
+	/* Go to S0 state */
+	hook_notify(HOOK_CHIPSET_SUSPEND);
+	hook_notify(HOOK_CHIPSET_RESUME);
+	msleep(1000);
+	TEST_ASSERT(sensor_active == SENSOR_ACTIVE_S0);
+	TEST_ASSERT(lid->drv->get_data_rate(lid) == TEST_LID_FREQUENCY);
+
+	/* Open lid, testing close to 180 degree. */
+	gpio_set_level(GPIO_LID_OPEN, 1);
+	msleep(1000);
+
+	cprints(CC_ACCEL, "start loop");
+	/* Force clamshell mode, to be sure we go in tablet mode ASAP. */
+	tablet_set_mode(0);
+
+	/* Check we stay in tablet mode, even when hinge is vertical. */
+	while (index < kAccelerometerVerticalHingeTestDataLength) {
+		feed_accel_data(kAccelerometerVerticalHingeTestData,
+				&index, filler);
+		wait_for_valid_sample();
+		lid_angle = motion_lid_get_angle();
+		cprints(CC_ACCEL, "%d : LID(%d, %d, %d)/BASE(%d, %d, %d): %d",
+				index / TEST_LID_SAMPLE_SIZE,
+				lid->xyz[X], lid->xyz[Y], lid->xyz[Z],
+				base->xyz[X], base->xyz[Y], base->xyz[Z],
+				lid_angle);
+		/* We need few sample to debounce and enter laptop mode. */
+		TEST_ASSERT(index < 2 * TEST_LID_SAMPLE_SIZE * \
+				(TABLET_MODE_DEBOUNCE_COUNT + 2) ||
+			    tablet_get_mode());
+	}
+	/*
+	 * Check we stay in tablet mode, even when hinge is vertical and
+	 * shaked.
+	 */
+	tablet_set_mode(0);
+	while (index < kAccelerometerVerticalHingeUnstableTestDataLength) {
+		feed_accel_data(kAccelerometerVerticalHingeUnstableTestData,
+				&index, filler);
+		wait_for_valid_sample();
+		lid_angle = motion_lid_get_angle();
+		cprints(CC_ACCEL, "%d : LID(%d, %d, %d)/BASE(%d, %d, %d): %d",
+				index / TEST_LID_SAMPLE_SIZE,
+				lid->xyz[X], lid->xyz[Y], lid->xyz[Z],
+				base->xyz[X], base->xyz[Y], base->xyz[Z],
+				lid_angle);
+		/* We need few sample to debounce and enter laptop mode. */
+		TEST_ASSERT(index < TEST_LID_SAMPLE_SIZE *
+				(TABLET_MODE_DEBOUNCE_COUNT + 2) ||
+			    tablet_get_mode());
+	}
+	return EC_SUCCESS;
+}
+
+
+void run_test(void)
+{
+	test_reset();
+
+	RUN_TEST(test_lid_angle_less180);
+
+	test_print_result();
+}
+>>>>>>> CHANGE (0c71c4 motion sense: Calculate loop time based on sensor needs)
diff --git a/test/motion_common.h b/test/motion_common.h
new file mode 100644
index 0000000000..9e3dc4e47b
--- /dev/null
+++ b/test/motion_common.h
@@ -0,0 +1,71 @@
+<<<<<<< HEAD   (67a275 TCPMv1, TCPMv2: add pd event for receiving hard reset)
+=======
+/* Copyright 2018 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.
+ *
+ * Common test code to test lid angle calculation.
+ */
+#ifndef __CROS_EC_MOTION_COMMON_H
+#define __CROS_EC_MOTION_COMMON_H
+
+#include "motion_sense.h"
+/*
+ * Period in us for the motion task period.
+ * The task will read the vectors at that interval
+ */
+#define TEST_LID_EC_RATE (1 * MSEC)
+#define TEST_LID_FREQUENCY (1e9 / TEST_LID_EC_RATE)   /* mHz */
+
+/*
+ * Time in ms to wait for the task to read the vectors.
+ */
+#define TEST_LID_SLEEP_RATE (TEST_LID_EC_RATE / 5)
+
+/* We gather 6 elements [2 vectors of 3 axis] per sample. */
+#define TEST_LID_SAMPLE_SIZE (2 * 3)
+
+extern enum chipset_state_mask sensor_active;
+
+extern struct motion_sensor_t motion_sensors[];
+extern const unsigned int motion_sensor_count;
+
+void wait_for_valid_sample(void);
+void feed_accel_data(const float *array, int *idx,
+		int (filler)(const struct motion_sensor_t *s, const float f));
+
+/*
+ * External data - from
+ * chromium/src/ash/wm/tablet_mode/tablet_mode_controller_unittest.cc
+ *
+ * Test accelerometer data taken with the lid at less than 180 degrees while
+ * shaking the device around. The data is to be interpreted in groups of 6 where
+ * each 6 values corresponds to the base accelerometer (-y / g, -x / g, -z / g)
+ * followed by the lid accelerometer (-y / g , x / g, z / g).
+ * [ CONFIG_ACCEL_STD_REF_FRAME_OLD must be defined to used this array. ]
+ */
+extern const float kAccelerometerLaptopModeTestData[];
+extern const size_t kAccelerometerLaptopModeTestDataLength;
+
+/*
+ * Test accelerometer data taken with the lid open 360 degrees while
+ * shaking the device around. The data is to be interpreted in groups of 6 where
+ * each 6 values corresponds to the base accelerometer (-y / g, -x / g, -z / g)
+ * followed by the lid accelerometer (-y / g , x / g, z / g).
+ * [ CONFIG_ACCEL_STD_REF_FRAME_OLD must be defined to used this array. ]
+ */
+extern const float kAccelerometerFullyOpenTestData[];
+extern const size_t kAccelerometerFullyOpenTestDataLength;
+
+/*
+ * Test accelerometer data taken with the lid open 360 degrees while the device
+ * hinge was nearly vertical, while shaking the device around. The data is to be
+ * interpreted in groups of 6 where each 6 values corresponds to the X, Y, and Z
+ * readings from the base and lid accelerometers in this order.
+ */
+extern const float kAccelerometerVerticalHingeTestData[];
+extern const size_t kAccelerometerVerticalHingeTestDataLength;
+extern const float kAccelerometerVerticalHingeUnstableTestData[];
+extern const size_t kAccelerometerVerticalHingeUnstableTestDataLength;
+#endif  /* __CROS_EC_MOTION_COMMON_H */
+>>>>>>> CHANGE (0c71c4 motion sense: Calculate loop time based on sensor needs)
diff --git a/test/motion_lid.c b/test/motion_lid.c
index 3282812e86..67700d5aa2 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -21,7 +21,6 @@
 #include "util.h"
 
 extern enum chipset_state_mask sensor_active;
-extern unsigned motion_interval;
 
 /*
  * Period in us for the motion task period.
@@ -179,15 +178,18 @@ static int test_lid_angle(void)
 	hook_notify(HOOK_CHIPSET_SHUTDOWN);
 	TEST_ASSERT(sensor_active == SENSOR_ACTIVE_S5);
 	TEST_ASSERT(accel_get_data_rate(lid) == 0);
-	TEST_ASSERT(motion_interval == 0);
 
 	/* Go to S0 state */
 	hook_notify(HOOK_CHIPSET_SUSPEND);
 	hook_notify(HOOK_CHIPSET_RESUME);
 	msleep(1000);
 	TEST_ASSERT(sensor_active == SENSOR_ACTIVE_S0);
+<<<<<<< HEAD   (67a275 TCPMv1, TCPMv2: add pd event for receiving hard reset)
 	TEST_ASSERT(accel_get_data_rate(lid) == (119000 | ROUND_UP_FLAG));
 	TEST_ASSERT(motion_interval == TEST_LID_EC_RATE);
+=======
+	TEST_ASSERT(accel_get_data_rate(lid) == 119000);
+>>>>>>> CHANGE (0c71c4 motion sense: Calculate loop time based on sensor needs)
 
 	/*
 	 * Set the base accelerometer as if it were sitting flat on a desk