LSM6: Throw out junk data after ODR changes

After an ODR change on the LSM6DSx sensor, a certain amount of data will
be invalid until the filter for the sensor settles.  This change is to
throw out datapoints after an ODR change to avoid sending the AP any
outright bad values.

Currently, we're waiting for clarification from ST about how many data
points to throw out and whether the accelerometer and gyroscope ODR
changes affect each other.  For now, we're being conservative and
throwing out 4 samples of any sensors on after an ODR change.

BUG=b:122912601
BRANCH=octopus
TEST=almost all standard deviation failures gone when running CTS on
phaser360, a few very close gyro errors remain

Change-Id: Ie00c85e18333ce578152ed3ac616815405e8111d
Signed-off-by: Diana Z <dzigterman@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1423123
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-by: Enrico Granata <egranata@chromium.org>
Reviewed-by: Jett Rink <jettrink@chromium.org>

1 files changed, 21 insertions, 15 deletions

diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index cb36de8900..59a46cdaac 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -139,6 +139,8 @@ int accelgyro_fifo_enable(const struct motion_sensor_t *s)
 			decimator[i] = LSM6DSM_FIFO_DECIMATOR(max_odr / rate);
 			private->config.total_samples_in_pattern +=
 				private->config.samples_in_pattern[i];
+			private->samples_to_discard[i] =
+							LSM6DSM_DISCARD_SAMPLES;
 		} else {
 			/* Not in FIFO if sensor disabled. */
 			private->config.samples_in_pattern[i] = 0;
@@ -172,6 +174,8 @@ int accelgyro_fifo_enable(const struct motion_sensor_t *s)
  * |________|_______|_______|_______|________|_______|_______|
  *
  * Total samples for each pattern: 2 Gyro, 4 Acc, 1 Mag.
+ *
+ * Returns dev_fifo enum value of next sample to process
  */
 static int fifo_next(struct lsm6dsm_data *private)
 {
@@ -230,18 +234,24 @@ static void push_fifo_data(struct motion_sensor_t *accel, uint8_t *fifo,
 		if (next_fifo == FIFO_DEV_INVALID) {
 			return;
 		}
-		id = agm_maps[next_fifo];
-		axis = (accel + id)->raw_xyz;
-
-		/* Apply precision, sensitivity and rotation. */
-		st_normalize(accel + id, axis, fifo);
-		vect.data[X] = axis[X];
-		vect.data[Y] = axis[Y];
-		vect.data[Z] = axis[Z];
 
-		vect.flags = 0;
-		vect.sensor_num = accel - motion_sensors + id;
-		motion_sense_fifo_add_data(&vect, accel + id, 3, int_ts);
+		if (private->samples_to_discard[next_fifo] > 0) {
+			private->samples_to_discard[next_fifo]--;
+		} else {
+			id = agm_maps[next_fifo];
+			axis = (accel + id)->raw_xyz;
+
+			/* Apply precision, sensitivity and rotation. */
+			st_normalize(accel + id, axis, fifo);
+			vect.data[X] = axis[X];
+			vect.data[Y] = axis[Y];
+			vect.data[Z] = axis[Z];
+
+			vect.flags = 0;
+			vect.sensor_num = accel - motion_sensors + id;
+			motion_sense_fifo_add_data(&vect, accel + id, 3,
+						   int_ts);
+		}
 
 		fifo += OUT_XYZ_SIZE;
 		flen -= OUT_XYZ_SIZE;
@@ -264,10 +274,6 @@ static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts)
 	/*
 	 * TODO(b/122912601): phaser360: Investigate Standard Deviation error
 	 *				 during CtsSensorTests
-	 * - track number of samples to throw out after ODR changes
-	 * Accel discard: "should" be 0 for freq <= 26, 1 until 1666 Hz (table
-	 * 17)
-	 * Gyro discard: 12.5 Hz - 2, 26-833 Hz - 3 (table 19)
 	 * - check "pattern" register versus where code thinks it is parsing
 	 */