滤波函数

Posted 2021-03-15 zhangfengfly

仅供自己学习使用：
/********************滤波结构体定义*****************/
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 即为滤波后的值。