TI中文支持网
TI专业的中文技术问题搜集分享网站

TM4把陀螺仪的数据处理程序加入之后,程序跑飞

void MPU6050_Data_Prepare(float T)
{ u8 i;
s32 FILT_TMP[ITEMS] = {0,0,0,0,0,0,0};
// float auto_offset_temp[3];
float Gyro_tmp[3];
MPU6050_Data_Offset(); //У׼º¯Êý

/*¶ÁÈ¡bufferԭʼÊý¾Ý*/
mpu6050.Acc_I16.x = ((((int16_t)mpu6050_buffer[0]) << 8) | mpu6050_buffer[1]) ;
mpu6050.Acc_I16.y = ((((int16_t)mpu6050_buffer[2]) << 8) | mpu6050_buffer[3]) ;
mpu6050.Acc_I16.z = ((((int16_t)mpu6050_buffer[4]) << 8) | mpu6050_buffer[5]) ;
mpu6050.Gyro_I16.x = ((((int16_t)mpu6050_buffer[ 8]) << 8) | mpu6050_buffer[ 9]) ;
mpu6050.Gyro_I16.y = ((((int16_t)mpu6050_buffer[10]) << 8) | mpu6050_buffer[11]) ;
mpu6050.Gyro_I16.z = ((((int16_t)mpu6050_buffer[12]) << 8) | mpu6050_buffer[13]) ;

Gyro_tmp[0] = mpu6050.Gyro_I16.x ;//
Gyro_tmp[1] = mpu6050.Gyro_I16.y ;//
Gyro_tmp[2] = mpu6050.Gyro_I16.z ;//

// mpu6050.Tempreature = ((((int16_t)mpu6050_buffer[6]) << 8) | mpu6050_buffer[7]); //tempreature
// mpu6050.TEM_LPF += 2 *3.14f *T *(mpu6050.Tempreature – mpu6050.TEM_LPF);
// mpu6050.Ftempreature = mpu6050.TEM_LPF/340.0f + 36.5f;

//======================================================================
if( ++filter_cnt > FILTER_NUM ) {
filter_cnt = 0;
filter_cnt_old = 1;
}
else
{
filter_cnt_old = (filter_cnt == FILTER_NUM)? 0 : (filter_cnt + 1);
}
//10 170 4056
/* µÃ³öУ׼ºóµÄÊý¾Ý */
mpu6050_tmp[A_X] = (mpu6050.Acc_I16.x – mpu6050.Acc_Offset.x) ;
mpu6050_tmp[A_Y] = (mpu6050.Acc_I16.y – mpu6050.Acc_Offset.y) ;
mpu6050_tmp[A_Z] = (mpu6050.Acc_I16.z – mpu6050.Acc_Offset.z) ;
mpu6050_tmp[G_X] = Gyro_tmp[0] – mpu6050.Gyro_Offset.x ;//
mpu6050_tmp[G_Y] = Gyro_tmp[1] – mpu6050.Gyro_Offset.y ;//
mpu6050_tmp[G_Z] = Gyro_tmp[2] – mpu6050.Gyro_Offset.z ;//

/* ¸üÐÂÂ˲¨»¬¶¯´°¿ÚÊý×é */
FILT_BUF[A_X][filter_cnt] = mpu6050_tmp[A_X];
FILT_BUF[A_Y][filter_cnt] = mpu6050_tmp[A_Y];
FILT_BUF[A_Z][filter_cnt] = mpu6050_tmp[A_Z];
FILT_BUF[G_X][filter_cnt] = mpu6050_tmp[G_X]; FILT_BUF[G_Y][filter_cnt] = mpu6050_tmp[G_Y];
FILT_BUF[G_Z][filter_cnt] = mpu6050_tmp[G_Z];

for(i=0;i<FILTER_NUM;i++)
{
FILT_TMP[A_X] += FILT_BUF[A_X][i];
FILT_TMP[A_Y] += FILT_BUF[A_Y][i];
FILT_TMP[A_Z] += FILT_BUF[A_Z][i];
FILT_TMP[G_X] += FILT_BUF[G_X][i];
FILT_TMP[G_Y] += FILT_BUF[G_Y][i];
FILT_TMP[G_Z] += FILT_BUF[G_Z][i];
}

mpu_fil_tmp[A_X] = (float)( FILT_TMP[A_X] )/(float)FILTER_NUM;
mpu_fil_tmp[A_Y] = (float)( FILT_TMP[A_Y] )/(float)FILTER_NUM;
mpu_fil_tmp[A_Z] = (float)( FILT_TMP[A_Z] )/(float)FILTER_NUM;

mpu_fil_tmp[G_X] = (float)( FILT_TMP[G_X] )/(float)FILTER_NUM;
mpu_fil_tmp[G_Y] = (float)( FILT_TMP[G_Y] )/(float)FILTER_NUM;
mpu_fil_tmp[G_Z] = (float)( FILT_TMP[G_Z] )/(float)FILTER_NUM;
/*×ø±êת»»*/
Transform(mpu_fil_tmp[A_X],mpu_fil_tmp[A_Y],mpu_fil_tmp[A_Z],&mpu6050.Acc.x,&mpu6050.Acc.y,&mpu6050.Acc.z);
Transform(mpu_fil_tmp[G_X],mpu_fil_tmp[G_Y],mpu_fil_tmp[G_Z],&mpu6050.Gyro.x,&mpu6050.Gyro.y,&mpu6050.Gyro.z);

mpu6050.Gyro_deg.x = mpu6050.Gyro.x *TO_ANGLE;
mpu6050.Gyro_deg.y = mpu6050.Gyro.y *TO_ANGLE;
mpu6050.Gyro_deg.z = mpu6050.Gyro.z *TO_ANGLE;

//======================================================================
}

这是我添加的6050的数据处理代码,在一个定时器中断中添加的。但是添加之后,程序就会莫名其妙的跑飞,不管加哪,都会跑飞。我也不知道是程序问题还是

xyz549040622:

基本常识,定时器中不要进行浮点运算。不过这个TM4中带有浮点运算单元,你的FPU开启了没有,如果开启也还是不行的话,中断中还是不要进行浮点运算了。

rongwei li:

回复 xyz549040622:

中断中不能 浮点运算,不会吧,   四轴的姿态解算就在定时器中断中进行的 大部分都是浮点运算,按你这样说, 四轴还搞不搞了

rongwei li:

跑飞,不知道 你具体是什么情况,我写的程序 角度可以解算出来,但是在计算 静偏值offset时 浮点运算 ,(imu.accOffset[i]=accSum[i]/(float)(cnt);)  offset 经常显示1,#QNAN,  我就怀疑 TM4C123g的浮点运算有问题(程序从stm32移植过来的),我就没有开启浮点运算

xyz549040622:

回复 rongwei li:

不让使用浮点,是怕执行时间太长而引起中断的错误。那么你单步执行,看看是否是因为优化而引起的运算问题呢?单看程序,看不出什么问题了。

赞(0)
未经允许不得转载:TI中文支持网 » TM4把陀螺仪的数据处理程序加入之后,程序跑飞
分享到: 更多 (0)