/**
* @brief Sets the compass bias.
* @param bias
* Compass bias, length 3. Scale is micro Tesla's * 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(long *bias)
{
inv_error_t result = INV_SUCCESS;
long biasC[3];
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
inv_obj.mag->bias[0] = bias[0];
inv_obj.mag->bias[1] = bias[1];
inv_obj.mag->bias[2] = bias[2];
// Find Bias in units of the raw data scaled by 2^16 in chip mounting frame
biasC[0] = (long) (bias[0] * (1LL << 30) / inv_obj.mag->sens);
biasC[1] = (long) (bias[1] * (1LL << 30) / inv_obj.mag->sens);
biasC[2] = (long) (bias[2] * (1LL << 30) / inv_obj.mag->sens);
if (IS_INV_ADVFEATURES_ENABLED(inv_obj)) {
biasC[0] += inv_obj.adv_fusion->init_compass_bias[0] * (1LL << 16);
biasC[1] += inv_obj.adv_fusion->init_compass_bias[1] * (1LL << 16);
biasC[2] += inv_obj.adv_fusion->init_compass_bias[2] * (1LL << 16);
}
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] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2];
biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5];
biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[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;
}
/**
* @brief Sets the compass bias.
* @param bias
* Compass bias, length 3. Scale is micro Tesla's * 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(long *bias)
{
inv_error_t result = INV_SUCCESS;
long biasC[3];
struct mldl_cfg *mldlCfg = inv_get_dl_config();
inv_obj.compass_bias[0] = bias[0];
inv_obj.compass_bias[1] = bias[1];
inv_obj.compass_bias[2] = bias[2];
// Find Bias in units of the raw data scaled by 2^16 in chip mounting frame
biasC[0] =
(long)(bias[0] * (1LL << 30) / inv_obj.compass_sens) +
inv_obj.init_compass_bias[0] * (1L << 16);
biasC[1] =
(long)(bias[1] * (1LL << 30) / inv_obj.compass_sens) +
inv_obj.init_compass_bias[1] * (1L << 16);
biasC[2] =
(long)(bias[2] * (1LL << 30) / inv_obj.compass_sens) +
inv_obj.init_compass_bias[2] * (1L << 16);
if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
unsigned char reg[4];
long biasB[3];
signed char *orC = mldlCfg->pdata->compass.orientation;
// Now transform to body frame
biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2];
biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5];
biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[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;
}
/**
* @brief Sets the compass bias on the DMP. Only compatible with MPU6050B1.
* @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_new_compass_bias(long *bias)
{
inv_error_t result = INV_SUCCESS;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
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));
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
inv_int32_to_big8(biasB[1], reg));
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
inv_int32_to_big8(biasB[2], reg));
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
} else {
return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
}
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;
}
