/**
* @brief Get a sample of compass data from the device.
* @param data
* the buffer to store the compass raw data for
* X, Y, and Z axes.
* @return INV_SUCCESS or a non-zero error code.
*/
inv_error_t inv_get_compass_data(long *data)
{
inv_error_t result;
int ii;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
unsigned char *tmp = inv_obj.compass_raw_data;
if (mldl_cfg->compass->read_len > sizeof(inv_obj.compass_raw_data)) {
LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
return INV_ERROR_INVALID_CONFIGURATION;
}
if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_PRIMARY ||
!(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
/*--- read the compass sensor data.
The compass read function may return an INV_ERROR_COMPASS_* errors
when the data is not ready (read/refresh frequency mismatch) or
the internal data sampling timing of the device was not respected.
Returning the error code will make the sensor fusion supervisor
ignore this compass data sample. ---*/
result = (inv_error_t) inv_mpu_read_compass(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(),
tmp);
if (result) {
if (COMPASS_DEBUG) {
MPL_LOGV("inv_mpu_read_compass returned %d\n", result);
}
return result;
}
for (ii = 0; ii < 3; ii++) {
if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->compass->endian)
data[ii] =
((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1];
else
data[ii] =
((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii];
}
inv_obj.compass_overunder = (int)tmp[6];
} else {
#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
defined CONFIG_MPU_SENSORS_MPU6050B1
result = inv_get_external_sensor_data(data, 3);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
#if defined CONFIG_MPU_SENSORS_MPU6050A2
{
static unsigned char first = TRUE;
// one-off write to AKM
if (first) {
unsigned char regs[] = {
// beginning Mantis register for one-off slave R/W
MPUREG_I2C_SLV4_ADDR,
// the slave to write to
mldl_cfg->pdata->compass.address,
// the register to write to
/*mldl_cfg->compass->trigger->reg */ 0x0A,
// the value to write
/*mldl_cfg->compass->trigger->value */ 0x01,
// enable the write
0xC0
};
result =
inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr,
ARRAY_SIZE(regs), regs);
first = FALSE;
} else {
unsigned char regs[] = {
MPUREG_I2C_SLV4_CTRL,
0xC0
};
result =
inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr,
ARRAY_SIZE(regs), regs);
}
}
#endif
#else
return INV_ERROR_INVALID_CONFIGURATION;
#endif // CONFIG_MPU_SENSORS_xxxx
}
data[0] = inv_q30_mult(data[0], inv_obj.compass_asa[0]);
data[1] = inv_q30_mult(data[1], inv_obj.compass_asa[1]);
data[2] = inv_q30_mult(data[2], inv_obj.compass_asa[2]);
return INV_SUCCESS;
}
static int mpu_handle_mlsl(void *sl_handle,
unsigned char addr,
unsigned int cmd,
struct mpu_read_write __user *usr_msg)
{
int retval = 0;
struct mpu_read_write msg;
unsigned char *user_data;
retval = copy_from_user(&msg, usr_msg, sizeof(msg));
if (retval)
return -EFAULT;
user_data = msg.data;
if (msg.length && msg.data) {
unsigned char *data;
data = kmalloc(msg.length, GFP_KERNEL);
if (!data)
return -ENOMEM;
retval = copy_from_user(data,
(void __user *)msg.data, msg.length);
if (retval) {
retval = -EFAULT;
kfree(data);
return retval;
}
msg.data = data;
} else {
return -EPERM;
}
switch (cmd) {
case MPU_READ:
retval = inv_serial_read(sl_handle, addr,
msg.address, msg.length, msg.data);
break;
case MPU_WRITE:
retval = inv_serial_write(sl_handle, addr,
msg.length, msg.data);
break;
case MPU_READ_MEM:
retval = inv_serial_read_mem(sl_handle, addr,
msg.address, msg.length, msg.data);
break;
case MPU_WRITE_MEM:
retval = inv_serial_write_mem(sl_handle, addr,
msg.address, msg.length,
msg.data);
break;
case MPU_READ_FIFO:
retval = inv_serial_read_fifo(sl_handle, addr,
msg.length, msg.data);
break;
case MPU_WRITE_FIFO:
retval = inv_serial_write_fifo(sl_handle, addr,
msg.length, msg.data);
break;
};
if (retval) {
dev_err(&((struct i2c_adapter *)sl_handle)->dev,
"%s: i2c %d error %d\n",
__func__, cmd, retval);
kfree(msg.data);
return retval;
}
retval = copy_to_user((unsigned char __user *)user_data,
msg.data, msg.length);
kfree(msg.data);
return retval;
}
