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