MPU6050模組是塊好東西大夥都知道，圍繞這個幾塊錢的東西就可以做很多很好玩的東西，什麼四翼飛行器、平衡車等。當然要完全使用這塊模組不是那麼容易。

解析說明

• 其實我們主要是想通過6050得到尤拉角和四元數
• 要通過6050得到四元數和尤拉角，這個過程有兩種辦法，一種是用原始資料（三軸加速度、三軸角速度），通過一些（卡爾曼、積分運算、減少誤差零點漂移等）姿態融合運算轉化即可
• 另一種是直接用MPU6050 內部的自帶的數字運動處理器（即DMP），我們要用這個DMP功能的話，就要實現載入韌體
• 那麼這個DMP怎麼拿呢，Invensense公司有提供了一個MPU6050的嵌入式運動驅動庫，進而我們可以通過這個庫移植很方便得出尤拉角
• 不過Invensense公司提供的6050運動驅動庫是基於MSP430的，需要將其移植到其他晶片上（比如：STM32F1系列），當然就要移植改程式碼啦

移植：

該驅動重點就是兩個c檔案：inv_mu.c和inv_mpu_dmp_motion_driver.c其中，inv_mu.c中添加了幾個函式，方便讀者使用。我們要做的是怎麼操作上面兩個檔案裡面的函式，方便我們得出我們想要的結果，重點介紹兩個函式：
初始化DMP_Init()函式如下

void DMP_Init(void)
{ 
    if(!mpu_init())     //mpu初始化
  {
      if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)
 
)    //設定需要的感測器
         printf("mpu_set_sensor complete ......\r\n");
      if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //設定fifo
         printf("mpu_configure_fifo complete ......\r\n");
      if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))              //設定採集樣率
         printf("mpu_set_sample_rate complete ......\r\n"
 
);
      if(!dmp_load_motion_driver_firmware())                //載入dmp韌體
        printf("dmp_load_motion_driver_firmware complete ......\r\n");
      if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
         printf("dmp_set_orientation complete ......\r\n"); //設定陀螺儀方向
      if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
            DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
            DMP_FEATURE_GYRO_CAL))
         printf("dmp_enable_feature complete ......\r\n");
      if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))    //設定速率
         printf("dmp_set_fifo_rate complete ......\r\n");
      run_self_test();                          //自檢
      if(!mpu_set_dmp_state(1))                 //使能
         printf("mpu_set_dmp_state complete ......\r\n");
  }

}

DMP_Init的作用就是初始化，再設定mpu裡面各種引數，然後就可以通過
Read_DMP()函式讀取姿態結算資料了。
初始化Read_DMP()函式如下

uint8_t Read_DMP(float* Pitch,float* Roll,float* Yaw)
{   
        short gyro[3], accel[3], sensors;
        float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
      unsigned long sensor_timestamp;
        unsigned char more;
        long quat[4];
                if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)) return 1;      
                if (sensors & INV_WXYZ_QUAT)
                {    
                     q0=quat[0] / q30;
                     q1=quat[1] / q30;
                     q2=quat[2] / q30;
                     q3=quat[3] / q30;
                     *Pitch = (float)asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;     
                     *Roll = (float)atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
                     *Yaw = (float)atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
                     return 0;
                }   
                else 
                    return 2;

}

ps： 使用MPU6050的DMP輸出的四元數是q30格式的，也就是浮點數放大了2^30倍。在換算成尤拉角之前，必須先將其轉換為浮點數，也就是除以2^30，然後在計算。

然後到主函式裡，初始化後，直接呼叫Read_DMP即可。

    DMP_Init();

    while(1)
    {
        printf("Read_DMP Return is %d\n",Read_DMP(&Pitch,&Roll,&Yaw));
        printf("Pitch is:%f,Roll is:%f,Yaw is:%f\n",Pitch,Roll,Yaw);

    }

注意：Return_DMP函式的返回值，0成功，1失敗，說明不是每次都能成功讀出尤拉角，所以延時一段時間後再讀（比如一秒讀一次資料）很有可能失敗！！！

還有一點
因為IIC函式我們提供，注意inv_mpu.c檔案實現MPU6050和IIC介面連線的宣告

#define i2c_write   i2cwrite
#define i2c_read    i2cread
#define delay_ms    delay_ms
#define get_ms      get_ms

工程檔案：https://pan.baidu.com/s/1jIp5ZaY
工程環境：Keil4.7
晶片：STM32F103
PS：一年前嘗試做平衡小車被各種姿態公式打敗了，最近隔壁宿舍有位同學做輛平衡小車通過dmp獲取尤拉角，遂再研究一下。接下來有時間的話再嘗試平衡車。

本人理解不深，有問題希望能和大夥一起探討。