Commit ffc9648a authored by Jean-Baptiste Maneyrol's avatar Jean-Baptiste Maneyrol Committed by Jonathan Cameron
Browse files

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: default avatarJean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent 92e7407a

drivers/iio/imu/inv_mpu6050/inv_mpu_core.c

+34 −2
Original line number Diff line number Diff line
@@ -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;

drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c

+0 −21
Original line number Diff line number Diff line
@@ -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)

drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h

+3 −0
Original line number Diff line number Diff line
@@ -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);



/**

CRA Git | Maintained and supported by SUSTech CRA and CCSE