diff options
-rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 56 |
1 files changed, 30 insertions, 26 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c index d8d5bed65e07..ef76afe2643c 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c @@ -410,42 +410,46 @@ error_read_raw: } } -static int inv_mpu6050_write_fsr(struct inv_mpu6050_state *st, int fsr) +static int inv_mpu6050_write_gyro_scale(struct inv_mpu6050_state *st, int val) { - int result; + int result, i; u8 d; - if (fsr < 0 || fsr > INV_MPU6050_MAX_GYRO_FS_PARAM) - return -EINVAL; - if (fsr == st->chip_config.fsr) - return 0; + for (i = 0; i < ARRAY_SIZE(gyro_scale_6050); ++i) { + if (gyro_scale_6050[i] == val) { + d = (i << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); + result = inv_mpu6050_write_reg(st, + st->reg->gyro_config, d); + if (result) + return result; - d = (fsr << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); - result = inv_mpu6050_write_reg(st, st->reg->gyro_config, d); - if (result) - return result; - st->chip_config.fsr = fsr; + st->chip_config.fsr = i; + return 0; + } + } - return 0; + return -EINVAL; } -static int inv_mpu6050_write_accel_fs(struct inv_mpu6050_state *st, int fs) +static int inv_mpu6050_write_accel_scale(struct inv_mpu6050_state *st, int val) { - int result; + int result, i; u8 d; - if (fs < 0 || fs > INV_MPU6050_MAX_ACCL_FS_PARAM) - return -EINVAL; - if (fs == st->chip_config.accl_fs) - return 0; + for (i = 0; i < ARRAY_SIZE(accel_scale); ++i) { + if (accel_scale[i] == val) { + d = (i << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); + result = inv_mpu6050_write_reg(st, + st->reg->accl_config, d); + if (result) + return result; - d = (fs << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); - result = inv_mpu6050_write_reg(st, st->reg->accl_config, d); - if (result) - return result; - st->chip_config.accl_fs = fs; + st->chip_config.accl_fs = i; + return 0; + } + } - return 0; + return -EINVAL; } static int inv_mpu6050_write_raw(struct iio_dev *indio_dev, @@ -471,10 +475,10 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev, case IIO_CHAN_INFO_SCALE: switch (chan->type) { case IIO_ANGL_VEL: - result = inv_mpu6050_write_fsr(st, val); + result = inv_mpu6050_write_gyro_scale(st, val2); break; case IIO_ACCEL: - result = inv_mpu6050_write_accel_fs(st, val); + result = inv_mpu6050_write_accel_scale(st, val2); break; default: result = -EINVAL; |