/** Converts from internal DMP gyro bias registers to external hardware gyro bias by
* applying scaling and transformation.
*/
void inv_convert_bias(const unsigned char *regs, short *bias)
{
long biasTmp2[3], biasTmp[3], biasPrev[3];
int i;
int sf;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (mldl_cfg->mpu_chip_info->gyro_sens_trim != 0) {
sf = 2000 * 131 / mldl_cfg->mpu_chip_info->gyro_sens_trim;
} else {
sf = 2000;
}
for (i = 0; i < 3; i++) {
biasTmp2[i] = inv_big8_to_int32(®s[i * 4]);
}
// Rotate bias vector by the transpose of the orientation matrix
for (i = 0; i < 3; ++i) {
biasTmp[i] = inv_q30_mult(biasTmp2[0], inv_obj.calmat->gyro_orient[i]) +
inv_q30_mult(biasTmp2[1], inv_obj.calmat->gyro_orient[i + 3]) +
inv_q30_mult(biasTmp2[2], inv_obj.calmat->gyro_orient[i + 6]);
}
for (i = 0; i < GYRO_NUM_AXES; i++) {
biasTmp[i] = (long)(biasTmp[i] * 1.39882274201861f / sf);
biasPrev[i] = (long)mldl_cfg->mpu_offsets->gyro[i];
if (biasPrev[i] > 32767)
biasPrev[i] -= 65536L;
}
for (i = 0; i < GYRO_NUM_AXES; i++) {
bias[i] = (short)(biasPrev[i] - biasTmp[i]);
}
}
/**
* @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 Sets the compass bias.
* @param bias
* Compass bias, length 3. Scale is chip units * 2^16.
* Frame is mount frame which may be different from body frame.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_set_compass_bias(struct compass_obj_t *obj, long *bias)
{
inv_error_t result = INV_SUCCESS;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
extern struct compass_obj_t inv_compass_obj;
if (obj == NULL) {
obj = &inv_compass_obj;
}
obj->bias[0] = bias[0];
obj->bias[1] = bias[1];
obj->bias[2] = bias[2];
inv_obj.mag->bias[0] = inv_q30_mult(bias[0], inv_obj.mag->sens);
inv_obj.mag->bias[1] = inv_q30_mult(bias[1], inv_obj.mag->sens);
inv_obj.mag->bias[2] = inv_q30_mult(bias[2], inv_obj.mag->sens);
if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
unsigned char reg[4];
long biasB[3];
signed char *orC =
mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->orientation;
// Now transform to body frame
biasB[0] = bias[0] * orC[0] + bias[1] * orC[1] + bias[2] * orC[2];
biasB[1] = bias[0] * orC[3] + bias[1] * orC[4] + bias[2] * orC[5];
biasB[2] = bias[0] * orC[6] + bias[1] * orC[7] + bias[2] * orC[8];
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4,
inv_int32_to_big8(biasB[0], reg));
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
inv_int32_to_big8(biasB[1], reg));
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
inv_int32_to_big8(biasB[2], reg));
}
return result;
}
void inv_matrix_vector_mult(const long *A, const long *x, long *y)
{
y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]);
y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]);
y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]);
}
/**
* @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;
}
