iv/linux
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
f2f45a5364
linux/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c
Jean-Baptiste Maneyrol ffc9648a52 iio: imu: inv_mpu6050: fix data polling interface 
When reading data with the polling interface, we need to wait
at 1 sampling period to have a sample.
For gyroscope and magnetometer, we need to wait for 2 periods
before having a correct sample.

Not suitable for stable or backporting.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
2020-03-08 17:28:38 +00:00

360 lines
8.3 KiB
C
Raw Blame History

// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (C) 2019 TDK-InvenSense, Inc.
*/
#include <linux/kernel.h>
#include <linux/device.h>
#include <linux/string.h>
#include "inv_mpu_aux.h"
#include "inv_mpu_iio.h"
#include "inv_mpu_magn.h"
/*
* MPU9xxx magnetometer are AKM chips on I2C aux bus
* MPU9150 is AK8975
* MPU9250 is AK8963
*/
#define INV_MPU_MAGN_I2C_ADDR 0x0C
#define INV_MPU_MAGN_REG_WIA 0x00
#define INV_MPU_MAGN_BITS_WIA 0x48
#define INV_MPU_MAGN_REG_ST1 0x02
#define INV_MPU_MAGN_BIT_DRDY 0x01
#define INV_MPU_MAGN_BIT_DOR 0x02
#define INV_MPU_MAGN_REG_DATA 0x03
#define INV_MPU_MAGN_REG_ST2 0x09
#define INV_MPU_MAGN_BIT_HOFL 0x08
#define INV_MPU_MAGN_BIT_BITM 0x10
#define INV_MPU_MAGN_REG_CNTL1 0x0A
#define INV_MPU_MAGN_BITS_MODE_PWDN 0x00
#define INV_MPU_MAGN_BITS_MODE_SINGLE 0x01
#define INV_MPU_MAGN_BITS_MODE_FUSE 0x0F
#define INV_MPU9250_MAGN_BIT_OUTPUT_BIT 0x10
#define INV_MPU9250_MAGN_REG_CNTL2 0x0B
#define INV_MPU9250_MAGN_BIT_SRST 0x01
#define INV_MPU_MAGN_REG_ASAX 0x10
#define INV_MPU_MAGN_REG_ASAY 0x11
#define INV_MPU_MAGN_REG_ASAZ 0x12
static bool inv_magn_supported(const struct inv_mpu6050_state *st)
{
switch (st->chip_type) {
case INV_MPU9150:
case INV_MPU9250:
case INV_MPU9255:
return true;
default:
return false;
}
}
/* init magnetometer chip */
static int inv_magn_init(struct inv_mpu6050_state *st)
{
uint8_t val;
uint8_t asa[3];
int32_t sensitivity;
int ret;
/* check whoami */
ret = inv_mpu_aux_read(st, INV_MPU_MAGN_I2C_ADDR, INV_MPU_MAGN_REG_WIA,
&val, sizeof(val));
if (ret)
return ret;
if (val != INV_MPU_MAGN_BITS_WIA)
return -ENODEV;
/* software reset for MPU925x only */
switch (st->chip_type) {
case INV_MPU9250:
case INV_MPU9255:
ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR,
INV_MPU9250_MAGN_REG_CNTL2,
INV_MPU9250_MAGN_BIT_SRST);
if (ret)
return ret;
break;
default:
break;
}
/* read fuse ROM data */
ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR,
INV_MPU_MAGN_REG_CNTL1,
INV_MPU_MAGN_BITS_MODE_FUSE);
if (ret)
return ret;
ret = inv_mpu_aux_read(st, INV_MPU_MAGN_I2C_ADDR, INV_MPU_MAGN_REG_ASAX,
asa, sizeof(asa));
if (ret)
return ret;
/* switch back to power-down */
ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR,
INV_MPU_MAGN_REG_CNTL1,
INV_MPU_MAGN_BITS_MODE_PWDN);
if (ret)
return ret;
/*
* Sensor sentivity
* 1 uT = 0.01 G and value is in micron (1e6)
* sensitvity = x uT * 0.01 * 1e6
*/
switch (st->chip_type) {
case INV_MPU9150:
/* sensor sensitivity is 0.3 uT */
sensitivity = 3000;
break;
case INV_MPU9250:
case INV_MPU9255:
/* sensor sensitivity in 16 bits mode: 0.15 uT */
sensitivity = 1500;
break;
default:
return -EINVAL;
}
/*
* Sensitivity adjustement and scale to Gauss
*
* Hadj = H * (((ASA - 128) * 0.5 / 128) + 1)
* Factor simplification:
* Hadj = H * ((ASA + 128) / 256)
*
* raw_to_gauss = Hadj * sensitivity
*/
st->magn_raw_to_gauss[0] = (((int32_t)asa[0] + 128) * sensitivity) / 256;
st->magn_raw_to_gauss[1] = (((int32_t)asa[1] + 128) * sensitivity) / 256;
st->magn_raw_to_gauss[2] = (((int32_t)asa[2] + 128) * sensitivity) / 256;
return 0;
}
/**
* inv_mpu_magn_probe() - probe and setup magnetometer chip
* @st: driver internal state
*
* Returns 0 on success, a negative error code otherwise
*
* It is probing the chip and setting up all needed i2c transfers.
* Noop if there is no magnetometer in the chip.
*/
int inv_mpu_magn_probe(struct inv_mpu6050_state *st)
{
uint8_t val;
int ret;
/* quit if chip is not supported */
if (!inv_magn_supported(st))
return 0;
/* configure i2c master aux port */
ret = inv_mpu_aux_init(st);
if (ret)
return ret;
/* check and init mag chip */
ret = inv_magn_init(st);
if (ret)
return ret;
/*
* configure mpu i2c master accesses
* i2c SLV0: read sensor data, 7 bytes data(6)-ST2
* Byte swap data to store them in big-endian in impair address groups
*/
ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0),
INV_MPU6050_BIT_I2C_SLV_RNW | INV_MPU_MAGN_I2C_ADDR);
if (ret)
return ret;
ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(0),
INV_MPU_MAGN_REG_DATA);
if (ret)
return ret;
ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0),
INV_MPU6050_BIT_SLV_EN |
INV_MPU6050_BIT_SLV_BYTE_SW |
INV_MPU6050_BIT_SLV_GRP |
INV_MPU9X50_BYTES_MAGN);
if (ret)
return ret;
/* i2c SLV1: launch single measurement */
ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(1),
INV_MPU_MAGN_I2C_ADDR);
if (ret)
return ret;
ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(1),
INV_MPU_MAGN_REG_CNTL1);
if (ret)
return ret;
/* add 16 bits mode for MPU925x */
val = INV_MPU_MAGN_BITS_MODE_SINGLE;
switch (st->chip_type) {
case INV_MPU9250:
case INV_MPU9255:
val |= INV_MPU9250_MAGN_BIT_OUTPUT_BIT;
break;
default:
break;
}
ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_DO(1), val);
if (ret)
return ret;
return regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(1),
INV_MPU6050_BIT_SLV_EN | 1);
}
/**
* inv_mpu_magn_set_rate() - set magnetometer sampling rate
* @st: driver internal state
* @fifo_rate: mpu set fifo rate
*
* Returns 0 on success, a negative error code otherwise
*
* Limit sampling frequency to the maximum value supported by the
* magnetometer chip. Resulting in duplicated data for higher frequencies.
* Noop if there is no magnetometer in the chip.
*/
int inv_mpu_magn_set_rate(const struct inv_mpu6050_state *st, int fifo_rate)
{
uint8_t d;
/* quit if chip is not supported */
if (!inv_magn_supported(st))
return 0;
/*
* update i2c master delay to limit mag sampling to max frequency
* compute fifo_rate divider d: rate = fifo_rate / (d + 1)
*/
if (fifo_rate > INV_MPU_MAGN_FREQ_HZ_MAX)
d = fifo_rate / INV_MPU_MAGN_FREQ_HZ_MAX - 1;
else
d = 0;
return regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_CTRL, d);
}
/**
* inv_mpu_magn_set_orient() - fill magnetometer mounting matrix
* @st: driver internal state
*
* Returns 0 on success, a negative error code otherwise
*
* Fill magnetometer mounting matrix using the provided chip matrix.
*/
int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st)
{
const char *orient;
char *str;
int i;
/* fill magnetometer orientation */
switch (st->chip_type) {
case INV_MPU9150:
case INV_MPU9250:
case INV_MPU9255:
/* x <- y */
st->magn_orient.rotation[0] = st->orientation.rotation[3];
st->magn_orient.rotation[1] = st->orientation.rotation[4];
st->magn_orient.rotation[2] = st->orientation.rotation[5];
/* y <- x */
st->magn_orient.rotation[3] = st->orientation.rotation[0];
st->magn_orient.rotation[4] = st->orientation.rotation[1];
st->magn_orient.rotation[5] = st->orientation.rotation[2];
/* z <- -z */
for (i = 0; i < 3; ++i) {
orient = st->orientation.rotation[6 + i];
/* use length + 2 for adding minus sign if needed */
str = devm_kzalloc(regmap_get_device(st->map),
strlen(orient) + 2, GFP_KERNEL);
if (str == NULL)
return -ENOMEM;
if (strcmp(orient, "0") == 0) {
strcpy(str, orient);
} else if (orient[0] == '-') {
strcpy(str, &orient[1]);
} else {
str[0] = '-';
strcpy(&str[1], orient);
}
st->magn_orient.rotation[6 + i] = str;
}
break;
default:
st->magn_orient = st->orientation;
break;
}
return 0;
}
/**
* inv_mpu_magn_read() - read magnetometer data
* @st: driver internal state
* @axis: IIO modifier axis value
* @val: store corresponding axis value
*
* Returns 0 on success, a negative error code otherwise
*/
int inv_mpu_magn_read(struct inv_mpu6050_state *st, int axis, int *val)
{
unsigned int status;
__be16 data;
uint8_t addr;
int ret;
/* quit if chip is not supported */
if (!inv_magn_supported(st))
return -ENODEV;
/* Mag data: XH,XL,YH,YL,ZH,ZL */
switch (axis) {
case IIO_MOD_X:
addr = 0;
break;
case IIO_MOD_Y:
addr = 2;
break;
case IIO_MOD_Z:
addr = 4;
break;
default:
return -EINVAL;
}
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);
if (ret)
return ret;
if (status & INV_MPU6050_BIT_I2C_SLV0_NACK ||
status & INV_MPU6050_BIT_I2C_SLV1_NACK)
return -EIO;
ret = regmap_bulk_read(st->map, addr, &data, sizeof(data));
if (ret)
return ret;
*val = (int16_t)be16_to_cpu(data);
return IIO_VAL_INT;
}
Reference in New Issue View Git Blame Copy Permalink