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.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx>
---
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);
/**
--
2.17.1
