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

當(dāng)我從網(wǎng)上down來源代碼, 復(fù)制到我的程序的時(shí)候,發(fā)現(xiàn).?
DMP輸出的數(shù)據(jù)呈現(xiàn)周期性的數(shù)據(jù)異常. 數(shù)據(jù)波動(dòng)非常大. 這要是用在飛機(jī)上一定機(jī)毀人亡….?
期初我以為是干擾,研究了一個(gè)禮拜的卡爾曼數(shù)據(jù)濾波算法. 最后也沒搞定.?
我發(fā)現(xiàn)凡事數(shù)據(jù)有異常的附近 一定會(huì)出現(xiàn) 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

這個(gè)條件的意思是,如果mpuIntStatus 的狀態(tài)不對(duì),或者讀取的數(shù)據(jù)長(zhǎng)度等于 1024?
那么都會(huì)報(bào)這個(gè)錯(cuò)誤.

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

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

經(jīng)過我的仔細(xì)研究數(shù)據(jù)讀取代碼. 竟然發(fā)現(xiàn)了,我的數(shù)據(jù)為啥會(huì)異常的根本原因..

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

見下面的代碼注釋

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

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

你只要簡(jiǎn)單的將代碼改一下就可以了.把代碼

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

改成

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。