diff options
| -rw-r--r-- | driver/accelgyro_lsm6dsm.c | 36 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6dsm.h | 13 |
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 |