diff options
| author |   Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> | 2020-02-19 15:39:54 +0100 |
|---|---|---|
| committer |   Jonathan Cameron <Jonathan.Cameron@huawei.com> | 2020-03-08 17:28:38 +0000 |
| commit | ffc9648a52c4cfae3eaadf629d79a0356f3f5900 (patch) | |
| tree | 862139c89b94bdf05792ba68e3d2d6e54b042e1f /drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | |
| parent | iio: imu: inv_mpu6050: rewrite power and engine management (diff) | |
| download | wireguard-linux-ffc9648a52c4cfae3eaadf629d79a0356f3f5900.tar.xz wireguard-linux-ffc9648a52c4cfae3eaadf629d79a0356f3f5900.zip | |
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>
Diffstat (limited to 'drivers/iio/imu/inv_mpu6050/inv_mpu_core.c')
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 36 |
1 files changed, 34 insertions, 2 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; |