diff options
Diffstat (limited to 'drivers/iio')
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 51 | ||||
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 10 |
2 files changed, 59 insertions, 2 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c index 1578493f6105..53b302960654 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c @@ -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', \ diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h index a6c45ce459ee..c4e24148c733 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h @@ -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 |