motion: Alter ec_rate to prevent samples without data

If EC sampling rate is close to sensor rate, decrease sampling frequency
by 5% to prevent samples by the EC without data.
It can happen when the clocks are slightly different and get
unsynchronized.

BRANCH=smaug
BUG=b:24367625
TEST=Ran cts.SingleSensorTests overnight without error.

Change-Id: Iab5e578763171411eb474e1e717167c8e1ef7ecf
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/312985
Reviewed-by: Alec Berg <alecaberg@chromium.org>
(cherry picked from commit d98999f3685ddbd27af86e1d96f2af030af9beab)
Reviewed-on: https://chromium-review.googlesource.com/313348

1 files changed, 40 insertions, 9 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index ac2ffb8148..8a21377abf 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -163,11 +163,11 @@ static inline int motion_sensor_time_to_read(const timestamp_t *ts,
 	if (rate == 0)
 		return 0;
 	/*
-	 * converting from kHz to us, need 1e9,
+	 * converting from kHz to us.
 	 * If within 95% of the time, check sensor.
 	 */
 	return time_after(ts->le.lo,
-			  sensor->last_collection + 950000000 / rate);
+			  sensor->last_collection + SECOND * 950 / rate);
 }
 
 static enum sensor_config motion_sense_get_ec_config(void)
@@ -236,6 +236,41 @@ int motion_sense_set_data_rate(struct motion_sensor_t *sensor)
 	return 0;
 }
 
+static int motion_sense_set_ec_rate_from_ap(
+		const struct motion_sensor_t *sensor,
+		unsigned int new_rate)
+{
+	int ap_odr = BASE_ODR(sensor->config[SENSOR_CONFIG_AP].odr);
+
+	if (new_rate == 0)
+		return 0;
+#ifdef CONFIG_ACCEL_FORCE_MODE_MASK
+	if (CONFIG_ACCEL_FORCE_MODE_MASK & (1 << (sensor - motion_sensors)))
+		goto end_set_ec_rate_from_ap;
+#endif
+	if (ap_odr == 0)
+		goto end_set_ec_rate_from_ap;
+
+	/*
+	 * If the EC collection rate is close to the sensor data rate,
+	 * given variation from the EC scheduler, it is possible that a sensor
+	 * will not present any measurement for a given time slice, and then 2
+	 * measurement for the next. That will create a large interval between
+	 * 2 measurements.
+	 * To prevent that, increase the EC period by 5% to be sure to get at
+	 * least one measurement at every collection time.
+	 * We wll apply that correction only if the ec rate is within 10% of
+	 * the data rate.
+	 */
+	if (SECOND * 1100 / ap_odr > new_rate)
+		new_rate = new_rate / 100 * 105;
+
+end_set_ec_rate_from_ap:
+	return MAX(new_rate, motion_min_interval);
+}
+
+
+
 static int motion_sense_select_ec_rate(
 		const struct motion_sensor_t *sensor,
 		enum sensor_config config_id)
@@ -917,13 +952,9 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 		 * has a value.
 		 */
 		if (in->ec_rate.data != EC_MOTION_SENSE_NO_VALUE) {
-			if (in->ec_rate.data == 0)
-				sensor->config[SENSOR_CONFIG_AP].ec_rate = 0;
-			else
-				sensor->config[SENSOR_CONFIG_AP].ec_rate =
-					MAX(in->ec_rate.data * MSEC,
-					    motion_min_interval);
-
+			sensor->config[SENSOR_CONFIG_AP].ec_rate =
+				motion_sense_set_ec_rate_from_ap(
+					sensor, in->ec_rate.data * MSEC);
 			/* Bound the new sampling rate. */
 			motion_sense_set_motion_intervals();
 		}