陀螺仪加速度滤波

#define sample_freq 50.0  //50.0Hz

#define cutoff_freq  1.8      //3.0Hz

//X轴数据滤波器

int32_t LPF2_T2_X_Data(float xin,unsigned char First_EnterFlag)

{

   static float lpf2_yout[3] = {0};

   static float lpf2_xin[3] = {0};

   float ohm = tan(PI * cutoff_freq /sample_freq);

   float c = 1 + 1.414 * ohm  + ohm * ohm;

   float b0 = ohm * ohm /c;

   float b1 = 2.0f * b0;

   float b2 = b0;

   float a1 = 2.0f * (ohm * ohm - 1.0f) /c;

   float a2 = (1.0f - 1.414 * ohm + ohm * ohm) / c;

  

   if(First_EnterFlag == 1){

    lpf2_xin[0]=lpf2_xin[1]=lpf2_xin[2]=0;

    lpf2_yout[0]=lpf2_yout[1]=lpf2_yout[2]=0;

   }

   lpf2_xin[2] = xin;

   lpf2_yout[2] = b0 * lpf2_xin[2] + b1 * lpf2_xin[1] + b2 * lpf2_xin[0] - a1 * lpf2_yout[1] - a2 * lpf2_yout[0];

   lpf2_xin[0] = lpf2_xin[1];

   lpf2_xin[1] = lpf2_xin[2];

   lpf2_yout[0] = lpf2_yout[1];

   lpf2_yout[1] = lpf2_yout[2];

   return (int32_t)lpf2_yout[2];

}

//Y轴数据滤波器

int32_t LPF2_T2_Y_Data(float xin,unsigned char First_EnterFlag)

{

   static float lpf2_yout[3] = {0};

   static float lpf2_xin[3] = {0};

   float ohm = tan(PI * cutoff_freq /sample_freq);

   float c = 1 + 1.414 * ohm  + ohm * ohm;

  

   float b0 = ohm * ohm /c;

   float b1 = 2.0f * b0;

   float b2 = b0;

   float a1 = 2.0f * (ohm * ohm - 1.0f) /c;

   float a2 = (1.0f - 1.414 * ohm + ohm * ohm) / c;

  

   if(First_EnterFlag == 1){

    lpf2_xin[0]=lpf2_xin[1]=lpf2_xin[2]=0;

    lpf2_yout[0]=lpf2_yout[1]=lpf2_yout[2]=0;

   }

   lpf2_xin[2] = xin;

   lpf2_yout[2] = b0 * lpf2_xin[2] + b1 * lpf2_xin[1] + b2 * lpf2_xin[0] - a1 * lpf2_yout[1] - a2 * lpf2_yout[0];

   lpf2_xin[0] = lpf2_xin[1];

   lpf2_xin[1] = lpf2_xin[2];

   lpf2_yout[0] = lpf2_yout[1];

   lpf2_yout[1] = lpf2_yout[2];

   return (int32_t)lpf2_yout[2];

}

//Z轴数据滤波器

int32_t LPF2_T2_Z_Data(float xin,unsigned char First_EnterFlag)

{

   static float lpf2_yout[3] = {0};

   static float lpf2_xin[3] = {0};

   float ohm = tan(PI * cutoff_freq /sample_freq);

   float c = 1 + 1.414 * ohm  + ohm * ohm;