低通滤波2

//---------------------------------------------------------------------------
//  Name     :        FILT_lLowPass
//                  ======================================
//
//  Function : Low-pass filter with two filter constants
//
//            Output = (FiltConst1 * (NewInput + LastInput)) + (FiltConst2 * LastOutput) 
//
//  Inputs    : As function arguments:
//             - FLOAT32 lNewInput              : Filter new input
//             - S_LOW_PASS_FILTER_TYPE* psFilt  : Pointer to filter parameters
// 
//  Output   : As return value:
//             - FLOAT32 lOutput                : Filtered output
//
//---------------------------------------------------------------------------

FLOAT32 FILT_lLowPass(S_LOW_PASS_FILTER_TYPE* psFilt, FLOAT32 fNewInput)
{
   FLOAT32 fOut1, fOut2, fOutput;   

   // Calculate filter output
   fOut1 = fNewInput+ psFilt->fLastInput;
   fOut1 = fOut1* psFilt->fFiltConst1;
   fOut2 = psFilt->fLastOutput* psFilt->fFiltConst2;
   fOutput = fOut1 + fOut2;
   
   // Update filter parameters
   psFilt->fLastInput = fNewInput;
   psFilt->fLastOutput = fOutput;

   return fOutput;
}

/*
Tcon单位：s , example 2k = 0.0005
fFlterTime单位：ms , example 100ms = 100
*/
const1 = Tcon/Tcon + 2.0f * fFltrTime *0.001f);

const2 = FLOAT_1_P_0 - FLOAT_2_P_0 *const1;

filterData = FILT_lLowPass(FILTER , newData);