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

Posted 2022-06-13 lida2003

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

• 滤波模块
• 滤波类型
• • 1. slewFilter
  • 2. simpleLowpassFilter
  • 3. laggedMovingAverage
  • 4. pt1Filter
  • 5. pt2Filter
  • 6. pt3Filter
  • 7. biquadFilter
  • • 7.1 biquad类型
    • 7.2 biquad滤波方式
    • • Direct form 1
      • Transposed direct form 2
    • 7.3 biquad滤波接口
  • 8. dynNotchFilter
  • 9. rpmFilter

基于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 <from filter.c/h>
2. 【单一滤波方法】simpleLowpassFilter <from filter.c/h>
3. 【单一滤波方法】laggedMovingAverage <from filter.c/h>
4. 【单一滤波方法】pt1Filter <from filter.c/h>
5. 【单一滤波方法】pt2Filter <from filter.c/h>
6. 【单一滤波方法】pt3Filter <from filter.c/h>
7. 【支持多种滤波方法】 biquadFilter <from filter.c/h>
8. 【支持多种滤波混合】dynNotchFilter <from dyn_notch_filter.c/h>
9. 【支持多种滤波混合】rpmFilter <from rpm_filter.c/h>

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}}{\overbrace{(\frac{\Delta T}{RC+\Delta T})}}+y_{i-1}\stackrel{\text{Inertia from previous Output}}{\overbrace{(\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;             BetaFlight模块设计之二十六：接收机任务分析

 BetaFlight模块设计之二十：CMS菜单模块分析
 BetaFlight模块设计之二十八：MainPidLoop任务分析
 BetaFlight模块设计之二十二：地面测距任务分析
 BetaFlight模块设计之二十一：dashboard任务分析
 BetaFlight模块设计之二十五：dispatch任务分析