iio: imu: inv_mpu6050: fix data polling interface

When reading data with the polling interface, we need to wait
at 1 sampling period to have a sample.
For gyroscope and magnetometer, we need to wait for 2 periods
before having a correct sample.

Not suitable for stable or backporting.

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_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index a51efc4c941b951df7b63536aba1b3f788bbf8c3..aeee39696d3aca6cd7592bf5ff3fc71686c905fd 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -563,9 +563,14 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev,
                                         int *val)
 {
        struct inv_mpu6050_state *st = iio_priv(indio_dev);
+       unsigned int freq_hz, period_us, min_sleep_us, max_sleep_us;
        int result;
        int ret;
 
+       /* compute sample period */
+       freq_hz = INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
+       period_us = 1000000 / freq_hz;
+
        result = inv_mpu6050_set_power_itg(st, true);
        if (result)
                return result;
@@ -576,6 +581,10 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev,
                                INV_MPU6050_SENSOR_GYRO);
                if (result)
                        goto error_power_off;
+               /* need to wait 2 periods to have first valid sample */
+               min_sleep_us = 2 * period_us;
+               max_sleep_us = 2 * (period_us + period_us / 2);
+               usleep_range(min_sleep_us, max_sleep_us);
                ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro,
                                              chan->channel2, val);
                result = inv_mpu6050_switch_engine(st, false,
@@ -588,6 +597,10 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev,
                                INV_MPU6050_SENSOR_ACCL);
                if (result)
                        goto error_power_off;
+               /* wait 1 period for first sample availability */
+               min_sleep_us = period_us;
+               max_sleep_us = period_us + period_us / 2;
+               usleep_range(min_sleep_us, max_sleep_us);
                ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl,
                                              chan->channel2, val);
                result = inv_mpu6050_switch_engine(st, false,
@@ -600,8 +613,10 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev,
                                INV_MPU6050_SENSOR_TEMP);
                if (result)
                        goto error_power_off;
-               /* wait for stablization */
-               msleep(INV_MPU6050_TEMP_UP_TIME);
+               /* wait 1 period for first sample availability */
+               min_sleep_us = period_us;
+               max_sleep_us = period_us + period_us / 2;
+               usleep_range(min_sleep_us, max_sleep_us);
                ret = inv_mpu6050_sensor_show(st, st->reg->temperature,
                                              IIO_MOD_X, val);
                result = inv_mpu6050_switch_engine(st, false,
@@ -610,7 +625,24 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev,
                        goto error_power_off;
                break;
        case IIO_MAGN:
+               result = inv_mpu6050_switch_engine(st, true,
+                               INV_MPU6050_SENSOR_MAGN);
+               if (result)
+                       goto error_power_off;
+               /* frequency is limited for magnetometer */
+               if (freq_hz > INV_MPU_MAGN_FREQ_HZ_MAX) {
+                       freq_hz = INV_MPU_MAGN_FREQ_HZ_MAX;
+                       period_us = 1000000 / freq_hz;
+               }
+               /* need to wait 2 periods to have first valid sample */
+               min_sleep_us = 2 * period_us;
+               max_sleep_us = 2 * (period_us + period_us / 2);
+               usleep_range(min_sleep_us, max_sleep_us);
                ret = inv_mpu_magn_read(st, chan->channel2, val);
+               result = inv_mpu6050_switch_engine(st, false,
+                               INV_MPU6050_SENSOR_MAGN);
+               if (result)
+                       goto error_power_off;
                break;
        default:
                ret = -EINVAL;

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c
index 3f402fa56d9551414242900c76dc8327526c10ab..f282e9cc34c5329a82555067773c7c54325c80cc 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c
@@ -44,9 +44,6 @@
 #define INV_MPU_MAGN_REG_ASAY          0x11
 #define INV_MPU_MAGN_REG_ASAZ          0x12
 
-/* Magnetometer maximum frequency */
-#define INV_MPU_MAGN_FREQ_HZ_MAX       50
-
 static bool inv_magn_supported(const struct inv_mpu6050_state *st)
 {
        switch (st->chip_type) {
@@ -321,7 +318,6 @@ int inv_mpu_magn_read(struct inv_mpu6050_state *st, int axis, int *val)
        unsigned int status;
        __be16 data;
        uint8_t addr;
-       unsigned int freq_hz, period_ms;
        int ret;
 
        /* quit if chip is not supported */
@@ -344,23 +340,6 @@ int inv_mpu_magn_read(struct inv_mpu6050_state *st, int axis, int *val)
        }
        addr += INV_MPU6050_REG_EXT_SENS_DATA;
 
-       /* compute period depending on current sampling rate */
-       freq_hz = INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
-       if (freq_hz > INV_MPU_MAGN_FREQ_HZ_MAX)
-               freq_hz = INV_MPU_MAGN_FREQ_HZ_MAX;
-       period_ms = 1000 / freq_hz;
-
-       ret = inv_mpu6050_switch_engine(st, true, INV_MPU6050_SENSOR_MAGN);
-       if (ret)
-               return ret;
-
-       /* need to wait 2 periods + half-period margin */
-       msleep(period_ms * 2 + period_ms / 2);
-
-       ret = inv_mpu6050_switch_engine(st, false, INV_MPU6050_SENSOR_MAGN);
-       if (ret)
-               return ret;
-
        /* check i2c status and read raw data */
        ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status);
        if (ret)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h
index f7ad50ca6c1b111a07fb2157b082f7af9aa49ade..185c000c697c70ef07bb1d1a4bc7ada46b0de10a 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h
@@ -10,6 +10,9 @@
 
 #include "inv_mpu_iio.h"
 
+/* Magnetometer maximum frequency */
+#define INV_MPU_MAGN_FREQ_HZ_MAX       50
+
 int inv_mpu_magn_probe(struct inv_mpu6050_state *st);
 
 /**