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>

2 files changed, 30 insertions, 15 deletions

diff --git a/include/motion_sense.h b/include/motion_sense.h
index cebd7907a7..b5fc36dfe4 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -180,13 +180,17 @@ struct motion_sensor_t {
 	uint16_t lost;
 
 	/*
-	 * Time since last collection:
-	 * For sensor with hardware FIFO,  time since last sample
-	 * has move from the hardware FIFO to the FIFO (used if fifo rate != 0).
-	 * For sensor without FIFO, time since the last event was collect
-	 * from sensor registers.
+	 * For sensors in forced mode the ideal time to collect the next
+	 * measurement.
+	 *
+	 * This is unused with sensors that interrupt the ec like hw fifo chips.
+	 */
+	uint32_t next_collection;
+
+	/*
+	 * The time in us between collection measurements
 	 */
-	uint32_t last_collection;
+	uint32_t collection_rate;
 
 	/* Minimum supported sampling frequency in miliHertz for this sensor */
 	uint32_t min_frequency;
diff --git a/include/timer.h b/include/timer.h
index 02a50070c1..47fbe7a4bd 100644
--- a/include/timer.h
+++ b/include/timer.h
@@ -134,6 +134,23 @@ void force_time(timestamp_t ts);
 void timer_print_info(void);
 
 /**
+ * Returns a free running millisecond clock counter, which matches tpm2
+ * library expectations.
+ */
+clock_t clock(void);
+
+/**
+ * Compute how far to_time is from from_time with rollover taken into account
+ *
+ * Return us until to_time given from_time, if negative then to_time has
+ * passeed from_time.
+ */
+static inline int time_until(uint32_t from_time, uint32_t to_time)
+{
+	return (int32_t)(to_time - from_time);
+}
+
+/**
  * Returns the number of microseconds that have elapsed from a start time.
  *
  * This function is for timing short delays typically of a few milliseconds
@@ -143,27 +160,21 @@ void timer_print_info(void);
  * hour. After that, the value returned will wrap.
  *
  * @param start		Start time to compare against
- * @return number of microseconds that have elspsed since that start time
+ * @return number of microseconds that have elapsed since that start time
  */
 static inline unsigned time_since32(timestamp_t start)
 {
-	return get_time().le.lo - start.le.lo;
+	return time_until(start.le.lo, get_time().le.lo);
 }
 
 /**
- * Returns a free running millisecond clock counter, which matches tpm2
- * library expectations.
- */
-clock_t clock(void);
-
-/**
  * To compare time and deal with rollover
  *
  * Return true if a is after b.
  */
 static inline int time_after(uint32_t a, uint32_t b)
 {
-	return (int32_t)(b - a) < 0;
+	return time_until(a, b) < 0;
 }
 
 #endif  /* __CROS_EC_TIMER_H */