From ffc9648a52c4cfae3eaadf629d79a0356f3f5900 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Maneyrol Date: Wed, 19 Feb 2020 15:39:54 +0100 Subject: [PATCH] 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 Signed-off-by: Jonathan Cameron --- drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 36 ++++++++++++++++++++-- drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c | 21 ------------- drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h | 3 ++ 3 files changed, 37 insertions(+), 23 deletions(-) diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c index a51efc4c941b..aeee39696d3a 100644 --- 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 3f402fa56d95..f282e9cc34c5 100644 --- 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 f7ad50ca6c1b..185c000c697c 100644 --- 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); /**