inv_error_t inv_set_max_gyro_bias(long bias)
{
inv_error_t result = INV_SUCCESS;
if (bias < 0) {
bias = bias * -1;
}
sgb.maxGyroBias = bias;
//DMP Code to push down into Mantis
{
unsigned char regs[4];
long dmp_thresh;
dmp_thresh = (long)(bias*46850.8254f);
inv_int32_to_big8(dmp_thresh, ®s[0]);
result = inv_set_mpu_memory(KEY_D_2_236, 4, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
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 *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;
}
/**
* @brief
*/
int populate_data_store(unsigned long sensor_mask,
short temp_avg, short gyro_biases[],
short accel_biases[], long accel_sens[])
{
int ptr = 0;
int tmp;
long long lltmp;
uint32_t chk;
/* total len of factory cal */
data_store[ptr++] = 0;
data_store[ptr++] = 0;
data_store[ptr++] = 0;
data_store[ptr++] = ML_INIT_CAL_LEN;
/* record type 5 - initial calibration */
data_store[ptr++] = 0;
data_store[ptr++] = 5;
if (sensor_mask & INV_THREE_AXIS_GYRO) {
/* temperature */
tmp = temp_avg;
if (tmp < 0)
tmp += 2 << 16;
inv_int16_to_big8(tmp, &data_store[ptr]);
ptr += 2;
/* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */
/* x gyro avg */
lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs;
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
ptr += 4;
/* y gyro avg */
lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs;
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
ptr += 4;
/* z gyro avg */
lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs;
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
ptr += 4;
} else {
ptr += (2 + 4 + 4 + 4);
}
if (sensor_mask & INV_THREE_AXIS_ACCEL) {
signed char *mtx =
mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]->orientation;
short tmp[3];
int ii;
/* need store the biases in chip frame - that is,
inverting the rotation applied in inv_get_accel_data */
memcpy(tmp, accel_biases, sizeof(tmp));
for (ii = 0; ii < ARRAY_SIZE(accel_biases); ii++) {
accel_biases[ii] = tmp[0] * mtx[3 * 0 + ii] +
tmp[1] * mtx[3 * 1 + ii] +
tmp[2] * mtx[3 * 2 + ii];
}
/*
if (sensor_mask & INV_X_ACCEL) {
// x accel avg
lltmp = (long)accel_biases[0] * 65536L / accel_sens[0];
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
}
*/
ptr += 4;
if (sensor_mask & INV_Y_ACCEL) {
/* y accel avg */
lltmp = (long)accel_biases[1] * 65536L / accel_sens[1];
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
}
ptr += 4;
/* z accel avg */
if (sensor_mask & INV_Z_ACCEL) {
lltmp = (long)accel_biases[2] * 65536L / accel_sens[2];
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
}
ptr += 4;
} else {
ptr += 12;
}
/* add a checksum for data */
chk = inv_checksum(
data_store + INV_CAL_HDR_LEN,
ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN);
inv_int32_to_big8(chk, &data_store[ptr]);
ptr += 4;
if (ptr != ML_INIT_CAL_LEN) {
MPL_LOGI("Invalid calibration data length: exp %d, got %d\n",
ML_INIT_CAL_LEN, ptr);
return -1;
}
return 0;
}
