Add startup time for each chip using inv_mpu6050 driver

Add startup time for each chip familly. This allows a better behaviour of
the gyro and the accel. The gyro has now the time to stabilise itself
thus making initial data discarding for gyro irrelevant.

Signed-off-by: Baptiste Mansuy <bmansuy@invensense.com>
Acked-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Link: https://lore.kernel.org/r/20210621085731.9212-1-bmansuy@invensense.com
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 8a7a920..597768c 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -143,6 +143,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6050,
                .fifo_size = 1024,
                .temp = {INV_MPU6050_TEMP_OFFSET, INV_MPU6050_TEMP_SCALE},
+               .startup_time = {INV_MPU6050_GYRO_STARTUP_TIME, INV_MPU6050_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_MPU6500_WHOAMI_VALUE,
@@ -151,6 +152,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 512,
                .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},
+               .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_MPU6515_WHOAMI_VALUE,
@@ -159,6 +161,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 512,
                .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},
+               .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_MPU6880_WHOAMI_VALUE,
@@ -167,6 +170,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 4096,
                .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},
+               .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_MPU6000_WHOAMI_VALUE,
@@ -175,6 +179,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6050,
                .fifo_size = 1024,
                .temp = {INV_MPU6050_TEMP_OFFSET, INV_MPU6050_TEMP_SCALE},
+               .startup_time = {INV_MPU6050_GYRO_STARTUP_TIME, INV_MPU6050_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_MPU9150_WHOAMI_VALUE,
@@ -183,6 +188,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6050,
                .fifo_size = 1024,
                .temp = {INV_MPU6050_TEMP_OFFSET, INV_MPU6050_TEMP_SCALE},
+               .startup_time = {INV_MPU6050_GYRO_STARTUP_TIME, INV_MPU6050_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_MPU9250_WHOAMI_VALUE,
@@ -191,6 +197,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 512,
                .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},
+               .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_MPU9255_WHOAMI_VALUE,
@@ -199,6 +206,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 512,
                .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},
+               .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_ICM20608_WHOAMI_VALUE,
@@ -207,6 +215,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 512,
                .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
+               .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_ICM20609_WHOAMI_VALUE,
@@ -215,6 +224,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 4 * 1024,
                .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
+               .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_ICM20689_WHOAMI_VALUE,
@@ -223,6 +233,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 4 * 1024,
                .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
+               .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_ICM20602_WHOAMI_VALUE,
@@ -231,6 +242,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 1008,
                .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
+               .startup_time = {INV_ICM20602_GYRO_STARTUP_TIME, INV_ICM20602_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_ICM20690_WHOAMI_VALUE,
@@ -239,6 +251,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 1024,
                .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
+               .startup_time = {INV_ICM20690_GYRO_STARTUP_TIME, INV_ICM20690_ACCEL_STARTUP_TIME},
        },
        {
                .whoami = INV_IAM20680_WHOAMI_VALUE,
@@ -247,6 +260,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
                .config = &chip_config_6500,
                .fifo_size = 512,
                .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
+               .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
        },
 };
 
@@ -379,12 +393,12 @@ int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en,
                sleep = 0;
                if (en) {
                        if (mask & INV_MPU6050_SENSOR_ACCL) {
-                               if (sleep < INV_MPU6050_ACCEL_UP_TIME)
-                                       sleep = INV_MPU6050_ACCEL_UP_TIME;
+                               if (sleep < st->hw->startup_time.accel)
+                                       sleep = st->hw->startup_time.accel;
                        }
                        if (mask & INV_MPU6050_SENSOR_GYRO) {
-                               if (sleep < INV_MPU6050_GYRO_UP_TIME)
-                                       sleep = INV_MPU6050_GYRO_UP_TIME;
+                               if (sleep < st->hw->startup_time.gyro)
+                                       sleep = st->hw->startup_time.gyro;
                        }
                } else {
                        if (mask & INV_MPU6050_SENSOR_GYRO) {

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 58188dc..c6aa36e 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -149,6 +149,10 @@ struct inv_mpu6050_hw {
                int offset;
                int scale;
        } temp;
+       struct {
+               unsigned int accel;
+               unsigned int gyro;
+       } startup_time;
 };
 
 /*
@@ -320,11 +324,21 @@ struct inv_mpu6050_state {
 /* delay time in milliseconds */
 #define INV_MPU6050_POWER_UP_TIME            100
 #define INV_MPU6050_TEMP_UP_TIME             100
-#define INV_MPU6050_ACCEL_UP_TIME            20
-#define INV_MPU6050_GYRO_UP_TIME             35
+#define INV_MPU6050_ACCEL_STARTUP_TIME       20
+#define INV_MPU6050_GYRO_STARTUP_TIME        60
 #define INV_MPU6050_GYRO_DOWN_TIME           150
 #define INV_MPU6050_SUSPEND_DELAY_MS         2000
 
+#define INV_MPU6500_GYRO_STARTUP_TIME        70
+#define INV_MPU6500_ACCEL_STARTUP_TIME       30
+
+#define INV_ICM20602_GYRO_STARTUP_TIME       100
+#define INV_ICM20602_ACCEL_STARTUP_TIME      20
+
+#define INV_ICM20690_GYRO_STARTUP_TIME       80
+#define INV_ICM20690_ACCEL_STARTUP_TIME      10
+
+
 /* delay time in microseconds */
 #define INV_MPU6050_REG_UP_TIME_MIN          5000
 #define INV_MPU6050_REG_UP_TIME_MAX          10000

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index 2d0e8cd..8825468 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -91,22 +91,11 @@ static unsigned int inv_scan_query(struct iio_dev *indio_dev)
 
 static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
 {
-       unsigned int gyro_skip = 0;
-       unsigned int magn_skip = 0;
-       unsigned int skip_samples;
-
-       /* gyro first sample is out of specs, skip it */
-       if (st->chip_config.gyro_fifo_enable)
-               gyro_skip = 1;
+       unsigned int skip_samples = 0;
 
        /* mag first sample is always not ready, skip it */
        if (st->chip_config.magn_fifo_enable)
-               magn_skip = 1;
-
-       /* compute first samples to skip */
-       skip_samples = gyro_skip;
-       if (magn_skip > skip_samples)
-               skip_samples = magn_skip;
+               skip_samples = 1;
 
        return skip_samples;
 }