获取滤波结果

参考FreeRTOS的设计方式，在获取滤波结果时，将之前创建的滤波器句柄作为参数传进来，实现对特定滤波数据的获取。

/*滑动平均滤波器——获取结果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,%f\r\n",pitch,pitch_res,roll,roll_res);//打印原始数据与滤波后的数据    }    vTaskDelay(1);}

滤波后的结果如下，蓝色为原始数据，橙色为滤波后的结果：