mirror of
https://git.kernel.org/pub/scm/linux/kernel/git/stable/linux.git
synced 2024-09-14 06:35:12 +00:00
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>
This commit is contained in:
parent
92e7407ab1
commit
ffc9648a52
3 changed files with 37 additions and 23 deletions
|
@ -563,9 +563,14 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev,
|
||||||
int *val)
|
int *val)
|
||||||
{
|
{
|
||||||
struct inv_mpu6050_state *st = iio_priv(indio_dev);
|
struct inv_mpu6050_state *st = iio_priv(indio_dev);
|
||||||
|
unsigned int freq_hz, period_us, min_sleep_us, max_sleep_us;
|
||||||
int result;
|
int result;
|
||||||
int ret;
|
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);
|
result = inv_mpu6050_set_power_itg(st, true);
|
||||||
if (result)
|
if (result)
|
||||||
return result;
|
return result;
|
||||||
|
@ -576,6 +581,10 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev,
|
||||||
INV_MPU6050_SENSOR_GYRO);
|
INV_MPU6050_SENSOR_GYRO);
|
||||||
if (result)
|
if (result)
|
||||||
goto error_power_off;
|
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,
|
ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro,
|
||||||
chan->channel2, val);
|
chan->channel2, val);
|
||||||
result = inv_mpu6050_switch_engine(st, false,
|
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);
|
INV_MPU6050_SENSOR_ACCL);
|
||||||
if (result)
|
if (result)
|
||||||
goto error_power_off;
|
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,
|
ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl,
|
||||||
chan->channel2, val);
|
chan->channel2, val);
|
||||||
result = inv_mpu6050_switch_engine(st, false,
|
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);
|
INV_MPU6050_SENSOR_TEMP);
|
||||||
if (result)
|
if (result)
|
||||||
goto error_power_off;
|
goto error_power_off;
|
||||||
/* wait for stablization */
|
/* wait 1 period for first sample availability */
|
||||||
msleep(INV_MPU6050_TEMP_UP_TIME);
|
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,
|
ret = inv_mpu6050_sensor_show(st, st->reg->temperature,
|
||||||
IIO_MOD_X, val);
|
IIO_MOD_X, val);
|
||||||
result = inv_mpu6050_switch_engine(st, false,
|
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;
|
goto error_power_off;
|
||||||
break;
|
break;
|
||||||
case IIO_MAGN:
|
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);
|
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;
|
break;
|
||||||
default:
|
default:
|
||||||
ret = -EINVAL;
|
ret = -EINVAL;
|
||||||
|
|
|
@ -44,9 +44,6 @@
|
||||||
#define INV_MPU_MAGN_REG_ASAY 0x11
|
#define INV_MPU_MAGN_REG_ASAY 0x11
|
||||||
#define INV_MPU_MAGN_REG_ASAZ 0x12
|
#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)
|
static bool inv_magn_supported(const struct inv_mpu6050_state *st)
|
||||||
{
|
{
|
||||||
switch (st->chip_type) {
|
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;
|
unsigned int status;
|
||||||
__be16 data;
|
__be16 data;
|
||||||
uint8_t addr;
|
uint8_t addr;
|
||||||
unsigned int freq_hz, period_ms;
|
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
/* quit if chip is not supported */
|
/* 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;
|
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 */
|
/* check i2c status and read raw data */
|
||||||
ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status);
|
ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status);
|
||||||
if (ret)
|
if (ret)
|
||||||
|
|
|
@ -10,6 +10,9 @@
|
||||||
|
|
||||||
#include "inv_mpu_iio.h"
|
#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);
|
int inv_mpu_magn_probe(struct inv_mpu6050_state *st);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
Loading…
Reference in a new issue