/** * @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; }