仅供自己学习使用：
/********************滤波结构体定义*****************/
typedef struct
{
ulong Out32L; /* The filtered value multiplied by coeff */
ulong Output; /* The filtered value */
ulong MinGap; /* The minimum interval value to filter */
ushort Coeff; /* The coefficent filter */
} FLTS_1stOrderFilter23_UInt32_t;
typedef struct
{
double Out32L; /* The filtered value multiplied by coeff */
double Output; /* The filtered value */
double MinGap; /* The minimum interval value to filter */
ushort Coeff; /* The coefficent filter */
} FLTS_1stOrderFilter23_Double_t;
typedef struct
{
float Out32L; /* The filtered value multiplied by coeff */
float Output; /* The filtered value */
float MinGap; /* The minimum interval value to filter */
ushort Coeff; /* The coefficent filter */
} FLTS_1stOrderFilter23_Float_t;
/*******************结构体初始化函数******************/
template<typename T1,typename T2>
void FLTS_Init1stOrderFilter23bits(FLTS_1stOrderFilter23_UInt32_t *Filter, T1 InitValue, T2 Coeff, T1 MinGap)
{
Filter->Output = InitValue;
Filter->Out32L = InitValue * Coeff;
Filter->Coeff = Coeff;
Filter->MinGap = MinGap;
}
/******************************************************************************/
/*Name : FLTS_1stOrderFilter23bits */
/*Role : Filter a value with a first order filter */
/*Interface : ValueToFilter IN Value to filter (Vin=(0 - (2^23-1))) */
/* FilterCoef IN Coefficient Filter */
/* FilteredValue IN/OUT The filtered value at time t-1 and t */
/* */
/*Pre-condition : Before 1st call, you have to initialize the struct as below */
/* FilterValue.Out32L = InitValue * FilterValue.Coeff */
/* done by FLTS_Init1stOrderFilter23bits(.) */
/*Constraints : */
/* Filtered Angle at its final value using a 1st order never reach its final */
/* value. It could have 1 unit between requested value and filtered value */
/* that because we use 1 "1st filter", and the final value of 1st filter */
/* could be 1 unit less or more than its requested value */
/* Use minimum gap to avoid 1 unit deviation. When gap between requested value*/
/* and filtered value is less than MinGap, filtered value is set to requested */
/* value. */
/******************************************************************************/
/* PROC FLTS_1stOrderFilter23bits */
/* ( */
/* IN The Rest of the previous value */
/* ) */
/* DO */
/* ValueToFilter - OldFilteredValue */
/* FilteredValue := ----------------------------- + OldFilteredValue */
/* Coef */
/* OD */
/******************************************************************************/
/*unsigned 32-bit int*/
template<typename T>
void FLTS_1stOrderFilter23bits(T ValueToFilter, FLTS_1stOrderFilter23_UInt32_t *FilterValue)
{
if (ValueToFilter > (FilterValue->Output + FilterValue->MinGap))
{
FilterValue->Out32L =
((((ValueToFilter)* FilterValue->Coeff) - FilterValue->Out32L)
/ FilterValue->Coeff)
+ FilterValue->Out32L;
}
else if ((ValueToFilter + FilterValue->MinGap) < FilterValue->Output)
{
FilterValue->Out32L =
FilterValue->Out32L -
((FilterValue->Out32L - ((ValueToFilter)* FilterValue->Coeff))
/ FilterValue->Coeff);
}
else
{
FilterValue->Out32L = ValueToFilter * FilterValue->Coeff;
}
FilterValue->Output = (T)(FilterValue->Out32L / FilterValue->Coeff);
}
接口使用：
1.定义结构体变量
eg.
FLTS_1stOrderFilter23_UInt32_t value;
2.调用结构体初始化函数进行初始化
eg.
FLTS_Init1stOrderFilter23bits(&value, 0, 5, 1);
3.调用滤波函数进行滤波
eg.
FLTS_1stOrderFilter23bits(m_DisplayValue, &value);
m_OutValue = value.Output;
comment：
接口中传入m_DisplayValue为滤波前的值。m_OutValue 即为滤波后的值。