使用 MWC V2.5 中的 MPU6050中的DMP进行计算姿态（转载）

玩四轴的都知道, 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 cleanly        mpu.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 DEBUG    if (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 cleanly        mpu.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 的大小 /本来这里是 == 1024  if (         (mpuIntStatus & 0x10) ||         ( fifoCount  % packetSize != 0 ) ||                         (fifoCount >= 840 )     )        //换成这个if条件就好了.    {        // reset so we can continue cleanly        mpu.resetFIFO();          //DEBUG_PRINTLN(F("FIFO overflow !"));         //FIFO overflow 问题的本质就是，读取数据速度不够快导致FIFO溢出。        //据说解决的方法就是，不能只用一个dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);函数。        //我参考了据说“非常成功”的代码，他们使用的库都差不多，或者一样，那么玄机就在如何加快读取？        //请各位大神帮帮忙，要不然我就直接读取陀螺仪加速度计原始数据自己做数据融合了，但是我觉得INVsense公司做dmp必定有它的道理吧。        //还是想用DMP读取数据        return 9;     }