diff options
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 1 | ||||
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 7 | ||||
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 3 |
3 files changed, 10 insertions, 1 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h index 142a835a29e4..dfb9e4e8c40a 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h @@ -148,6 +148,7 @@ struct inv_mpu6050_state { struct regmap *map; int irq; u8 irq_mask; + unsigned skip_samples; }; /*register and associated bit definition*/ diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c index e51404fdce97..1b5735474728 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c @@ -185,7 +185,12 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) if (result == 0) timestamp = 0; - iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp); + /* skip first samples if needed */ + if (st->skip_samples) + st->skip_samples--; + else + iio_push_to_buffers_with_timestamp(indio_dev, data, + timestamp); fifo_count -= bytes_per_datum; } diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c index 8a9f869b9d14..f4b1c7135bfd 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c @@ -49,11 +49,14 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) if (result) return result; inv_scan_query(indio_dev); + st->skip_samples = 0; if (st->chip_config.gyro_fifo_enable) { result = inv_mpu6050_switch_engine(st, true, INV_MPU6050_BIT_PWR_GYRO_STBY); if (result) goto error_power_off; + /* gyro first sample is out of specs, skip it */ + st->skip_samples = 1; } if (st->chip_config.accl_fifo_enable) { result = inv_mpu6050_switch_engine(st, true, |