在原子F4的MPU6050的程序中,DMP得到欧拉角的过程是这样的(箭头表示调用函数):
-->mpu_dmp_get_data //DMP得到角度
-->dmp_read_fifo //DMP读取FIFO数据
-->mpu_read_fifo_stream
在最后一个函数中,有一个判断语句:
[mw_shl_code=c,true] 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) {
printf("FIFO full\t\n");
mpu_reset_fifo();
return -2;
}
}[/mw_shl_code]
可以看到,当FIFO大于50%的时候程序会清空FIFO,然后重新读取.若此时"重新读取"的到这里的时候又满了,就会无限循环这个过程.你可以直接注释掉这一段,但是可能以后读取的数据中会出现莫名其妙的值.更好的方式是降低DMP的输出速率.
把程序定位到DMP初始化部分,通过dmp_set_fifo_rate(DEFAULT_MPU_HZ)降低到50hz等;
[mw_shl_code=c,true]u8 mpu_dmp_init(void)
{
u8 res=0;
IIC_Init();
if(mpu_init()==0)
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ);
if(res)return 3;
res=dmp_load_motion_driver_firmware();
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置FIFO输出速率
if(res)return 7;
res=run_self_test();
if(res)return 8;
res=mpu_set_dmp_state(1);
if(res)return 9;
} [/mw_shl_code]
以下是我把速率设置到最高(200hz)的调试信息:
每一个行对应一次数据的读取,出现(stream)FIFO full表示读取失败(其实是50%full,程序清空FIFO重新读取).可以发现是比较频繁的,若设置为50,则读取失败的次数明显降低很多.我们可以通过在进入读取程序的时候写入调试信息,观测其FIFO情况.
因为程序使用的是while(mpu_dmp_get_data(&pitch,&roll,&yaw))不断的读取直到成功,所以出现死循环的原因可能是中断.
考虑一种情况,在你读取FIFO的过程中,出现一个中断,单片机处理其他事情后FIFO已经50%full了,于是清空管道重新读取,在第二次读取FIFO依旧出现这个中断,再次回来FIFO依旧是50%Full,于是出现了死循环.但是这只是经网友提醒后猜测的一种情况.并不清楚程序是否可能真的出现这种情况.
总之,假设你的程序一直卡在FIFO里面,可以根据自己写入的调试信息,观察FIFO数据情况,然后不断降低DMP输出速率,直到程序可以较好的运行.
比如在inv_mpu.c第26行:
[mw_shl_code=c,true]#define DEFAULT_MPU_HZ (50) //100Hz[/mw_shl_code]
将默认的100改为50.(单位hz)
|
/2 
|手机版|OpenEdv-开源电子网
( 粤ICP备12000418号-1 )
发表于 2015-9-14 16:34:07
发表于 2015-9-14 23:42:59
