之前的几篇文章对FreeRTOS的部分源码进行了分析，可以发现FreeRTOS对于任务、事件标志组、消息队列等的实现都是通过控制块的方式来操作。
比如任务(FreeRTOS源码探析之——任务调度相关)有任务控制块TCB_t，
事件标志组(FreeRTOS源码探析之——事件标志组)有事件控制块EventGroup_t，消息队列(FreeRTOS源码探析之——消息队列)有消息队列控制块Queue_t，
软件定时器(FreeRTOS源码探析之——软件定时器)有软件定时器控制块Timer_t。
使用它们前都是先创建（内存分配资源），返回一个控制块的句柄，之后就可以通过句柄来操作这个对象了。
1
基本原理
以FreeRTOS的这种机制为参考，我们可以模仿着实现一个简单的滤波器，可以用于对传感器的数据进行滤波。
先来分析一下基本原理：
- 设定我们的滑动平均滤波器的窗口宽度为5，可以使用一个数组来实现
- 使用一个index来指示下次数据将要存放在数组中的位置
- 初始状态，滤波器数组都是0，index指向数组的起始位置
滤波器是初始状态如下图
开始阶段
原始数据依次存入滤波器数组，这时的滤波器输出有两种选择：
- 方式1：既然数组还未存满，就先不输出滤波结果
- 方式2：虽然数组还未存满，但可以计算已经存入的这几个数的平均值作为滤波输出
这两种方式只在初始阶段存在差别，本文后续编码将采用方式2。
数据第1次存满滤波器数组
数据继续存入滤波器数组，当恰好存满时，就可以计算整个数组的平均值了，作为此次的滤波结果。
滑动存储阶段
滤波器数组首次存满后，就需要进行覆盖存储了(实现滑动获取数据的效果)。
这里也有两种方式：
- 方式1：将新的数据(a5)覆盖存入最早的数据(a0)，然后遍历数组求和，再求平均值作为输出
- 方式2：借用上次的求和数据
sum(a0~a4)，将sum先减去最早的数据(a0)，加上新的数据(a5)，再将新的数据(a5)覆盖存入最早的数据(a0)，最后对sum除以5求平均值作为输出
相比较而言，方式2的计算量更小，本文后续编码将采用方式2。
2
编码实现
下面来看一下编码实现：
滤波器控制块
参考FreeRTOS的设计方式，为我们的滑动平均滤波器设计一个控制块，也就是一个结构体，该结构体包含滤波器所需要的资源。
代码语言：javascript复制/*滑动平均滤波器——结构体*/
typedef struct SlipAveFilter
{
u16 len; /*窗口宽度*/
u16 index; /*索引*/
u16 has; /*已有的数据个数*/
char isfull; /*数组数据是否已满*/
float *data; /*滤波器数组*/
float sum; /*求和*/
float res; /*滤波结果*/
} SAFiter_t;创建滤波器
参考FreeRTOS的设计方式，使用滤波器时，通过创建函数来创建一个滤波器，创建好之后，会返回一个句柄以供后续对滤波器操作。
代码语言：javascript复制/*
滑动平均滤波器——创建
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;
}获取滤波结果
参考FreeRTOS的设计方式，在获取滤波结果时，将之前创建的滤波器句柄作为参数传进来，实现对特定滤波数据的获取。
代码语言：javascript复制/*
滑动平均滤波器——获取结果
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;
}3
使用示例
以MPU6050陀螺仪的数据滤波为例(相关介绍参考MPU6050姿态解算方式1-DMP)，假设需要对pitch数据和roll数据进行滤波，使用方式为：
- 定义2滤波器句柄
- 创建2滤波器
- 循环获取数据并滤波
//滤波器句柄
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,%frn",pitch,pitch_res,roll,roll_res);//打印原始数据与滤波后的数据
}
vTaskDelay(1);
}滤波后的结果如下，蓝色为原始数据，橙色为滤波后的结果：
