就阿波罗开发板那个,设置满量程范围,是怎么设置的,这里来看不太懂? 还有那个欧拉角是怎么看出来哪个是pitch,哪个是roll

2019-07-20 08:00发布

  mpu_get_sample_rate(&gyro_rate);
        mpu_get_gyro_fsr(&gyro_fsr);
        mpu_get_accel_fsr(&accel_fsr);
        mpu_get_compass_fsr(&compass_fsr);
        inv_set_gyro_sample_rate(1000000L/gyro_rate);
        inv_set_accel_sample_rate(1000000L/gyro_rate);


、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@brief      Body-to-world frame euler angles.
*  The euler angles are output with the following convention:
*  Pitch: -180 to 180
*  Roll: -90 to 90
*  Yaw: -180 to 180
*  @param[out] data        Euler angles in degrees, q16 fixed point.
*  @param[out] accuracy    Accuracy of the measurement from 0 (least accurate)
*                          to 3 (most accurate).
*  @param[out] timestamp   The time in milliseconds when this sensor was read.
*  @return     1 if data was updated.
*/
int inv_get_sensor_type_euler(long *data, int8_t *accuracy, inv_time_t *timestamp)
{
    long t1, t2, t3;
    long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
    float values[3];

    q00 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[0]);
    q01 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[1]);
    q02 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[2]);
    q03 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[3]);
    q11 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[1]);
    q12 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[2]);
    q13 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[3]);
    q22 = inv_q29_mult(eMPL_out.quat[2], eMPL_out.quat[2]);
    q23 = inv_q29_mult(eMPL_out.quat[2], eMPL_out.quat[3]);
    q33 = inv_q29_mult(eMPL_out.quat[3], eMPL_out.quat[3]);

    /* X component of the Ybody axis in World frame */
    t1 = q12 - q03;

    /* Y component of the Ybody axis in World frame */
    t2 = q22 + q00 - (1L << 30);
    values[2] = -atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;

    /* Z component of the Ybody axis in World frame */
    t3 = q23 + q01;
    values[0] =
        atan2f((float) t3,
                sqrtf((float) t1 * t1 +
                      (float) t2 * t2)) * 180.f / (float) M_PI;
    /* Z component of the Zbody axis in World frame */
    t2 = q33 + q00 - (1L << 30);
    if (t2 < 0) {
        if (values[0] >= 0)
            values[0] = 180.f - values[0];
        else
            values[0] = -180.f - values[0];
    }

    /* X component of the Xbody axis in World frame */
    t1 = q11 + q00 - (1L << 30);
    /* Y component of the Xbody axis in World frame */
    t2 = q12 + q03;
    /* Z component of the Xbody axis in World frame */
    t3 = q13 - q02;

    values[1] =
        (atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) *
          180.f / (float) M_PI - 90);
    if (values[1] >= 90)
        values[1] = 180 - values[1];

    if (values[1] < -90)
        values[1] = -180 - values[1];
    data[0] = (long)(values[0] * 65536.f);
    data[1] = (long)(values[1] * 65536.f);
    data[2] = (long)(values[2] * 65536.f);

    accuracy[0] = eMPL_out.quat_accuracy;
    timestamp[0] = eMPL_out.nine_axis_timestamp;
    return eMPL_out.nine_axis_status;
}











友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。