mpu6050dmp姿态解算stm32

### 使用STM32实现MPU6050 DMP姿态解算

#### 初始化函数 `MPU6050_DMP_init`

为了使能DMP功能，`MPU6050_DMP_init` 函数负责初始化MPU6050传感器，并加载必要的固件来配置DMP模块。该函数执行一系列操作以确保设备处于最佳工作状态。

#include "mpu6050.h"
#include "i2c.h"

int MPU6050_DMP_init(void) {
    // I2C初始化, 设置通信接口
    HAL_I2C_MspInit(&hi2c1);
    
    // 解除MPU6050睡眠模式
    mpu_set_sleep_enabled(0);

    // 配置采样率，默认为200Hz
    mpu_set_sample_rate(DEFAULT_MPU_HZ);

    // 加速度计满量程范围设为±8g
    mpu_set_full_scale_accel_range(MPU6050_ACCEL_FS_8);

    // 陀螺仪满量程范围设为±1000度/秒
    mpu_set_full_scale_gyro_range(MPU6050_GYRO_FS_1000);

    // 开启DMP特性
    if (mpu_dmp_initialize() != INV_SUCCESS) {
        return -1;
    }

    // 载入预编译好的DMP固件程序
    if (!mpu_setup_fifo()) {
        return -1;
    }
  
    // 启动DMP运行
    mpu_set_dmp_state(1);

    return 0; // 成功完成初始化
}

上述代码展示了如何利用I2C总线与MPU6050交互，调整其内部参数以便于后续的数据采集和处理[^1]。

#### 测试函数验证DMP库移植效果

创建一个简单的测试应用程序用于确认DMP库已经正确安装并且可以正常运作：

void test_mpu6050_dmp(void){
    uint8_t packetBuffer[14]; // FIFO buffer
    
    // 执行初始化流程
    if (MPU6050_DMP_init()){
        printf("Failed to initialize MPU6050\n");
        while (1); // 错误发生时进入死循环等待调试
    }else{
        printf("MPU6050 initialized successfully.\n");

        // 循环读取FIFO中的数据包直到接收到有效的Quaternion四元数信息为止
        do {
            int fifoCount = mpu_get_fifo_count();
            
            if(fifoCount >= 42){ // 当前有足够多的新样本可供读取
                
                mpu_get_int_status(); // 清理中断标志位
                
                // 尝试从FIFO队列里取出一帧完整的数据集
                mpu_get_fifodata(packetBuffer,sizeof(packetBuffer));
                
                Quaternion q; VectorFloat gravity;
                float ypr[3];

                // 获取当前的姿态角(Pitch,Roll,Yaw)
                mpu_dmp_get_data(q,gravity,ypr);

                // 输出角度至串口监视器查看结果
                printf("y:%.2f p:%.2f r:%.2f \r\n", DEG(ypr[YAW]),DEG(ypr[PITCH]),DEG(ypr[ROLL]));
            }
        }while(1);
    }
}

这段示例说明了怎样持续监控来自IMU芯片的信息流，并从中提取出代表物体空间方位变化的角度值[^2]。