156 lines
2.4 KiB
C
156 lines
2.4 KiB
C
|
||
#define _IN_FILTER_C
|
||
#include "filter.h"
|
||
|
||
//------------------------------------------------------
|
||
// 一阶滞后滤波器
|
||
#if (1)
|
||
|
||
int LPF1st(LPF1stData * pLpf1st, int newData)
|
||
{
|
||
int data = 0;
|
||
if (pLpf1st != NULL)
|
||
{
|
||
if (pLpf1st->f1value < 0)
|
||
{
|
||
pLpf1st->f1value = newData;
|
||
}
|
||
|
||
// 一阶滞后滤波
|
||
pLpf1st->f1value = newData * pLpf1st->lowpassxs + pLpf1st->f1value * (1-pLpf1st->lowpassxs);
|
||
data = pLpf1st->f1value;
|
||
}
|
||
|
||
return data;
|
||
}
|
||
|
||
#endif
|
||
|
||
// 二阶IIR滤波器
|
||
#if (1)
|
||
|
||
//------------------------------------------------------
|
||
|
||
int LPF2nd(LPF2ndData * pLpf2nd, int newData)
|
||
{
|
||
int data = 0;
|
||
|
||
if (pLpf2nd != NULL)
|
||
{
|
||
if (pLpf2nd->f2value < 0)
|
||
{
|
||
pLpf2nd->preout = newData;
|
||
pLpf2nd->lastout = newData;
|
||
}
|
||
|
||
// 对于二阶IIR滤波器，输出公式:Y（n)= b0*xn + b1*xn-1 + b2*xn-2 - (a1*xn-1 + a2*xn-2)
|
||
|
||
// 此处忽略了b1 * xn-1 + b2 * xn-2 ，b1和b2的系数为0
|
||
pLpf2nd->f2value = (newData * pLpf2nd->b0)+ (pLpf2nd->lastout * pLpf2nd->a1) - (pLpf2nd->preout * pLpf2nd->a2);
|
||
|
||
// 更新数值，留待下次运算
|
||
pLpf2nd->preout = pLpf2nd->lastout;
|
||
pLpf2nd->lastout = pLpf2nd->f2value;
|
||
data = pLpf2nd->lastout;
|
||
}
|
||
|
||
return data;
|
||
}
|
||
|
||
#endif
|
||
|
||
//------------------------------------------------------
|
||
|
||
// 加权递推平均滤波器
|
||
#if (1)
|
||
|
||
// 线性权值
|
||
int WRAFilter(int * pBuff, int len)
|
||
{
|
||
float value = 0;
|
||
int i;
|
||
int coe, coesum;
|
||
|
||
if (pBuff == NULL || len <= 0)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
coe = 1; // 权值从1开始
|
||
coesum = 0;
|
||
for (i = 0; i < len; i++)
|
||
{
|
||
value += *pBuff++ * coe; // 加权求和
|
||
coesum += coe;
|
||
coe++; // 权值增加，后面的数据权值大
|
||
}
|
||
value /= coesum; // 加权平均值
|
||
|
||
return value;
|
||
}
|
||
|
||
#endif
|
||
|
||
//------------------------------------------------------
|
||
|
||
// 中位值平均滤波器
|
||
#if (1)
|
||
|
||
int MAFilter(int * pBuff, int len)
|
||
{
|
||
int i;
|
||
s32 value = 0;
|
||
|
||
if (pBuff == NULL || len <= 0)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
if (len <= 5) // 数量较少，直接取平均值
|
||
{
|
||
// 取平均数
|
||
for (i = 0; i < len; i++)
|
||
{
|
||
value += *pBuff++;
|
||
}
|
||
value /= len;
|
||
}
|
||
else
|
||
{
|
||
s32 min, max, temp;
|
||
min = max = *pBuff; // 第一个值
|
||
|
||
// 取得所有值的累加和并得到最大最小值
|
||
for (i = 0; i < len; i++)
|
||
{
|
||
temp = *pBuff++;
|
||
value += temp; // 加上当前值
|
||
if (min > temp)
|
||
{
|
||
min = temp;
|
||
}
|
||
if (max < temp)
|
||
{
|
||
max = temp;
|
||
}
|
||
}
|
||
|
||
// 累加和减去最大最小值
|
||
value -= min;
|
||
value -= max;
|
||
|
||
// 剩余数据的平均值
|
||
value /= (len-2);
|
||
}
|
||
|
||
return value;
|
||
}
|
||
|
||
#endif
|
||
|
||
|
||
//------------------------------------------------------
|
||
|
||
//------------------------------------------------------
|
||
|