MPU6050 DMP调试出现FIFO溢出问题

在移植MPU6050_EMPL库时，使用 freerots 任务（5HZ频率） 进行数据采集，代码如下：

void Task_5HZ(void)
{
	
	//使用定义的结构体对象 OutMpu 直接从inv_mpu_user.c文件中获取数据
 	mpu_dmp_get_data();
	printf("x轴方向加速度为%d\r\n",OutMpu.acc_x);
	printf("y轴方向加速度为%d\r\n",OutMpu.acc_y);
	printf("z轴方向加速度为%d\r\n",OutMpu.acc_z);
	printf("x轴方向角速度为%d\r\n",OutMpu.gyro_x);
	printf("y轴方向角速度为%d\r\n",OutMpu.gyro_y);
	printf("z轴方向角速度为%d\r\n",OutMpu.gyro_z);
	printf("小车的俯仰角为%f\r\n",OutMpu.pitch);
	printf("小车的翻滚角为%f\r\n",OutMpu.roll);
	printf("小车的偏航角为%f\r\n",OutMpu.yaw);
	
}

mpu_dmp_get_data函数如下：

/***************************************************************************************************************
*函数名:mpu_dmp_get_data()
*功能:得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
*形参:(struct _out_angle *angle):DMP解算得到的姿态
*返回值：0：成功/1：DMP_FIFO读取失败/2:数据读取失败
***************************************************************************************************************/

float pitch,roll,yaw;
u8 mpu_dmp_get_data(void)
{
	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
	//short res=0;
	short gyro_dmp[3], accel_dmp[3], sensors_dmp;
	unsigned long sensor_timestamp;
	unsigned char more;
	long quat[4]; 

	if((dmp_read_fifo(gyro_dmp, accel_dmp, quat, &sensor_timestamp, &sensors_dmp,&more)))
		return 1;	
	if(sensors_dmp&INV_WXYZ_QUAT) 
	{
		q0 = quat[0] / q30;	//q30格式转换为浮点数
		q1 = quat[1] / q30;
		q2 = quat[2] / q30;
		q3 = quat[3] / q30; 
		//计算得到俯仰角/横滚角/航向角
		roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;																	// pitch
		pitch  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
		yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
	
	}else return 2;
	
	OutMpu.acc_x = accel_dmp[0];
	OutMpu.acc_y = accel_dmp[1];
	OutMpu.acc_z = accel_dmp[2];
	
	OutMpu.gyro_x = gyro_dmp[0];
	OutMpu.gyro_y = gyro_dmp[1];
	OutMpu.gyro_z = gyro_dmp[2];
	
	OutMpu.pitch = pitch;
	OutMpu.roll  = roll;
	OutMpu.yaw   = yaw;
	
	return 0;
}

通过 mpu_dmp_get_data 函数采集到的数据都为0

通过debug发现代码卡在dmp_read_fifo函数

	if((dmp_read_fifo(gyro_dmp, accel_dmp, quat, &sensor_timestamp, &sensors_dmp,&more)))
		return 1;	

再进入 dmp_read_fifo 函数中代码卡在mpu_read_fifo_stream函数

/**
 *  @brief      Get one unparsed packet from the FIFO.
 *  This function should be used if the packet is to be parsed elsewhere.
 *  @param[in]  length  Length of one FIFO packet.
 *  @param[in]  data    FIFO packet.
 *  @param[in]  more    Number of remaining packets.
 */
int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
    unsigned char *more)
{
    unsigned char tmp[2];
    unsigned short fifo_count;
    if (!st.chip_cfg.dmp_on)
        return -1;
    if (!st.chip_cfg.sensors)
        return -1;

    if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
        return -1;
    fifo_count = (tmp[0] << 8) | tmp[1];
    if (fifo_count < length) {
        more[0] = 0;
        return -1;
    }
    if (fifo_count > (st.hw->max_fifo >> 1)) {
        /* FIFO is 50% full, better check overflow bit. */
        if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
            return -1;
        if (tmp[0] & BIT_FIFO_OVERFLOW) {
            mpu_reset_fifo();
            return -2;
        }
    }

    if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))
        return -1;
    more[0] = fifo_count / length - 1;
    return 0;
}

最终停止在了

if (fifo_count > (st.hw->max_fifo >> 1)) {
        /* FIFO is 50% full, better check overflow bit. */
        if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
            return -1;
        if (tmp[0] & BIT_FIFO_OVERFLOW) {
            mpu_reset_fifo();
            return -2;
        }

需要调节MPU6050以及FREERTOS任务的频率，MPU6050最大速率为200HZ