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 d4e81a72de85ec7eecfecf687abd396632dd9e78)
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1588075
Commit-Queue: Jett Rink <jettrink@chromium.org>
Tested-by: Jett Rink <jettrink@chromium.org>

1 files changed, 69 insertions, 67 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 4b4d996cc2..3ab78cf7b4 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -45,13 +45,8 @@ const intv3_t orientation_modes[] = {
 };
 #endif
 
-/*
- * Sampling interval for measuring acceleration and calculating lid angle.
- */
-test_export_static unsigned int motion_interval;
-
 /* Delay between FIFO interruption. */
-static unsigned int motion_int_interval;
+static unsigned int ap_event_interval;
 
 /* Minimum time in between running motion sense task loop. */
 unsigned int motion_min_interval = CONFIG_MOTION_MIN_SENSE_WAIT_TIME * MSEC;
@@ -212,22 +207,19 @@ static inline int motion_sensor_in_forced_mode(
 #endif
 }
 
-
-
 /* Minimal amount of time since last collection before triggering a new one */
 static inline int motion_sensor_time_to_read(const timestamp_t *ts,
 		const struct motion_sensor_t *sensor)
 {
-	int rate_mhz = sensor->drv->get_data_rate(sensor);
-
-	if (rate_mhz == 0)
+	if (sensor->collection_rate == 0)
 		return 0;
+
 	/*
-	 * converting from mHz to us.
-	 * If within 95% of the time, check sensor.
+	 * If the time is within the min motion interval (3 ms) go ahead and
+	 * read from the sensor
 	 */
 	return time_after(ts->le.lo,
-			  sensor->last_collection + SECOND * 950 / rate_mhz);
+			  sensor->next_collection - motion_min_interval);
 }
 
 static enum sensor_config motion_sense_get_ec_config(void)
@@ -301,7 +293,9 @@ int motion_sense_set_data_rate(struct motion_sensor_t *sensor)
 	 * Reset last collection: the last collection may be so much in the past
 	 * it may appear to be in the future.
 	 */
-	sensor->last_collection = ts.le.lo;
+	odr = sensor->drv->get_data_rate(sensor);
+	sensor->collection_rate = odr > 0 ? SECOND * 1000 / odr : 0;
+	sensor->next_collection = ts.le.lo + sensor->collection_rate;
 	sensor->oversampling = 0;
 	mutex_unlock(&g_sensor_mutex);
 	return 0;
@@ -407,9 +401,9 @@ static int motion_sense_ec_rate(struct motion_sensor_t *sensor)
  *
  * Note: Not static to be tested.
  */
-static int motion_sense_set_motion_intervals(void)
+static void motion_sense_set_motion_intervals(void)
 {
-	int i, sensor_ec_rate, ec_rate = 0, ec_int_rate = 0;
+	int i, sensor_ec_rate, ec_int_rate = 0;
 	struct motion_sensor_t *sensor;
 	for (i = 0; i < motion_sensor_count; ++i) {
 		sensor = &motion_sensors[i];
@@ -420,28 +414,20 @@ static int motion_sense_set_motion_intervals(void)
 		    (sensor->drv->get_data_rate(sensor) == 0))
 			continue;
 
-		sensor_ec_rate = motion_sense_ec_rate(sensor);
-		if (sensor_ec_rate == 0)
-			continue;
-		if (ec_rate == 0 || sensor_ec_rate < ec_rate)
-			ec_rate = sensor_ec_rate;
-
 		sensor_ec_rate = motion_sense_select_ec_rate(
 				sensor, SENSOR_CONFIG_AP, 1);
 		if (ec_int_rate == 0 ||
 		    (sensor_ec_rate && sensor_ec_rate < ec_int_rate))
 			ec_int_rate = sensor_ec_rate;
 	}
-	motion_interval = ec_rate;
 
-	motion_int_interval =
+	ap_event_interval =
 		MAX(0, ec_int_rate - MOTION_SENSOR_INT_ADJUSTMENT_US);
 	/*
 	 * Wake up the motion sense task: we want to sensor task to take
 	 * in account the new period right away.
 	 */
 	task_wake(TASK_ID_MOTIONSENSE);
-	return motion_interval;
 }
 
 static inline int motion_sense_init(struct motion_sensor_t *sensor)
@@ -717,6 +703,28 @@ static int motion_sense_read(struct motion_sensor_t *sensor)
 	return sensor->drv->read(sensor, sensor->raw_xyz);
 }
 
+
+static inline void increment_sensor_collection(struct motion_sensor_t *sensor,
+					       const timestamp_t *ts)
+{
+	sensor->next_collection += sensor->collection_rate;
+
+	while (time_after(ts->le.lo, sensor->next_collection)) {
+		/*
+		 * If we get here it means that we completely missed a sensor
+		 * collection time and we attempt to recover by incrementing
+		 * the next collection time until we recover. This should not
+		 * happen and if it does it means that the ec cannot handle the
+		 * requested data rate.
+		 */
+
+		CPRINTS("%s Missed data collection at %u - rate: %d",
+			sensor->name, sensor->next_collection,
+			sensor->collection_rate);
+		sensor->next_collection += sensor->collection_rate;
+	}
+}
+
 static int motion_sense_process(struct motion_sensor_t *sensor,
 				uint32_t *event,
 				const timestamp_t *ts)
@@ -759,7 +767,7 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 				motion_sense_fifo_add_data(&vector, sensor, 3,
 						   __hw_clock_source_read());
 			}
