关于MPU6050的DMP输出问题以及如何使用加速度陀螺仪数据进行姿态融合

Wechen

在火哥MPU6050视频教程中以及零死角玩转STM32教程中，都说STM32无需自己计算，可直接获取姿态角，但是通过分析代码，姿态角像是不是直接获取，而是通过四元数计算的方法得到姿态角，不过计算过程是移植官方库的，STM32依旧需要计算。[mw_shl_code=c,true]/**
*  @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;
}[/mw_shl_code]

不知道我的理解是否存在问题，所以发帖求助一下。
同时，求助如何将加速度结合陀螺仪进行一阶滤波得到姿态角？谢谢大家。

WishWish

帮顶

AIxuexi12

楼主解决没。