MPU6050移植读取数据时出现FIFO溢出问题解析

　　MPU-6000（6050）为全球首例整合性6轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时间轴之差的问题，减少了大量的封装空间。

　　MPU-6000（6050）的角速度全格感测范围为±250、±500、±1000与±2000°/sec （dps），可准确追踪快速与慢速动作，并且，用户可程式控制的加速器全格感测范围为±2g、±4g±8g与±16g。产品传输可透过最高至400kHz的IIC或最高达20MHz的SPI（MPU-6050没有SPI）。MPU-6000可在不同电压下工作，VDD供电电压介为2.5V±5%、3.0V±5%或3.3V±5%，逻辑接口VDDIO供电为1.8V± 5%（MPU6000仅用VDD）。MPU-6000的包装尺寸4x4x0.9mm（QFN），在业界是革命性的尺寸。其他的特征包含内建的温度感测器、包含在运作环境中仅有±1%变动的振荡器。

　　对问题做个简单的分析：为什么FIFO数据会溢出？MPU6050的DMP在工作的时候，其实大概的工作过程是mpu6050对陀螺仪和加速度计按照一定的采样速率进行采样，对采样得到的陀螺仪和加速度计数据通过DMP处理后得到姿态角（pitch，roll，yaw），然后存入FIFO中，这个过程在你初始化完DMP后就会不受人为控制的持续进行，那么如果你不及时读取FIFO的数据，FIFO数据很快就会溢出，那么就会出现以上的问题啦！

　　好了，说了那么一大堆，问题该如何解决呢？ 其实很简答啦，只要一个简单循环其实就解决了！看如下的程序：

　　while（mpu_dmp_get_data（&pitch，&roll，&yaw）！=0）{}

　　就是上面这么一句话，如果读取失败，马上进行第二次读取，这时候FIFO一般没有溢出，搞定！（因为检测到FIFO溢出后会马上reset一下FIFO）。

　　最后上一发图：

　　int dmp_read_fifo（short *gyro， short *accel， long *quat，

　　unsigned long *timestamp， short *sensors， unsigned char *more）

　　{

　　unsigned char fifo_data［MAX_PACKET_LENGTH］;

　　unsigned char ii = 0;

　　/* TODO： sensors［0］ only changes when dmp_enable_feature is called. We can

　　* cache this value and save some cycles.

　　*/

　　sensors［0］ = 0;

　　/* Get a packet. */

　　if （mpu_read_fifo_stream（dmp.packet_length， fifo_data， more））

　　return -1;

　　/* Parse DMP packet. */

　　if （dmp.feature_mask & （DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT）） {

　　#ifdef FIFO_CORRUPTION_CHECK

　　long quat_q14［4］， quat_mag_sq;

　　#endif

　　quat［0］ = （（long）fifo_data［0］ 《《 24） | （（long）fifo_data［1］ 《《 16） |

　　（（long）fifo_data［2］ 《《 8） | fifo_data［3］;

　　quat［1］ = （（long）fifo_data［4］ 《《 24） | （（long）fifo_data［5］ 《《 16） |

　　（（long）fifo_data［6］ 《《 8） | fifo_data［7］;

　　quat［2］ = （（long）fifo_data［8］ 《《 24） | （（long）fifo_data［9］ 《《 16） |

　　（（long）fifo_data［10］ 《《 8） | fifo_data［11］;

　　quat［3］ = （（long）fifo_data［12］ 《《 24） | （（long）fifo_data［13］ 《《 16） |

　　（（long）fifo_data［14］ 《《 8） | fifo_data［15］;

　　ii += 16;

　　#ifdef FIFO_CORRUPTION_CHECK

　　/* We can detect a corrupted FIFO by monitoring the quaternion data and

　　* ensuring that the magnitude is always normalized to one. This

　　* shouldn‘t happen in normal operation， but if an I2C error occurs，

　　* the FIFO reads might become misaligned.

　　*

　　* Let’s start by scaling down the quaternion data to avoid long long

　　* math.

　　*/

　　quat_q14［0］ = quat［0］ 》》 16;

　　quat_q14［1］ = quat［1］ 》》 16;

　　quat_q14［2］ = quat［2］ 》》 16;

　　quat_q14［3］ = quat［3］ 》》 16;

　　quat_mag_sq = quat_q14［0］ * quat_q14［0］ + quat_q14［1］ * quat_q14［1］ +

　　quat_q14［2］ * quat_q14［2］ + quat_q14［3］ * quat_q14［3］;

　　if （（quat_mag_sq 《 QUAT_MAG_SQ_MIN） ||

　　（quat_mag_sq 》 QUAT_MAG_SQ_MAX）） {

　　/* Quaternion is outside of the acceptable threshold. */

　　mpu_reset_fifo（）;

　　sensors［0］ = 0;

　　return -1;

　　}

　　sensors［0］ |= INV_WXYZ_QUAT;

　　#endif

　　}

　　if （dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL） {

　　accel［0］ = （（short）fifo_data［ii+0］ 《《 8） | fifo_data［ii+1］;

　　accel［1］ = （（short）fifo_data［ii+2］ 《《 8） | fifo_data［ii+3］;

　　accel［2］ = （（short）fifo_data［ii+4］ 《《 8） | fifo_data［ii+5］;

　　ii += 6;

　　sensors［0］ |= INV_XYZ_ACCEL;

　　}

　　if （dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO） {

　　gyro［0］ = （（short）fifo_data［ii+0］ 《《 8） | fifo_data［ii+1］;

　　gyro［1］ = （（short）fifo_data［ii+2］ 《《 8） | fifo_data［ii+3］;

　　gyro［2］ = （（short）fifo_data［ii+4］ 《《 8） | fifo_data［ii+5］;

　　ii += 6;

　　sensors［0］ |= INV_XYZ_GYRO;

　　}

　　/* Gesture data is at the end of the DMP packet. Parse it and call

　　* the gesture callbacks （if registered）。

　　*/

　　if （dmp.feature_mask & （DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT））

　　decode_gesture（fifo_data + ii）;

　　get_ms（timestamp）;

　　return 0;

　　}