diff options
Diffstat (limited to 'drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c')
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c | 49 |
1 files changed, 9 insertions, 40 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c index 4f192352521e..f282e9cc34c5 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c @@ -44,9 +44,6 @@ #define INV_MPU_MAGN_REG_ASAY 0x11 #define INV_MPU_MAGN_REG_ASAZ 0x12 -/* Magnetometer maximum frequency */ -#define INV_MPU_MAGN_FREQ_HZ_MAX 50 - static bool inv_magn_supported(const struct inv_mpu6050_state *st) { switch (st->chip_type) { @@ -316,59 +313,32 @@ int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st) * * Returns 0 on success, a negative error code otherwise */ -int inv_mpu_magn_read(const struct inv_mpu6050_state *st, int axis, int *val) +int inv_mpu_magn_read(struct inv_mpu6050_state *st, int axis, int *val) { - unsigned int user_ctrl, status; - __be16 data[3]; + unsigned int status; + __be16 data; uint8_t addr; - uint8_t d; - unsigned int period_ms; int ret; /* quit if chip is not supported */ if (!inv_magn_supported(st)) return -ENODEV; - /* Mag data: X - Y - Z */ + /* Mag data: XH,XL,YH,YL,ZH,ZL */ switch (axis) { case IIO_MOD_X: addr = 0; break; case IIO_MOD_Y: - addr = 1; + addr = 2; break; case IIO_MOD_Z: - addr = 2; + addr = 4; break; default: return -EINVAL; } - - /* set sample rate to max mag freq */ - d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU_MAGN_FREQ_HZ_MAX); - ret = regmap_write(st->map, st->reg->sample_rate_div, d); - if (ret) - return ret; - - /* start i2c master, wait for xfer, stop */ - user_ctrl = st->chip_config.user_ctrl | INV_MPU6050_BIT_I2C_MST_EN; - ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl); - if (ret) - return ret; - - /* need to wait 2 periods + half-period margin */ - period_ms = 1000 / INV_MPU_MAGN_FREQ_HZ_MAX; - msleep(period_ms * 2 + period_ms / 2); - user_ctrl = st->chip_config.user_ctrl; - ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl); - if (ret) - return ret; - - /* restore sample rate */ - d = st->chip_config.divider; - ret = regmap_write(st->map, st->reg->sample_rate_div, d); - if (ret) - return ret; + addr += INV_MPU6050_REG_EXT_SENS_DATA; /* check i2c status and read raw data */ ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status); @@ -379,12 +349,11 @@ int inv_mpu_magn_read(const struct inv_mpu6050_state *st, int axis, int *val) status & INV_MPU6050_BIT_I2C_SLV1_NACK) return -EIO; - ret = regmap_bulk_read(st->map, INV_MPU6050_REG_EXT_SENS_DATA, - data, sizeof(data)); + ret = regmap_bulk_read(st->map, addr, &data, sizeof(data)); if (ret) return ret; - *val = (int16_t)be16_to_cpu(data[addr]); + *val = (int16_t)be16_to_cpu(data); return IIO_VAL_INT; } |