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的数据处理代码，在一个定时器中断中添加的。但是添加之后，程序就会莫名其妙的跑飞，不管加哪，都会跑飞。我也不知道是程序问题还是