UbiquitousOS
/
OpenCloudOS-Kernel
mirror of https://gitee.com/OpenCloudOS/OpenCloudOS-Kernel.git
11
0
Fork 0
Code Issues Projects Releases Wiki Activity

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: Matt Ranostay <matt.ranostay@intel.com>
Signed-off-by: Jonathan Cameron <jic23@kernel.org>
This commit is contained in:
Matt Ranostay 2016-02-22 13:39:10 -08:00 committed by Jonathan Cameron
parent 725f645d87
commit d509844714
2 changed files with 59 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
View File

@ -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 @@ error_read_raw:
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', \

10
drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
View File

@ -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