BetaFlight模块设计之二十九：滤波模块分析

基于BetaFlight开源代码框架简介的框架设计，逐步分析内部模块功能设计。

滤波模块

前面分析BetaFlight模块设计主要基于其任务代码框架来展开，因为这样从理解以及代码上来说更容易切入。随着总体任务的熟悉，其实每个任务细节部分都是需要软件模块来逐一完成的。这里我们主要针对BetaFlight的滤波函数进行整理归纳，期望得到一个相对完成的模块分析，同时也对这里的历史问题展开一定的讨论。有兴趣的朋友可以留言或者联系我，可以深入探讨！

GYRO_FILTER_FUNCTION_NAME是一个整体的数据滤波实现，实际在代码中因宏定义差别会有debug和release版本。从概念及逻辑的角度来说都一样，debug会有一些开发人员感兴趣的打点位置。

关于引入这个函数的任务，详见BetaFlight模块设计之十三：Gyro过滤任务分析

注：有一点需要注意的，数字滤波必定导致数据延迟。

GYRO_FILTER_FUNCTION_NAME
 ├──> <loop axis> // downsample the individual gyro samples, XYZ_AXIS_COUNT=3
 │   ├──> <gyro.downsampleFilterEnabled> // using gyro lowpass 2 filter for downsampling
 │   │   └──> gyroADCf = gyro.sampleSum[axis];
 │   ├──> <!gyro.downsampleFilterEnabled> // using simple average for downsampling
 │   │   └──> gyroADCf = gyro.sampleSum[axis] / gyro.sampleCount;
 │   ├──> <USE_RPM_FILTER>  //主要用于过滤电机带来的扰动，后续可以在黑匣子数据上分析(gyro/motor随油门的频谱图)
 │   │   └──> rpmFilterGyro
 │   ├──> // apply static notch filters and software lowpass filters
 │   │   ├──> gyroADCf = gyro.notchFilter1ApplyFn((filter_t *)&gyro.notchFilter1[axis], gyroADCf); // 一阶带通
 │   │   ├──> gyroADCf = gyro.notchFilter2ApplyFn((filter_t *)&gyro.notchFilter2[axis], gyroADCf); // 二阶带通
 │   │   └──> gyroADCf = gyro.lowpassFilterApplyFn((filter_t *)&gyro.lowpassFilter[axis], gyroADCf); // 低通滤波
 │   ├──> <USE_DYN_NOTCH_FILTER><isDynNotchActive()>
 │   │   ├──> dynNotchPush(axis, gyroADCf);
 │   │   ├──> gyroADCf = dynNotchFilter(axis, gyroADCf);  //动态带通滤波
 │   └──> gyro.gyroADCf[axis] = gyroADCf;
 └──> gyro.sampleCount = 0;

gyro_filter_impl.c
 ├──> rpm_filter.c/h
 ├──> filter.c/h
 ├──> dyn_notch.c/h
 └──> dyn_notch_filter.c/h

滤波类型

1. 【单一滤波方法】slewFilter
2. 【单一滤波方法】simpleLowpassFilter
3. 【单一滤波方法】laggedMovingAverage
4. 【单一滤波方法】pt1Filter
5. 【单一滤波方法】pt2Filter
6. 【单一滤波方法】pt3Filter
7. 【支持多种滤波方法】 biquadFilter
8. 【支持多种滤波混合】dynNotchFilter
9. 【支持多种滤波混合】rpmFilter

1. slewFilter

中文翻译：回转过滤器

起作用就是当input参数超过threshold，过滤掉变化量不大于slewLimit的input(正反向等效)。

typedef struct slewFilter_s {
    float state;
    float slewLimit;
    float threshold;
} slewFilter_t;

void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
float slewFilterApply(slewFilter_t *filter, float input);

2. simpleLowpassFilter

中文翻译：简单低通滤波器

Low-pass_filter的概念来自物理学基尔霍夫定律。通常滤波需要采用否点运算方法，如下表达式：

// LPF: Y(n) = (1-ß)*Y(n-1) + (ß*X(n))) = Y(n-1) - (ß*(Y(n-1)-X(n)));
// ß = 0.125

而在很多不带浮点运算协处理的CPU，尤其是MCU上，我们需要简化算法(尽量采用整形的移位)，详见Fixed-point_arithmetic。

这里的重点是将变量范围尽量放在合适的区间(Fixed Point/Range)，以免丢失一些细节。BetaFlight的simpleLowpassFilter就是采用这种处理方式。

对细节比较纠结的朋友可以看下以下两个链接，会更加容易理解！
【1】A simple digital low-pass filter in C
【2】A lowpass filter algorithm for small controllers without multiplier
【3】Fixed Point Scaling – Low Pass Filter example

typedef struct simpleLowpassFilter_s {
    int32_t fp;
    int32_t beta;
    int32_t fpShift;
} simpleLowpassFilter_t;

int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal);
void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift);

3. laggedMovingAverage

中文翻译：滞后移动平均

lagged Moving Average filter 主要基于之前的数据，做线性平滑，去除类似白噪声等干扰。

typedef struct laggedMovingAverage_s {
    uint16_t movingWindowIndex;
    uint16_t windowSize;
    float movingSum;
    float *buf;
    bool primed;
} laggedMovingAverage_t;

void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);

4. pt1Filter

来自Low-pass_filter公式：

$$y_i=x_i\stackrel{\text{Input Contribution}}{(\frac{\Delta T}{RC+\Delta T})}+y_{i-1}\stackrel{\text{Inertia from previous Output}}{(\frac{RC}{RC+\Delta T})}$$

