1.滤波器控制块
/*滑动平均滤波器——结构体*/
typedef struct SlipAveFilter
{
u16 len; /*窗口宽度*/
u16 index; /*索引*/
u16 has; /*已有的数据个数*/
char isfull; /*数组数据是否已满*/
float *data; /*滤波器数组*/
float sum; /*求和*/
float res; /*滤波结果*/
} SAFiter_t;
2.通过创建函数来创建一个滤波器,创建好之后,会返回一个句柄以供后续对滤波器操作。
/*
滑动平均滤波器——创建
len:滤波器的窗口宽度
返回创建的滤波器句柄
*/
SAFilterHandle_t SlipAveFilterCreate(u16 len)
{
if(len < 1)
{
len = 1;
}
//创建一个滤波器并初始化
SAFiter_t *newFilter;
newFilter = MALLOC(sizeof(SAFiter_t));
newFilter->data = MALLOC(len * sizeof(float));
newFilter->len = len;
newFilter->index = 0;
newFilter->has = 0;
newFilter->isfull = 0;
newFilter->sum = 0;
newFilter->res = 0;
return newFilter;
}
3.获取滤波结果
在获取滤波结果时,将之前创建的滤波器句柄作为参数传进来,实现对特定滤波数据的获取。
/*
滑动平均滤波器——获取结果
SAFiter:滤波器句柄
input:未滤波的原始数据
返回滤波结果
*/
float GetSAFiterRes(SAFilterHandle_t SAFiter, float input)
{
SAFiter_t *pFilter = (SAFiter_t *)SAFiter;
if(!pFilter->isfull) /*还没有存满*/
{
pFilter->has++; /*求当前数组中已有数据数量*/
if(pFilter->has == pFilter->len)
{
pFilter->isfull = 1; /*标记数组已满*/
}
}
else /*已存满,覆盖写入*/
{
pFilter->sum -= pFilter->data[pFilter->index]; /*先移除最早的数据*/
}
/*写入新的数据*/
pFilter->data[pFilter->index] = input;
/*求当前数组中已有数据的总和*/
pFilter->sum += input;
/*更新下次数据的索引号*/
pFilter->index = (pFilter->index == pFilter->len - 1) ? 0 : pFilter->index + 1;
/*求当前数组中已有数据的平均值*/
pFilter->res = pFilter->sum / pFilter->has;
return pFilter->res;
}
4.示例
//滤波器句柄
SAFilterHandle_t SAFilter_pitch;
SAFilterHandle_t SAFilter_roll;
//创建2个滤波器
SAFilter_pitch = SlipAveFilterCreate(10);
SAFilter_roll = SlipAveFilterCreate(10);
//循环获取数据并滤波
while(1)
{
if(mpu_dmp_get_data(&pitch,&roll,&yaw,&accx,&accy,&accz,&gyrox,&gyroy,&gyroz)==0)
{
pitch_res = GetSAFiterRes(SAFilter_pitch,pitch);//pitch数据滤波结果
roll_res = GetSAFiterRes(SAFilter_roll,roll);//roll数据滤波结果
printf("pitch:%f,%f,roll:%f,%f\r\n",pitch,pitch_res,roll,roll_res);//打印原始数据与滤波后的数据
}
vTaskDelay(1);
}