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, &regs[0]);

        result = inv_set_mpu_memory(KEY_D_2_236, 4, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    }

    return result;
}
示例#2
0
文件: compass.c 项目: DuinoPilot/TMR
/** 
 *  @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;
}
示例#3
0
/**
 *  @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;
}
示例#4
0
文件: compass.c 项目: DuinoPilot/TMR
/**
 *  @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;
}