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>

2 files changed, 34 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
 	 */
 
diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h
index 3336326b90..fd7f59c3b3 100644
--- a/driver/accelgyro_lsm6dsm.h
+++ b/driver/accelgyro_lsm6dsm.h
@@ -267,9 +267,22 @@ struct lsm6dsm_data {
 	struct lsm6dsm_fifo_data config;
 	struct lsm6dsm_fifo_data current;
 	int next_in_patten;
+	/*
+	 * After an ODR change, the sensor filters need settling time; discard
+	 * initial samples with incorrect values
+	 */
+	unsigned int samples_to_discard[FIFO_DEV_NUM];
 #endif
 };
 
+/*
+ * Note: The specific number of samples to discard depends on the filters
+ * configured for the chip, as well as the ODR being set.  For most of our
+ * allowed ODRs, 4 should suffice.
+ * See: ST's LSM6DSM application notes (AN4987) Tables 17 and 19 for details
+ */
+#define LSM6DSM_DISCARD_SAMPLES 4
+
 #define LSM6DSM_MAIN_SENSOR(_s) ((_s) - (_s)->type)
 
 #ifdef CONFIG_ACCEL_FIFO