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>

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 1578493f6105ef2eb7ba085a8e736b4f2f4f0e22..53b302960654d91e9d2d21668fbf14989ec3cc47 100644 (file)
--- 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 a6c45ce459ee3942a15bcf30f96fc64b16bfc4cb..c4e24148c733d623ca1c8d4a53e4da3dd5988004 100644 (file)
--- 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