mpu_dmp_get_data的返回值一直是1，为什么主函数中还是能进入if语句。
造成返回值是1的原因我用J-Link调试的结果是下面这种情况。。请看代码中的红色部分
好像代码中加不了红色。。大家就看<span style="color:#E53333;">后面的话吧。
[mw_shl_code=c,true] if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)//返回值是1，但if语句还是能进去。这是问题一？ {
temp=MPU_Get_Temperature(); //得到温度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);//用自定义帧发送加速度和陀螺仪原始数据
if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
if((t%10)==0)
{
if(temp<0)
{
LCD_ShowChar(30+48,200,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,200,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,200,temp/100,3,16,0); //显示整数部分
LCD_ShowNum(30+48+40,200,temp%10,1,16,0); //显示小数部分 [/mw_shl_code]
[mw_shl_code=c,true][/mw_shl_code]
[mw_shl_code=c,true][/mw_shl_code]
[mw_shl_code=c,true]u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1; //dmp_read_fifo的返回值为-1使得mpu_dmp_get_data的返回值为1。也就是说下面的计算根本就没执行。[/mw_shl_code]
[mw_shl_code=c,true] 但在液晶屏上显示的结果像是对的。这是问题二？ if(sensors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//计算得到俯仰角/横滚角/航向角
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = 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;
return 0;
}[/mw_shl_code]
[mw_shl_code=c,true]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; //程序执行到这退出，返回了-1。mpu_read_fifo_stream的返回值为-2。下面的语句也是没有执行的。 /* 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;[/mw_shl_code]
[mw_shl_code=c,true]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; 调试的结果是程序在这退出，返回了-2 。正确的不应该是执行到最下面返回0吗 }
}
if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))
return -1;
more[0] = fifo_count / length - 1;
return 0;
}[/mw_shl_code]
我用的是原子哥的源程序，没做什么改动。是不是哪里的配置问题？
不知道有没有人遇到和我一样的困惑？
求大神解惑。。
|
/2 
|手机版|OpenEdv-开源电子网
( 粤ICP备12000418号-1 )
发表于 2015-4-8 17:49:47
楼主
发表于 2015-4-8 22:32:42
