|
我們期望得到的是姿態(tài)數(shù)據(jù)，也就是平衡小車的傾角。要得到平衡小車的傾角，就得利用我們的原始數(shù)據(jù)，進(jìn)行姿態(tài)融合解算，這個(gè)比較復(fù)雜，知識(shí)點(diǎn)比較多，初學(xué)者不易掌握。其實(shí)， MPU6050 自帶了數(shù)字運(yùn)動(dòng)處理器， 即 DMP（Digital Motion Processing）， 并且InvenSense提供了一個(gè) MPU6050 的嵌入式運(yùn)動(dòng)驅(qū)動(dòng)庫， 結(jié)合 MPU6050 的 DMP， 可以將我們的原始數(shù)據(jù)，直接轉(zhuǎn)換成四元數(shù)輸出，而得到四元數(shù)之后，就可以很方便的計(jì)算出歐拉角，從而得到 yaw、roll 和 pitch。 使用內(nèi)置的 DMP，簡化了平衡小車的代碼設(shè)計(jì)，且單片機(jī)不用進(jìn)行姿態(tài)解算過程，在一定程度上降低了 MCU 的負(fù)擔(dān)，從而有更多的時(shí)間去處理其他事件，提高系統(tǒng)實(shí)時(shí)性（其實(shí)卡爾曼濾波和互補(bǔ)濾波在STM32里面也沒花多少時(shí)間）。更重要的是，讓大家可以不需要接觸復(fù)雜的濾波算法就可以直接得到平衡小車傾角。 使用 MPU6050 的 DMP 輸出的四元數(shù)是 q30 格式的， 也就是浮點(diǎn)數(shù)放大了 2的 30 次方倍。在換算成歐拉角之前，必須先將其轉(zhuǎn)換為浮點(diǎn)數(shù)，也就是除以 2 的30 次方，然后再進(jìn)行計(jì)算， 計(jì)算公式為： q0=quat[0] / q30; //q30 格式轉(zhuǎn)換為浮點(diǎn)數(shù) q1=quat[1] / q30; q2=quat[2] / q30; q3=quat[3] / q30; //計(jì)算得到俯仰角/橫滾角/航向角 pitch=asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //俯仰角 roll=atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; //橫滾角 yaw=atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //航向角 這里，我們是做平衡小車而不是四軸飛行器，所以，僅僅使用pitch就行了。其他兩個(gè)角度直接在程序中屏蔽了，因?yàn)榉慈呛瘮?shù)運(yùn)算在STM32F1中還是需要不少時(shí)間的。 移植官方 DMP 驅(qū)動(dòng)庫并不難，下面我們來講解一下如何移植。 首先打開我們提供的資料里面的文件夾【DMP移植相關(guān)文件和工程】，可以看到如下文件 （見附件） 其中庫文件包含了模擬IIC的初始化和相關(guān)函數(shù)，普通工程就是一個(gè)簡單的工程。比如我們常用的開發(fā)板資源里面的流水燈實(shí)驗(yàn)工程代碼就行，也可以使用你們自己的工程。【移植成功的工程】是移植好了的，大家可以先不看。 首先，我們先把【庫文件】里面的三個(gè)文件夾都拷貝到【普通工程】里面的【HARDWARE】文件夾。 然后在【普通工程】的【USER】里面找到minibalance打開。 會(huì)出現(xiàn)如下界面 然后，我們在MDK里面雙擊上圖中的HARDWARE,在彈出的對話框中把【HARDWARE】里面的所有C文件ADD進(jìn)去 完成后工程如下圖所示 然后加入文件包含路徑，操作如下圖所示 然后，把下面的文件包含到SYS.H里面 view plaincopy toclipboardprint? 1. #include "delay.h" 2. #include "usart.h" 3. #include "ioi2c.h" 4. #include "mpu6050.h" 5. #include "inv_mpu.h" 6. #include "inv_mpu_dmp_motion_driver.h" 7. #include "dmpKey.h" 8. #include "dmpmap.h" 9. #include 10. #include 11. #include 12. #include 13. #include 14. #include
成功后如下圖所示 然后回到主函數(shù)，調(diào)用IIC和MPU6050的初始化函數(shù) 至此，程序移植就完成了，然后我們來到void Read_DMP(void)函數(shù)里面，添加串口打印語句 printf("%f\r\n",Pitch);，成功后代碼如下所示： view plaincopy toclipboardprint? 1. void Read_DMP(void) 2. 3. { 4. 5. unsigned long sensor_timestamp; 6. 7. unsigned char more; 8. 9. long quat[4]; 10. 11. dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); 12. 13. if (sensors & INV_WXYZ_QUAT ) 14. 15. { 16. 17. q0=quat[0] / q30; 18. 19. q1=quat[1] / q30; 20. 21. q2=quat[2] / q30; 22. 23. q3=quat[3] / q30; 24. 25. Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; 26. 27. printf("%f\r\n",Pitch); 28. 29. } 30. 31. }
還有，請大家注意MPU6050連接STM32的IO,因?yàn)槭悄MIIC，所以可以隨便接，只要IO初始化合適就行了 #define IIC_SCL PAout(11) //SCL
#define IIC_SDA PAout(8) //SDA
#define READ_SDA PAin(8) //輸入SDA
在本實(shí)驗(yàn)中，我們SDA接PA8 ,SCL接PA11 至此，我們就可以把程序下載進(jìn)去驗(yàn)證了。串口調(diào)試助手會(huì)顯示MPU6050的俯仰角Pitch。
| 
