玩四軸的都知道, MWC V2.5 飛控主板,板載陀螺儀傳感器就是MPU6050.(不信你可以拿放大鏡看).?
而默認的MWC開源程序是自己讀取MPU6050的原始數據,經過,自己的一套算法.算出來四軸當前的姿態.?
所以,MWC的源程序里面也就沒有使用 MPU6050的 DMP進行計算姿態.?
筆者為了做產品. 研究了很長時間的MWC源程序,始終沒看懂. 后來不得已.最終在同事的勸說下.鼓起勇氣?
全部自己寫..?
經過一個星期的拆解移植.終于把MWC的開源程序大卸八塊成獨立可用的程序. 然后又重新組裝在一起.?
但是讀取MPU6050 陀螺儀姿態的時候發現,讀取的是原始數據. 無法使用.而且還需要轉換成Yaw Pitch Roll 這種數據格式.?
這套復雜的數據融合算法. 俺表示搞不定..?
在上網查找資料的時候發現,MPU6050傳感器默認就能進行數據融合計算.而且不占CPU資源?
這個天大的好消息.引起了我的興趣.本來CPU就不夠用. 這一定要利用起來.

當我從網上down來源代碼, 復制到我的程序的時候,發現.?
DMP輸出的數據呈現周期性的數據異常. 數據波動非常大. 這要是用在飛機上一定機毀人亡….?
期初我以為是干擾,研究了一個禮拜的卡爾曼數據濾波算法. 最后也沒搞定.?
我發現凡事數據有異常的附近 一定會出現 FIFO overflow !?
找到到源代碼 DEBUG_PRINTLN(F(“FIFO overflow !”));?
打印這行代碼的條件是

if ((mpuIntStatus & 0x10) || fifoCount == 1024) {// reset so we can continue cleanlympu.resetFIFO();Serial.println(F("FIFO overflow!"));// otherwise, check for DMP data ready interrupt (this should happen frequently)}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

這個條件的意思是,如果mpuIntStatus 的狀態不對,或者讀取的數據長度等于 1024?
那么都會報這個錯誤.

我單純的注釋掉 DEBUG_PRINTLN(F(“FIFO overflow !”)); 是不行的.數據還是會周期性的異常.?
數據不正確,干啥都不行.?
百度了下,網上說,這個問題的根本原因是CPU讀取DMP的數據 速度太慢造成了 FIFO 緩沖器溢出.?
經過多方查資料,才知道,MPU6050 要想正確使用DMP必須將傳感器的12號引腳連接到CPU的外部中斷引腳上,?
使用中斷函數立即去讀取DMP的數據.就不會溢出了.

但是悲劇的情況是,MWCV2.5 電路板竟然沒有將MPU6050的12號針腳引出. 針腳特別小.我的焊工又不行.真想扔了重新買一塊.?
那怎么辦呢??
我想反正也能讀出來數據, 我拋棄掉異常數據,或者不讓異常數據產生就好了.?
我第一個方法是,在每次讀完數據以后就reset一下DMP輸出的FIFO數據.這樣就不會溢出了.緩沖區還是很大的.?
結果是不會溢出了. 但還是會有異常數據.?
于是我將DMP讀取的數據長度打印出來, 發現,正常的數據長度都是84或者126, 不正常的就各種長度.

經過我的仔細研究數據讀取代碼. 竟然發現了,我的數據為啥會異常的根本原因..

原因就是我沒用中斷去讀DMP的數據.而是循環不定時隨機讀的. 不同步的讀取勢必會有DMP計算到一部分,我這邊CPU去讀的情況.?
出現這種情況,CPU讀到一部分數據,數據不全. 肯定異常 現在問題明白了. .?
解決方法如下.?
如何才能判斷數據是讀完的呢?這個地方用了點小技巧.?
只要每次讀取數據能讀滿, 那么這次數據就是正常的.否則.就是不正常的數據..

見下面的代碼注釋

//鄭桂良發現的超級規則..... #ifdef DEBUGif (fifoCount % packetSize != 0 ) //840 是 packetSize * 20 的大小 packetSize是每次讀取數據的大小.{//mpu.resetFIFO(); 不用reset也行.//DEBUG_PRINTLN(F("數據讀取異常,未按節拍讀取,MPU6050 的 DMP 未計算完畢就讀取了,只讀取了一部分.造成后面的數據計算錯誤.")); //這種不同步異常造成的原因是MPU6050 的12號針腳 是一個對外發送中斷信號的針腳.當DMP計算完畢以后會改變這個針腳的電平.//需要將這個12針腳與單片機的外部中斷針腳相連.使用外部中斷函數立即去讀取DMP的數據. 才不會導致數據越堆越多.//但是悲劇的情況是,我的電路板竟然沒有將MPU6050的12號針腳引出....我的程序是循環讀.就出現這個異常了.return ; //放棄本次讀取,下次再讀.} #endif //鄭桂良發現的超級規則.....

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13

你只要簡單的將代碼改一下就可以了.把代碼

if ((mpuIntStatus & 0x10) || fifoCount == 1024) {// reset so we can continue cleanlympu.resetFIFO();Serial.println(F("FIFO overflow!"));// otherwise, check for DMP data ready interrupt (this should happen frequently)}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

改成

//鄭桂良發現的超級規則..... //840 是 packetSize * 20 的大小 /本來這里是 == 1024if ( (mpuIntStatus & 0x10) || ( fifoCount % packetSize != 0 ) || (fifoCount >= 840 )) //換成這個if條件就好了.{// reset so we can continue cleanlympu.resetFIFO(); //DEBUG_PRINTLN(F("FIFO overflow !")); //FIFO overflow 問題的本質就是，讀取數據速度不夠快導致FIFO溢出。//據說解決的方法就是，不能只用一個dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);函數。//我參考了據說“非常成功”的代碼，他們使用的庫都差不多，或者一樣，那么玄機就在如何加快讀取？//請各位大神幫幫忙，要不然我就直接讀取陀螺儀加速度計原始數據自己做數據融合了，但是我覺得INVsense公司做dmp必定有它的道理吧。//還是想用DMP讀取數據return 9; }

總結

以上是生活随笔為你收集整理的使用 MWC V2.5 中的 MPU6050中的DMP进行计算姿态（转载）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。