其中pt1FilterGain函数根据下面表达式解决pt1FilterInit和pt1FilterUpdateCutoff的k值。

$$RC=\frac{1}{2\pi f_c}$$

$$\alpha =\frac{\Delta T}{RC+\Delta T}$$

typedef struct pt1Filter_s {
    float state;
    float k;
} pt1Filter_t;

float pt1FilterGain(float f_cut, float dT);
void pt1FilterInit(pt1Filter_t *filter, float k);
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
float pt1FilterApply(pt1Filter_t *filter, float input);

5. pt2Filter

来自Passive Low Pass RC Filters公式：

$$f_{(-3dB)}=f_c\sqrt{2^{(1/2)}-1}$$

$$RC=\frac{1}{2\pi f_{(-3dB)}}$$

typedef struct pt2Filter_s {
    float state;
    float state1;
    float k;
} pt2Filter_t;

float pt2FilterGain(float f_cut, float dT);
void pt2FilterInit(pt2Filter_t *filter, float k);
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k);
float pt2FilterApply(pt2Filter_t *filter, float input);

6. pt3Filter

与pt2Filter类似，当n=3时，就是三阶RC滤波。

$$f_{(-3dB)}=f_c\sqrt{2^{(1/n)}-1}$$

$$RC=\frac{1}{2\pi f_{(-3dB)}}$$

typedef struct pt3Filter_s {
    float state;
    float state1;
    float state2;
    float k;
} pt3Filter_t;

float pt3FilterGain(float f_cut, float dT);
void pt3FilterInit(pt3Filter_t *filter, float k);
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
float pt3FilterApply(pt3Filter_t *filter, float input);

7. biquadFilter

7.1 biquad类型

biquad Filter在BetaFlight代码实现上，有三种类型：

1. 低通滤波(FILTER_LPF)
2. 高通滤波(FILTER_BPF)
3. 带通滤波(FILTER_NOTCH)

typedef enum {
    FILTER_LPF,    // 2nd order Butterworth section
    FILTER_NOTCH,
    FILTER_BPF,
} biquadFilterType_e;

注：提供一个可以看波形的链接，感受下不同类型的效果图。

7.2 biquad滤波方式

来自Digital_biquad_filter。

• biquadFilterApplyDF1采用Direct form 1
• biquadFilterApplyDF1Weighted采用Direct form 1 + Weight(类似simpleLowpassFilter)
  注：Wiki中描述到“higher-order filters are typically implemented as serially-cascaded biquad sections (and a first-order filter if necessary)”。
• biquadFilterApply采用Transposed direct form 2

Direct form 1

Transposed direct form 2

7.3 biquad滤波接口

/* this holds the data required to update samples thru a filter */
typedef struct biquadFilter_s {
    float b0, b1, b2, a1, a2;
    float x1, x2, y1, y2;
    float weight;
} biquadFilter_t;

void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);

float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApplyDF1Weighted(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
float filterGetNotchQ(float centerFreq, float cutoffFreq);

8. dynNotchFilter

其采用biquadFilter + FILTER_NOTCH(带通滤波) + biquadFilterApplyDF1(Direct form 1)的方式对gyro数据进行带通滤波。主要应用在BetaFlight模块设计之十三：Gyro过滤任务分析。

typedef struct dynNotchConfig_s
{
    uint16_t dyn_notch_min_hz;
    uint16_t dyn_notch_max_hz;
    uint16_t dyn_notch_q;
    uint8_t  dyn_notch_count;

} dynNotchConfig_t;

void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeUs);
void dynNotchPush(const int axis, const float sample);
void dynNotchUpdate(void);
float dynNotchFilter(const int axis, float value);
bool isDynNotchActive(void);
int getMaxFFT(void);
void resetMaxFFT(void);

9. rpmFilter

rpmFilter滤波器里面使用到多种滤波技术

1. pt1Filter
2. rpmNotchFilter：biquadFilter + FILTER_NOTCH(带通滤波) + biquadFilterApplyDF1Weighted(Direct form 1 + Weight)的方式对motor数据进行带通滤波。

上述rpmFilter的主要目的是过滤掉电机固有振动频率，以及振动随油门变化而变化的振动频率带来的干扰。

注：因为本机的电调没有这部分功能，从平铺图上很容易发现机架和电机的振动干扰。

typedef struct rpmFilterConfig_s
{
    uint8_t  rpm_filter_harmonics;     // how many harmonics should be covered with notches? 0 means filter off
    uint8_t  rpm_filter_min_hz;        // minimum frequency of the notches
    uint16_t rpm_filter_fade_range_hz; // range in which to gradually turn off notches down to minHz
    uint16_t rpm_filter_q;             // q of the notches

    uint16_t rpm_filter_lpf_hz;        // the cutoff of the lpf on reported motor rpm

} rpmFilterConfig_t;

void  rpmFilterInit(const rpmFilterConfig_t *config);
float rpmFilterGyro(const int axis, float value);
void  rpmFilterUpdate(void);
bool isRpmFilterEnabled(void);
float rpmMinMotorFrequency(void);

静态滤波 F450机架

针对我们这台F450（大机架）的静态滤波情况：

• 四轴飞控DIY简明步骤介绍
• 四轴飞控DIY调试起飞简明步骤
• 四轴飞控DIY集成FPV功能

大机架的振动频率相对较低，且F450的电调不支持RPM冬天滤波，所以我们这里只是采用了静态滤波的方式，整体调整下来的情况如下（还是非常可控的）：