四旋翼无人机从0到1的实现（二十二）无人机工程中滤波

Author：家有仙妻谢掌柜
Date：2021/2/18

今年会更新一个系列，小四轴无人机从功能设计→思维导图→原理图设计→PCBLayout→焊接PCB→程序代码的编写→整机调试一系列，以此记录自己的成长历程！
这个小四轴无人机是大学时期学习制作的，加上现在工作学习对嵌入式的理解更加深入，因此想要重新梳理一下小四轴，之后在此基础上实现大四轴的飞控设计，这些都将在工作之余完成！

//小四轴无人机设计中涉及到滤波器

#include "filter.h"
/*******************************************************************************
 * fuction	biquadFilterInit
 * brief	低通滤波器滤波系数计算
 * param	滤波器 截止频率 刷频率 品质因数
 * return	无
 * 二阶IIR滤波器系统函数表达式：H(Z) = (b0+b1*Z^-1+b2*Z^-2)/(a0+a1*Z^-1+a2*Z^-2)
 *******************************************************************************/  
void biquadFilterInit(biquadFilter_t *filter,uint32_t filterFreq,uint32_t refreshRate,float Q)
{
    
    
	const float SampleRate =  1/((float)refreshRate*0.000001f);
	const float omega = 2* _PI_*filterFreq/SampleRate;
	const float sn = sinf(omega);
	const float cs = cosf(omega);
	//const float alpha = sinf(2*Q);
	const float alpha = sn/(2*Q);         //   对二阶低通滤波参数进行优化
	float b0 = 0,b1=0,b2=0,a0=0,a1=0,a2=0;
	b0 = (1-cs)/2;
	b1 = 1-cs;
	b2 = (1-cs)/2;
	a0 = 1+ alpha;
	a1 = -2*cs;
	a2 = 1-alpha;
	filter->b0 = b0/a0;
	filter->b1 = b1/a0;
	filter->b2 = b2/a0;
	filter->a1 = a1/a0;
	filter->a2 = a2/a0;
	filter->d1 = 0;
	filter->d2 = 0;
}
/*******************************************************************************
 * fuction	biquadFilterApply
 * brief	低通滤波器  滤波运用滤波前的值
 * param	滤波器
 * return	滤波运用滤波后的值
 * 二阶IIR滤波器系统函数表达式：Y(n) = b0x(n)+b1x(n-1)+b2x(n-2)-a1y(n-1)-a2y(n-2)
 *******************************************************************************/ 
float biquadFilterApply(biquadFilter_t *filter,float input)
{
    
    
	const float result = filter->b0*input + filter->d1;
	filter->d1 = filter->b1 * input - filter->a1*result + filter->d2;
	filter->d2 = filter->b2*input - filter->a2*result;
	return result;
}
/* 作为陀螺仪滤波  陀螺仪有三个轴 加速度有三个  总共有6个轴的数据都是需要滤波 */
/* 需要调用6次滤波器    每调用一次，滤波器的值会发生变化 */  
// 由于系数全部封装在一个结构体中，每一次d1 和d2都发生变化，则结构体只能调用一次
// 解决结构体只能调用一次的矛盾
// 需要再建立新的结构体，让其值在每一个周期里面保持一样
/*******************************************************************************
 * fuction	filterValuation
 * brief	获取滤波值
 * param	输入 输出
 * return	无
 * 二阶IIR滤波器系统函数表达式：Y(n) = b0x(n)+b1x(n-1)+b2x(n-2)-a1y(n-1)-a2y(n-2)
 *******************************************************************************/ 
void filterValuation(biquadFilter_t *filterInput,biquadFilter_t *filterOutput)
{
    
    
	filterOutput->a1 = filterInput->a1;
	filterOutput->a2 = filterInput->a2;
	filterOutput->b0 = filterInput->b0;
	filterOutput->b1 = filterInput->b1;
	filterOutput->b2 = filterInput->b2;
	filterOutput->d1 = filterInput->d1;
	filterOutput->d2 = filterInput->d2;
}
/*******************************************************************************
 * fuction	buttworth_flier_parameter
 * brief	低通滤波器的系数求解
 * param	无
 * return	无
 *******************************************************************************/ 
void buttworth_flier_parameter(void)
{
    
    
	/*陀螺仪低通滤波器系数计算	*/ 
	biquadFilterInit(&gryo_51Hz_parameter1,51, 1000, BIQUAD_Q);
	filterValuation(&gryo_51Hz_parameter1,&gryo_51Hz_parameter2);
	filterValuation(&gryo_51Hz_parameter1,&gryo_51Hz_parameter3);
	/*陀螺仪有X Y Z三个轴，三个轴都要进行低通滤波，需要三个结构体系数，相等	*/ 
	//  biquadFilter_t gryo_51Hz_parameter1,gryo_51Hz_parameter2,gryo_51Hz_parameter3;
	//  biquadFilter_t acc_30Hz_parameter1,acc_30Hz_parameter2,acc_30Hz_parameter3;
	/*加速度计低通滤波器系数计算	*/ 
	biquadFilterInit(&acc_30Hz_parameter1,30, 1000, BIQUAD_Q);
	filterValuation(&acc_30Hz_parameter1,&acc_30Hz_parameter2);
	filterValuation(&acc_30Hz_parameter1,&acc_30Hz_parameter3);
}

#ifndef _FILTER_H__
#define _FILTER_H__

#include "board_define.h"
#include "var_global.h"

#define _PI_  3.14159265358979323846f
#define BIQUAD_Q  1.0f/sqrtf(2.0f)
void biquadFilterInit(biquadFilter_t *filter,uint32_t filterFreq,uint32_t refreshRate,float Q);
float biquadFilterApply(biquadFilter_t *filter,float input);
void filterValuation(biquadFilter_t *filterInput,biquadFilter_t *filterOutput);
void buttworth_flier_parameter(void);

#endif