iio: imu: inv_mpu6050: skip first sample when gyro is on

Implement generic skip first samples mechanism and use it to
filter out first sample when gyro is on.

The problem for these chips is that the first sample of the gyro
is out of specs, because gyro is not completely stabilized. To
ensure all data are within sensor specs, we just skip the first
sample when turning gyro on.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 142a835a29e42d265b98916cdcfb7699e8d86a29..dfb9e4e8c40a3f93047ce91ed209f36ce1f92696 100644 (file)
--- 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 e51404fdce972f0e52198bc92b1d77ada1f42884..1b5735474728e3e84e6e9284df8b1e45b736998b 100644 (file)
--- 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 8a9f869b9d14769cfab981e561b073d97760ceb9..f4b1c7135bfdceea68a2cb5a2fd55ecd8cdccccb 100644 (file)
--- 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,