/**
* @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.mag->raw_data;
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->read_len >
sizeof(inv_obj.mag->raw_data)) {
LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
return INV_ERROR_INVALID_CONFIGURATION;
}
if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->bus ==
EXT_SLAVE_BUS_PRIMARY ||
!(mldl_cfg->inv_mpu_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->slave[EXT_SLAVE_TYPE_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];
}
} else { /* compass on the 2nd bus or DMP is off */
result = inv_get_external_sensor_data(data, 3);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
data[0] = inv_q30_mult(data[0], inv_obj.mag->asa[0]);
data[1] = inv_q30_mult(data[1], inv_obj.mag->asa[1]);
data[2] = inv_q30_mult(data[2], inv_obj.mag->asa[2]);
return INV_SUCCESS;
}
/**
* @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.mag->raw_data;
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->read_len >
sizeof(inv_obj.mag->raw_data)) {
LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
return INV_ERROR_INVALID_CONFIGURATION;
}
if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->bus ==
EXT_SLAVE_BUS_PRIMARY ||
!(mldl_cfg->inv_mpu_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->slave[EXT_SLAVE_TYPE_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];
}
} 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_slave[EXT_SLAVE_TYPE_COMPASS]->address,
// the register to write to
/*mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->trigger->reg */
0x0A,
// the value to write
/*mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->trigger->value */
0x01,
// enable the write
0xC0
};
result =
inv_serial_write(inv_get_serial_handle(),
mldl_cfg->mpu_chip_info->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->mpu_chip_info->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.mag->asa[0]);
data[1] = inv_q30_mult(data[1], inv_obj.mag->asa[1]);
data[2] = inv_q30_mult(data[2], inv_obj.mag->asa[2]);
return INV_SUCCESS;
}