-			sensor->last_collection = ts->le.lo;
+			increment_sensor_collection(sensor, ts);
 		} else {
 			ret = EC_ERROR_BUSY;
 		}
@@ -777,7 +785,7 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		if (motion_sensor_time_to_read(ts, sensor)) {
 			/* Get latest data for local calculation */
 			ret = motion_sense_read(sensor);
-			sensor->last_collection = ts->le.lo;
+			increment_sensor_collection(sensor, ts);
 		} else {
 			ret = EC_ERROR_BUSY;
 		}
@@ -925,6 +933,7 @@ void motion_sense_task(void *u)
 {
 	int i, ret, wait_us;
 	timestamp_t ts_begin_task, ts_end_task;
+	int32_t time_diff;
 	uint32_t event = 0;
 	uint16_t ready_status;
 	struct motion_sensor_t *sensor;
@@ -998,7 +1007,6 @@ void motion_sense_task(void *u)
 		update_sense_data(lpc_status, &sample_id);
 #endif
 
-		ts_end_task = get_time();
 #ifdef CONFIG_ACCEL_FIFO
 		/*
 		 * Ask the host to flush the queue if
@@ -1010,13 +1018,13 @@ void motion_sense_task(void *u)
 		    event & (TASK_EVENT_MOTION_ODR_CHANGE |
 			     TASK_EVENT_MOTION_FLUSH_PENDING) ||
 		    queue_space(&motion_sense_fifo) < CONFIG_ACCEL_FIFO_THRES ||
-		    (motion_int_interval > 0 &&
-		     time_after(ts_end_task.le.lo,
-				ts_last_int.le.lo + motion_int_interval))) {
+		    (ap_event_interval > 0 &&
+		     time_after(ts_begin_task.le.lo,
+				ts_last_int.le.lo + ap_event_interval))) {
 			if ((event & TASK_EVENT_MOTION_FLUSH_PENDING) == 0)
 				motion_sense_insert_timestamp(
 					__hw_clock_source_read());
-			ts_last_int = ts_end_task;
+			ts_last_int = ts_begin_task;
 			/*
 			 * Count the number of event the AP is allowed to
 			 * collect.
@@ -1040,25 +1048,36 @@ void motion_sense_task(void *u)
 #endif
 		}
 #endif
-		if (motion_interval > 0) {
-			/*
-			 * Delay appropriately to keep sampling time
-			 * consistent.
-			 */
-			wait_us = motion_interval -
-				(ts_end_task.val - ts_begin_task.val);
 
-			/* and it cannnot be negative */
-			wait_us = MAX(wait_us, 0);
+		ts_end_task = get_time();
+		wait_us = -1;
+
+		for (i = 0; i < motion_sensor_count; i++) {
+			struct motion_sensor_t *sensor = &motion_sensors[i];
+
+			if (!motion_sensor_in_forced_mode(sensor) ||
+			   sensor->collection_rate == 0)
+				continue;
+
+			time_diff = time_until(ts_end_task.le.lo,
+					       sensor->next_collection);
 
+			/* We missed our collection time so wake soon */
+			if (time_diff <= 0) {
+				wait_us = 0;
+				break;
+			}
+
+			if (wait_us == -1 || wait_us > time_diff)
+				wait_us = time_diff;
+		}
+
+		if (wait_us >= 0 && wait_us < motion_min_interval) {
 			/*
-			 * Guarantee some minimum delay to allow other lower
-			 * priority tasks to run.
-			 */
-			if (wait_us < motion_min_interval)
-				wait_us = motion_min_interval;
-		} else {
-			wait_us = -1;
+			* Guarantee some minimum delay to allow other lower
+			* priority tasks to run.
+			*/
+			wait_us = motion_min_interval;
 		}
 
 		event = task_wait_event(wait_us);
@@ -1666,8 +1685,7 @@ static int command_accel_data_rate(int argc, char **argv)
 			 sensor->drv->get_data_rate(sensor));
 		ccprintf("EC rate for sensor %d: %d\n", id,
 			 motion_sense_ec_rate(sensor));
-		ccprintf("Current EC rate: %d\n", motion_interval);
-		ccprintf("Current Interrupt rate: %d\n", motion_int_interval);
+		ccprintf("Current Interrupt rate: %d\n", ap_event_interval);
 	}
 
 	return EC_SUCCESS;
@@ -1743,7 +1761,6 @@ DECLARE_CONSOLE_COMMAND(accelinit, command_accel_init,
 #ifdef CONFIG_CMD_ACCEL_INFO
 static int command_display_accel_info(int argc, char **argv)
 {
-	char *e;
 	int val, i, j;
 
 	if (argc > 3)
@@ -1780,21 +1797,6 @@ static int command_display_accel_info(int argc, char **argv)
 		accel_disp = val;
 	}
 
-	/*
-	 * Second arg changes the accel task time interval. Note accel
-	 * sampling interval will be clobbered when chipset suspends or
-	 * resumes.
-	 */
-	if (argc > 2) {
-		val = strtoi(argv[2], &e, 0);
-		if (*e)
-			return EC_ERROR_PARAM2;
-
-		motion_interval = val * MSEC;
-		task_wake(TASK_ID_MOTIONSENSE);
-
-	}
-
 	return EC_SUCCESS;
 }
 DECLARE_CONSOLE_COMMAND(accelinfo, command_display_accel_info,