common: motion: Fix for calculating oversampling fix.

cl:302176 did not fully fix the issue:
- sampling rate would be unnecessary truncated to integer.
- Because the sensor can slightly oversample (15Hz -> 25Hz, 10Hz ->
  12.5Hz), we would skip samples for long period of time.
In both cases we skip samples in low speed tests, noticed by CTS tests.

BUG=b:24367625
BRANCH=smaug
TEST=Before we would fail some
android.hardware.cts.SingleSensorTests#testMagneticField_X,
Now pass.

Change-Id: Ic555e2add47ba89a0a0657f5eb492a5e7ca441d5
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/303010
Reviewed-by: Alec Berg <alecaberg@chromium.org>

1 files changed, 14 insertions, 10 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index c392e58b03..5c4ac84ea4 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -100,9 +100,10 @@ void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
 
 	/* For valid sensors, check if AP really needs this data */
 	if (valid_data) {
-		fp_t ap_odr = fp_div(BASE_ODR(sensor->config[SENSOR_CONFIG_AP].odr), 1000);
-
-		/* Use integer, conversion to FP will overflow */
+		/* Use Hz, conversion to FP will overflow with mHz */
+		fp_t ap_odr =
+			fp_div(BASE_ODR(sensor->config[SENSOR_CONFIG_AP].odr),
+			       1000);
 		fp_t rate = fp_div(sensor->drv->get_data_rate(sensor), 1000);
 
 		/*
@@ -127,13 +128,16 @@ void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
 			return;
 		}
 
-		/* Skip if EC is oversampling */
-		if (sensor->oversampling < 0) {
-			sensor->oversampling += fp_div(INT_TO_FP(1), rate);
-			return;
+		if (fp_mul(ap_odr, INT_TO_FP(2)) < rate) {
+			/* Skip if sensor is significantly oversampling */
+			if (sensor->oversampling < 0) {
+				sensor->oversampling +=
+					fp_div(INT_TO_FP(1), rate);
+				return;
+			}
+			sensor->oversampling += fp_div(INT_TO_FP(1), rate) -
+				fp_div(INT_TO_FP(1), ap_odr);
 		}
-		sensor->oversampling += fp_div(INT_TO_FP(1), rate) -
-			fp_div(INT_TO_FP(1), INT_TO_FP(ap_odr));
 	}
 	if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP) {
 		/*
@@ -234,6 +238,7 @@ int motion_sense_set_data_rate(struct motion_sensor_t *sensor)
 	roundup = !!(sensor->config[config_id].odr & ROUND_UP_FLAG);
 	CPRINTS("%s ODR: %d - roundup %d from config %d",
 		sensor->name, odr, roundup, config_id);
+	sensor->oversampling = 0;
 	return sensor->drv->set_data_rate(sensor, odr, roundup);
 }
 
@@ -339,7 +344,6 @@ static inline int motion_sense_init(struct motion_sensor_t *sensor)
 		timestamp_t ts = get_time();
 		sensor->state = SENSOR_INITIALIZED;
 		sensor->last_collection = ts.le.lo;
-		sensor->oversampling = 0;
 		motion_sense_set_data_rate(sensor);
 	}
 	return ret;