Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit d5098447 authored by Matt Ranostay's avatar Matt Ranostay Committed by Jonathan Cameron
Browse files

iio: imu: mpu6050: add calibration offset support 



Allow setting of the x/y/z axes calibration offsets for the gyroscope
and accelerometer.

Signed-off-by: default avatarMatt Ranostay <matt.ranostay@intel.com>
Signed-off-by: default avatarJonathan Cameron <jic23@kernel.org>
parent 725f645d

drivers/iio/imu/inv_mpu6050/inv_mpu_core.c

+49 −2
Original line number Diff line number Diff line
@@ -55,6 +55,8 @@ static const struct inv_mpu6050_reg_map reg_set_6050 = { 
	.pwr_mgmt_1             = INV_MPU6050_REG_PWR_MGMT_1,

	.pwr_mgmt_2             = INV_MPU6050_REG_PWR_MGMT_2,

	.int_pin_cfg		= INV_MPU6050_REG_INT_PIN_CFG,

	.accl_offset		= INV_MPU6050_REG_ACCEL_OFFSET,

	.gyro_offset		= INV_MPU6050_REG_GYRO_OFFSET,

};



static const struct inv_mpu6050_chip_config chip_config_6050 = {
@@ -203,6 +205,20 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev) 
	return result;

}



static int inv_mpu6050_sensor_set(struct inv_mpu6050_state  *st, int reg,

				int axis, int val)

{

	int ind, result;

	__be16 d = cpu_to_be16(val);



	ind = (axis - IIO_MOD_X) * 2;

	result = regmap_bulk_write(st->map, reg + ind, (u8 *)&d, 2);

	if (result)

		return -EINVAL;



	return 0;

}



static int inv_mpu6050_sensor_show(struct inv_mpu6050_state  *st, int reg,

				   int axis, int *val)

{
@@ -224,11 +240,12 @@ inv_mpu6050_read_raw(struct iio_dev *indio_dev, 
		     int *val, int *val2, long mask)

{

	struct inv_mpu6050_state  *st = iio_priv(indio_dev);

	int ret = 0;



	switch (mask) {

	case IIO_CHAN_INFO_RAW:

	{

		int ret, result;

		int result;



		ret = IIO_VAL_INT;

		result = 0;
@@ -324,6 +341,20 @@ inv_mpu6050_read_raw(struct iio_dev *indio_dev, 
		default:

			return -EINVAL;

		}

	case IIO_CHAN_INFO_CALIBBIAS:

		switch (chan->type) {

		case IIO_ANGL_VEL:

			ret = inv_mpu6050_sensor_show(st, st->reg->gyro_offset,

						chan->channel2, val);

			return IIO_VAL_INT;

		case IIO_ACCEL:

			ret = inv_mpu6050_sensor_show(st, st->reg->accl_offset,

						chan->channel2, val);

			return IIO_VAL_INT;



		default:

			return -EINVAL;

		}

	default:

		return -EINVAL;

	}
@@ -421,6 +452,21 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev, 
			break;

		}

		break;

	case IIO_CHAN_INFO_CALIBBIAS:

		switch (chan->type) {

		case IIO_ANGL_VEL:

			result = inv_mpu6050_sensor_set(st,

							st->reg->gyro_offset,

							chan->channel2, val);

			break;

		case IIO_ACCEL:

			result = inv_mpu6050_sensor_set(st,

							st->reg->accl_offset,

							chan->channel2, val);

			break;

		default:

			result = -EINVAL;

		}

	default:

		result = -EINVAL;

		break;
@@ -578,7 +624,8 @@ static int inv_mpu6050_validate_trigger(struct iio_dev *indio_dev, 
		.modified = 1,                                        \

		.channel2 = _channel2,                                \

		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \

		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),         \

		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |	      \

				      BIT(IIO_CHAN_INFO_CALIBBIAS),   \

		.scan_index = _index,                                 \

		.scan_type = {                                        \

				.sign = 's',                          \

drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h

+10 −0
Original line number Diff line number Diff line
@@ -40,6 +40,8 @@ 
 *  @pwr_mgmt_1:	Controls chip's power state and clock source.

 *  @pwr_mgmt_2:	Controls power state of individual sensors.

 *  @int_pin_cfg;	Controls interrupt pin configuration.

 *  @accl_offset:	Controls the accelerometer calibration offset.

 *  @gyro_offset:	Controls the gyroscope calibration offset.

 */

struct inv_mpu6050_reg_map {

	u8 sample_rate_div;
@@ -57,6 +59,8 @@ struct inv_mpu6050_reg_map { 
	u8 pwr_mgmt_1;

	u8 pwr_mgmt_2;

	u8 int_pin_cfg;

	u8 accl_offset;

	u8 gyro_offset;

};



/*device enum */
@@ -133,6 +137,9 @@ struct inv_mpu6050_state { 
};



/*register and associated bit definition*/

#define INV_MPU6050_REG_ACCEL_OFFSET        0x06

#define INV_MPU6050_REG_GYRO_OFFSET         0x13



#define INV_MPU6050_REG_SAMPLE_RATE_DIV     0x19

#define INV_MPU6050_REG_CONFIG              0x1A

#define INV_MPU6050_REG_GYRO_CONFIG         0x1B
@@ -174,6 +181,9 @@ struct inv_mpu6050_state { 
#define INV_MPU6050_FIFO_COUNT_BYTE          2

#define INV_MPU6050_FIFO_THRESHOLD           500



/* mpu6500 registers */

#define INV_MPU6500_REG_ACCEL_OFFSET        0x77



/* delay time in milliseconds */

#define INV_MPU6050_POWER_UP_TIME            100

#define INV_MPU6050_TEMP_UP_TIME             100