Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

gyro & d-term filters: remove filtering options except biquad/pt1

Browse source 
* through extensive testing prior to the beginning of the RC cycle, we have
  discovered that the simplest combination of filters appears to be up to four
  PT1 filters: two for gyro, and two for d-term.
* non-cascaded biquad filter plumbing is retained for noisy setups and the
  dynamic notch bandpass, although gyro and d-term variants of the filtering may
  eventually be removed in favor of pt1
* update all related unit tests
This commit is contained in:
AJ Christensen 2018-05-26 19:25:27 +12:00
parent 418fd4beaa
commit a63c8b0079
8 changed files with 28 additions and 531 deletions
Download patch file Download diff file Expand all files Collapse all files

236
src/main/common/filter.c
View file

@ -31,7 +31,6 @@
#define M_LN2_FLOAT 0.69314718055994530942f #define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f #define M_PI_FLOAT 3.14159265358979323846f
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - 2nd order butterworth*/ #define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - 2nd order butterworth*/
// NULL filter // NULL filter
@ -53,6 +52,7 @@ float pt1FilterGain(uint16_t f_cut, float dT)
void pt1FilterInit(pt1Filter_t *filter, float k) void pt1FilterInit(pt1Filter_t *filter, float k)
{ {
filter->state = 0.0f;
filter->k = k; filter->k = k;
} }
@ -99,51 +99,6 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
} }
// Q coefficients for Butterworth filter of given order. Implicit odd section coefficient of 0.5
// coefficients should be in ascending order
// generated by http://www.earlevel.com/main/2016/09/29/cascading-filters/
static const float butterworthLpfQ[] = {
#if BIQUAD_LPF_ORDER_MAX >= 2
0.70710678, // 2nd
#endif
#if BIQUAD_LPF_ORDER_MAX >= 3
1.0, // 3rd
#endif
#if BIQUAD_LPF_ORDER_MAX >= 4
0.54119610, 1.3065630, // 4th
#endif
#if BIQUAD_LPF_ORDER_MAX >= 5
0.61803399, 1.6180340, // 5th
#endif
#if BIQUAD_LPF_ORDER_MAX >= 6
0.51763809, 0.70710678, 1.9318517 // 6th
#endif
};
#define BUTTERWORTH_QINDEX(o) (((o) - 1) * ((o) - 1) / 4)
// make sure that we have correct number of coefficients
STATIC_ASSERT(BUTTERWORTH_QINDEX(BIQUAD_LPF_ORDER_MAX + 1) == ARRAYLEN(butterworthLpfQ), butterworthLpfQ_mismatch);
// setup cascade of biquad sections for Butterworth LPF filter
// sections is pointer to array of biquad sections, it must be long enough
// return number of sections used
int biquadFilterLpfCascadeInit(biquadFilter_t *sections, int order, float filterFreq, uint32_t refreshRate)
{
biquadFilter_t *section = sections;
// single-pole section first
if (order % 2 == 1) {
biquadFilterInit(section, filterFreq, refreshRate, 0.5, FILTER_LPF1);
section++;
}
const float *Qptr = butterworthLpfQ + BUTTERWORTH_QINDEX(order); // first coefficient for given order
// 2 poles per section
for (int i = order; i >= 2; i -= 2) {
biquadFilterInit(section, filterFreq, refreshRate, *Qptr, FILTER_LPF);
Qptr++; section++;
}
return section - sections;
}
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{ {
// setup variables // setup variables
@ -165,18 +120,6 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
a1 = -2 * cs; a1 = -2 * cs;
a2 = 1 - alpha; a2 = 1 - alpha;
break; break;
case FILTER_LPF1: {
// 1st order Butterworth, H(s) = 1 / (1 + s),
// transformed according to http://www.iowahills.com/A4IIRBilinearTransform.html
const float T = 2.0f * sn / (cs + 1.0f); // T = 2 * tan(omega / 2)
b0 = T;
b1 = T;
b2 = 0;
a0 = T + 2.0f;
a1 = T - 2.0f;
a2 = 0;
break;
}
case FILTER_NOTCH: case FILTER_NOTCH:
b0 = 1; b0 = 1;
b1 = -2 * cs; b1 = -2 * cs;
@ -249,180 +192,3 @@ FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
filter->x2 = filter->b2 * input - filter->a2 * result; filter->x2 = filter->b2 * input - filter->a2 * result;
return result; return result;
} }
FAST_CODE float biquadCascadeFilterApply(biquadFilterCascade_t *filter, float input)
{
for (int i = 0; i < filter->sections; i++ ) {
input = biquadFilterApply(&filter->biquad[i], input);
}
return input;
}
/*
* FIR filter
*/
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
{
filter->buf = buf;
filter->bufLength = bufLength;
filter->coeffs = coeffs;
filter->coeffsLength = coeffsLength;
filter->movingSum = 0.0f;
filter->index = 0;
filter->count = 0;
memset(filter->buf, 0, sizeof(float) * filter->bufLength);
}
/*
* FIR filter initialisation
* If the FIR filter is just to be used for averaging, then coeffs can be set to NULL
*/
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
{
firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
}
void firFilterUpdate(firFilter_t *filter, float input)
{
filter->buf[filter->index++] = input; // index is at the first empty buffer positon
if (filter->index >= filter->bufLength) {
filter->index = 0;
}
}
/*
* Update FIR filter maintaining a moving sum for quick moving average computation
*/
void firFilterUpdateAverage(firFilter_t *filter, float input)
{
filter->movingSum += input; // sum of the last <count> items, to allow quick moving average computation
filter->movingSum -= filter->buf[filter->index]; // subtract the value that "drops off" the end of the moving sum
filter->buf[filter->index++] = input; // index is at the first empty buffer positon
if (filter->index >= filter->bufLength) {
filter->index = 0;
}
if (filter->count < filter->bufLength) {
++filter->count;
}
}
FAST_CODE float firFilterApply(const firFilter_t *filter)
{
float ret = 0.0f;
int ii = 0;
int index;
for (index = filter->index - 1; index >= 0; ++ii, --index) {
ret += filter->coeffs[ii] * filter->buf[index];
}
for (index = filter->bufLength - 1; ii < filter->coeffsLength; ++ii, --index) {
ret += filter->coeffs[ii] * filter->buf[index];
}
return ret;
}
FAST_CODE float firFilterUpdateAndApply(firFilter_t *filter, float input)
{
firFilterUpdate(filter, input);
return firFilterApply(filter);
}
/*
* Returns average of the last <count> items.
*/
float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count)
{
float ret = 0.0f;
int index = filter->index;
for (int ii = 0; ii < filter->coeffsLength; ++ii) {
--index;
if (index < 0) {
index = filter->bufLength - 1;
}
ret += filter->buf[index];
}
return ret / count;
}
float firFilterCalcMovingAverage(const firFilter_t *filter)
{
return filter->movingSum / filter->count;
}
float firFilterLastInput(const firFilter_t *filter)
{
// filter->index points to next empty item in buffer
const int index = filter->index == 0 ? filter->bufLength - 1 : filter->index - 1;
return filter->buf[index];
}
#if defined(USE_FIR_FILTER_DENOISE)
void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime)
{
memset(filter, 0, sizeof(firFilterDenoise_t));
filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE);
}
// prototype function for denoising of signal by dynamic moving average. Mainly for test purposes
float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
{
filter->state[filter->index] = input;
filter->movingSum += filter->state[filter->index++];
if (filter->index == filter->targetCount) {
filter->index = 0;
}
filter->movingSum -= filter->state[filter->index];
if (filter->targetCount >= filter->filledCount) {
return filter->movingSum / filter->targetCount;
} else {
return filter->movingSum / ++filter->filledCount + 1;
}
}
#endif
// ledvinap's proposed RC+FIR2 Biquad-- 1st order IIR, RC filter k
void biquadRCFIR2FilterInit(biquadFilter_t *filter, float k)
{
filter->b0 = k / 2;
filter->b1 = k / 2;
filter->b2 = 0;
filter->a1 = -(1 - k);
filter->a2 = 0;
}
// rs2k's fast "kalman" filter
void fastKalmanInit(fastKalman_t *filter, float k)
{
filter->x = 0.0f; // set initial value, can be zero if unknown
filter->lastX = 0.0f; // set initial value, can be zero if unknown
filter->k = k; // "kalman" gain - half of RC coefficient, calculated externally
}
FAST_CODE float fastKalmanUpdate(fastKalman_t *filter, float input)
{
filter->x += (filter->x - filter->lastX);
filter->lastX = filter->x;
filter->x += filter->k * (input - filter->x);
return filter->x;
}
void lmaSmoothingInit(laggedMovingAverage_t *filter, uint8_t windowSize, float weight)
{
filter->movingWindowIndex = 0;
filter->windowSize = windowSize;
filter->weight = weight;
}
FAST_CODE float lmaSmoothingUpdate(laggedMovingAverage_t *filter, float input)
{
filter->movingSum -= filter->buf[filter->movingWindowIndex];
filter->buf[filter->movingWindowIndex] = input;
filter->movingSum += input;
if (++filter->movingWindowIndex == filter->windowSize) {
filter->movingWindowIndex = 0;
}
return input + (((filter->movingSum / filter->windowSize) - input) * filter->weight);
}

81
src/main/common/filter.h
View file

@ -20,16 +20,6 @@
#pragma once #pragma once
// Don't use it on F1 and F3 to lower RAM usage
// FIR/Denoise filter can be cleaned up in the future as it is rarely used and used to be experimental
#if (defined(STM32F1) || defined(STM32F3))
#define MAX_FIR_DENOISE_WINDOW_SIZE 1
#else
#define MAX_FIR_DENOISE_WINDOW_SIZE 120
#endif
#define MAX_LMA_WINDOW_SIZE 12
struct filter_s; struct filter_s;
typedef struct filter_s filter_t; typedef struct filter_s filter_t;
@ -50,103 +40,32 @@ typedef struct biquadFilter_s {
float x1, x2, y1, y2; float x1, x2, y1, y2;
} biquadFilter_t; } biquadFilter_t;
#define BIQUAD_LPF_ORDER_MAX 6
typedef struct biquadFilterCascade_s {
int sections;
biquadFilter_t biquad[(BIQUAD_LPF_ORDER_MAX + 1) / 2]; // each section is of second order
} biquadFilterCascade_t;
typedef struct firFilterDenoise_s {
int filledCount;
int targetCount;
int index;
float movingSum;
float state[MAX_FIR_DENOISE_WINDOW_SIZE];
} firFilterDenoise_t;
typedef struct fastKalman_s {
float k; // "kalman" gain
float x; // state
float lastX; // previous state
} fastKalman_t;
typedef struct laggedMovingAverage_s {
uint16_t movingWindowIndex;
uint16_t windowSize;
float weight;
float movingSum;
float buf[MAX_LMA_WINDOW_SIZE];
} laggedMovingAverage_t;
typedef enum { typedef enum {
FILTER_PT1 = 0, FILTER_PT1 = 0,
FILTER_BIQUAD, FILTER_BIQUAD,
FILTER_FIR,
FILTER_BUTTERWORTH,
FILTER_BIQUAD_RC_FIR2,
FILTER_FAST_KALMAN
} lowpassFilterType_e; } lowpassFilterType_e;
typedef enum { typedef enum {
FILTER_LPF, // 2nd order Butterworth section FILTER_LPF, // 2nd order Butterworth section
FILTER_NOTCH, FILTER_NOTCH,
FILTER_BPF, FILTER_BPF,
FILTER_LPF1, // 1st order Butterworth section
} biquadFilterType_e; } biquadFilterType_e;
typedef struct firFilter_s {
float *buf;
const float *coeffs;
float movingSum;
uint8_t index;
uint8_t count;
uint8_t bufLength;
uint8_t coeffsLength;
} firFilter_t;
typedef float (*filterApplyFnPtr)(filter_t *filter, float input); typedef float (*filterApplyFnPtr)(filter_t *filter, float input);
float nullFilterApply(filter_t *filter, float input); float nullFilterApply(filter_t *filter, float input);
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); 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); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
int biquadFilterLpfCascadeInit(biquadFilter_t *sections, int order, float filterFreq, uint32_t refreshRate);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input); float biquadFilterApply(biquadFilter_t *filter, float input);
float biquadCascadeFilterApply(biquadFilterCascade_t *filter, float input);
float filterGetNotchQ(float centerFreq, float cutoffFreq); float filterGetNotchQ(float centerFreq, float cutoffFreq);
void biquadRCFIR2FilterInit(biquadFilter_t *filter, float k);
void fastKalmanInit(fastKalman_t *filter, float k);
float fastKalmanUpdate(fastKalman_t *filter, float input);
void lmaSmoothingInit(laggedMovingAverage_t *filter, uint8_t windowSize, float weight);
float lmaSmoothingUpdate(laggedMovingAverage_t *filter, float input);
float pt1FilterGain(uint16_t f_cut, float dT); float pt1FilterGain(uint16_t f_cut, float dT);
void pt1FilterInit(pt1Filter_t *filter, float k); void pt1FilterInit(pt1Filter_t *filter, float k);
float pt1FilterApply(pt1Filter_t *filter, float input); float pt1FilterApply(pt1Filter_t *filter, float input);
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold); void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
float slewFilterApply(slewFilter_t *filter, float input); float slewFilterApply(slewFilter_t *filter, float input);
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
void firFilterUpdate(firFilter_t *filter, float input);
void firFilterUpdateAverage(firFilter_t *filter, float input);
float firFilterApply(const firFilter_t *filter);
float firFilterUpdateAndApply(firFilter_t *filter, float input);
float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count);
float firFilterCalcMovingAverage(const firFilter_t *filter);
float firFilterLastInput(const firFilter_t *filter);
#if defined(USE_FIR_FILTER_DENOISE)
void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime);
float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input);
#endif

11
src/main/flight/pid.c
View file

@ -185,9 +185,6 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
typedef union dtermLowpass_u { typedef union dtermLowpass_u {
pt1Filter_t pt1Filter; pt1Filter_t pt1Filter;
biquadFilter_t biquadFilter; biquadFilter_t biquadFilter;
#if defined(USE_FIR_FILTER_DENOISE)
firFilterDenoise_t denoisingFilter;
#endif
} dtermLowpass_t; } dtermLowpass_t;
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn;
@ -263,14 +260,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime); biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
} }
break; break;
#if defined(USE_FIR_FILTER_DENOISE)
case FILTER_FIR:
dtermLowpassApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
firFilterDenoiseInit(&dtermLowpass[axis].denoisingFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
}
break;
#endif
} }
} }

16
src/main/interface/settings.c
View file

@ -265,22 +265,11 @@ static const char * const lookupTableRcInterpolationChannels[] = {
static const char * const lookupTableLowpassType[] = { static const char * const lookupTableLowpassType[] = {
"PT1", "PT1",
"BIQUAD", "BIQUAD",
#if defined(USE_FIR_FILTER_DENOISE)
"FIR",
#else
NULL,
#endif
"BUTTERWORTH",
"BIQUAD_RC_FIR2",
"FAST_KALMAN"
}; };
static const char * const lookupTableDtermLowpassType[] = { static const char * const lookupTableDtermLowpassType[] = {
"PT1", "PT1",
"BIQUAD", "BIQUAD",
#if defined(USE_FIR_FILTER_DENOISE)
"FIR"
#endif
}; };
@ -436,20 +425,15 @@ const clivalue_t valueTable[] = {
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) }, { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) },
{ "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) }, { "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) },
{ "gyro_lowpass_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_order) },
{ "gyro_lowpass2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_type) }, { "gyro_lowpass2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_type) },
{ "gyro_lowpass2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz) }, { "gyro_lowpass2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz) },
{ "gyro_lowpass2_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_order) },
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) },
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) }, { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) },
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) }, { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) }, { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
{ "gyro_lma_depth", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 11}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_depth)},
{ "gyro_lma_weight", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 100}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_weight)},
{ "gyro_calib_duration", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 50, 3000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroCalibrationDuration) }, { "gyro_calib_duration", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 50, 3000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroCalibrationDuration) },
{ "gyro_calib_noise_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) }, { "gyro_calib_noise_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
{ "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) }, { "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) },

79
src/main/sensors/gyro.c
View file

@ -106,16 +106,9 @@ typedef struct gyroCalibration_s {
bool firstArmingCalibrationWasStarted = false; bool firstArmingCalibrationWasStarted = false;
#if GYRO_LPF_ORDER_MAX > BIQUAD_LPF_ORDER_MAX
#error "GYRO_LPF_ORDER_MAX is larger than BIQUAD_LPF_ORDER_MAX"
#endif
typedef union gyroLowpassFilter_u { typedef union gyroLowpassFilter_u {
pt1Filter_t pt1FilterState; pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState; biquadFilter_t biquadFilterState;
biquadFilterCascade_t biquadFilterCascadeState;
firFilterDenoise_t denoiseFilterState;
fastKalman_t fastKalmanFilterState;
} gyroLowpassFilter_t; } gyroLowpassFilter_t;
typedef struct gyroSensor_s { typedef struct gyroSensor_s {
@ -130,15 +123,13 @@ typedef struct gyroSensor_s {
filterApplyFnPtr lowpass2FilterApplyFn; filterApplyFnPtr lowpass2FilterApplyFn;
gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT]; gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
// lagged moving average smoothing
filterApplyFnPtr lmaSmoothingApplyFn;
laggedMovingAverage_t lmaSmoothingFilter[XYZ_AXIS_COUNT];
// notch filters // notch filters
filterApplyFnPtr notchFilter1ApplyFn; filterApplyFnPtr notchFilter1ApplyFn;
biquadFilter_t notchFilter1[XYZ_AXIS_COUNT]; biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilter2ApplyFn; filterApplyFnPtr notchFilter2ApplyFn;
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn; filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
@ -149,7 +140,6 @@ typedef struct gyroSensor_s {
timeUs_t yawSpinTimeUs; timeUs_t yawSpinTimeUs;
bool yawSpinDetected; bool yawSpinDetected;
#endif // USE_YAW_SPIN_RECOVERY #endif // USE_YAW_SPIN_RECOVERY
} gyroSensor_t; } gyroSensor_t;
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
@ -163,7 +153,7 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
#endif #endif
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz, int order); static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz);
#define DEBUG_GYRO_CALIBRATION 3 #define DEBUG_GYRO_CALIBRATION 3
@ -179,7 +169,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro) #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro) #define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 2); PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3);
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
@ -194,10 +184,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL, .gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL,
.gyro_lowpass_type = FILTER_PT1, .gyro_lowpass_type = FILTER_PT1,
.gyro_lowpass_hz = 90, .gyro_lowpass_hz = 90,
.gyro_lowpass_order = 1,
.gyro_lowpass2_type = FILTER_PT1, .gyro_lowpass2_type = FILTER_PT1,
.gyro_lowpass2_hz = 0, .gyro_lowpass2_hz = 0,
.gyro_lowpass2_order = 1,
.gyro_high_fsr = false, .gyro_high_fsr = false,
.gyro_use_32khz = false, .gyro_use_32khz = false,
.gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
@ -207,8 +195,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_soft_notch_cutoff_2 = 100, .gyro_soft_notch_cutoff_2 = 100,
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES, .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
.gyro_offset_yaw = 0, .gyro_offset_yaw = 0,
.gyro_lma_depth = 0,
.gyro_lma_weight = 100,
.yaw_spin_recovery = true, .yaw_spin_recovery = true,
.yaw_spin_threshold = 1950, .yaw_spin_threshold = 1950,
); );
@ -638,7 +624,7 @@ bool gyroInit(void)
return ret; return ret;
} }
void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz, int order) void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz)
{ {
filterApplyFnPtr *lowpassFilterApplyFn; filterApplyFnPtr *lowpassFilterApplyFn;
gyroLowpassFilter_t *lowpassFilter = NULL; gyroLowpassFilter_t *lowpassFilter = NULL;
@ -684,51 +670,6 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime); biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
} }
break; break;
case FILTER_BIQUAD_RC_FIR2:
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadRCFIR2FilterInit(&lowpassFilter[axis].biquadFilterState, gain);
}
break;
case FILTER_BUTTERWORTH:
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadCascadeFilterApply;
if (order <= GYRO_LPF_ORDER_MAX) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
int *sections = &lowpassFilter[axis].biquadFilterCascadeState.sections;
biquadFilter_t *biquadSections = lowpassFilter[axis].biquadFilterCascadeState.biquad;
*sections = biquadFilterLpfCascadeInit(biquadSections, order, lpfHz, gyro.targetLooptime);
}
}
break;
case FILTER_FAST_KALMAN:
*lowpassFilterApplyFn = (filterApplyFnPtr) fastKalmanUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fastKalmanInit(&lowpassFilter[axis].fastKalmanFilterState, gain / 2);
}
break;
#if defined(USE_FIR_FILTER_DENOISE)
case FILTER_FIR:
*lowpassFilterApplyFn = (filterApplyFnPtr) firFilterDenoiseUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
firFilterDenoiseInit(&lowpassFilter[axis].denoiseFilterState, lpfHz, gyro.targetLooptime);
}
break;
#endif
}
}
}
static void gyroInitLmaSmoothing(gyroSensor_t *gyroSensor, uint8_t depth, uint8_t weight)
{
gyroSensor->lmaSmoothingApplyFn = nullFilterApply;
if (depth && weight) {
const uint8_t windowSize = depth + 1;
const float lmaWeight = weight * 0.01f;
gyroSensor->lmaSmoothingApplyFn = (filterApplyFnPtr)lmaSmoothingUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
lmaSmoothingInit(&gyroSensor->lmaSmoothingFilter[axis], windowSize, lmaWeight);
} }
} }
} }
@ -817,20 +758,16 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
gyroSensor, gyroSensor,
FILTER_LOWPASS, FILTER_LOWPASS,
gyroConfig()->gyro_lowpass_type, gyroConfig()->gyro_lowpass_type,
gyroConfig()->gyro_lowpass_hz, gyroConfig()->gyro_lowpass_hz
gyroConfig()->gyro_lowpass_order
); );
gyroInitLowpassFilterLpf( gyroInitLowpassFilterLpf(
gyroSensor, gyroSensor,
FILTER_LOWPASS2, FILTER_LOWPASS2,
gyroConfig()->gyro_lowpass2_type, gyroConfig()->gyro_lowpass2_type,
gyroConfig()->gyro_lowpass2_hz, gyroConfig()->gyro_lowpass2_hz
gyroConfig()->gyro_lowpass2_order
); );
gyroInitLmaSmoothing(gyroSensor, gyroConfig()->gyro_lma_depth, gyroConfig()->gyro_lma_weight);
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
@ -1118,7 +1055,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
gyroADCf = gyroSensor->lmaSmoothingApplyFn((filter_t *)&gyroSensor->lmaSmoothingFilter[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
@ -1144,7 +1080,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
// apply lowpass2 filter // apply lowpass2 filter
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
gyroADCf = gyroSensor->lmaSmoothingApplyFn((filter_t *)&gyroSensor->lmaSmoothingFilter[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
// apply dynamic notch filter // apply dynamic notch filter

10
src/main/sensors/gyro.h
View file

@ -67,8 +67,6 @@ typedef enum {
FILTER_LOWPASS2 FILTER_LOWPASS2
} filterSlots; } filterSlots;
#define GYRO_LPF_ORDER_MAX 6
typedef struct gyroConfig_s { typedef struct gyroConfig_s {
sensor_align_e gyro_align; // gyro alignment sensor_align_e gyro_align; // gyro alignment
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
@ -80,18 +78,10 @@ typedef struct gyroConfig_s {
bool gyro_use_32khz; bool gyro_use_32khz;
uint8_t gyro_to_use; uint8_t gyro_to_use;
// Lagged Moving Average smoother
uint8_t gyro_lma_depth;
uint8_t gyro_lma_weight;
// Lowpass primary/secondary // Lowpass primary/secondary
uint8_t gyro_lowpass_type; uint8_t gyro_lowpass_type;
uint8_t gyro_lowpass2_type; uint8_t gyro_lowpass2_type;
// Order is used for the 'higher ordering' of cascaded butterworth/biquad sections
uint8_t gyro_lowpass_order;
uint8_t gyro_lowpass2_order;
uint16_t gyro_lowpass_hz; uint16_t gyro_lowpass_hz;
uint16_t gyro_lowpass2_hz; uint16_t gyro_lowpass2_hz;

124
src/test/unit/common_filter_unittest.cc
View file

@ -29,121 +29,37 @@ extern "C" {
#include "unittest_macros.h" #include "unittest_macros.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
TEST(FilterUnittest, TestFirFilterInit) TEST(FilterUnittest, TestPt1FilterInit)
{ {
#define BUFLEN 4 pt1Filter_t filter;
float buf[BUFLEN]; pt1FilterInit(&filter, 0.0f);
firFilter_t filter; EXPECT_EQ(0, filter.k);
pt1FilterInit(&filter, 1.0f);
firFilterInit(&filter, buf, BUFLEN, NULL); EXPECT_EQ(1.0, filter.k);
EXPECT_EQ(buf, filter.buf);
EXPECT_EQ(0, filter.coeffs);
EXPECT_EQ(0, filter.movingSum);
EXPECT_EQ(0, filter.index);
EXPECT_EQ(0, filter.count);
EXPECT_EQ(BUFLEN, filter.bufLength);
EXPECT_EQ(BUFLEN, filter.coeffsLength);
} }
TEST(FilterUnittest, TestFirFilterUpdateAverage) TEST(FilterUnittest, TestPt1FilterGain)
{ {
#define BUFLEN 4 EXPECT_FLOAT_EQ(0.999949, pt1FilterGain(100, 31.25f));
float buf[BUFLEN]; // handle cases over uint8_t boundary
const float coeffs[BUFLEN] = {1.0f, 1.0f, 1.0f, 1.0f}; EXPECT_FLOAT_EQ(0.99998301, pt1FilterGain(300, 31.25f));
firFilter_t filter;
firFilterInit(&filter, buf, BUFLEN, coeffs);
firFilterUpdateAverage(&filter, 2.0f);
EXPECT_FLOAT_EQ(2.0f, filter.buf[0]);
EXPECT_FLOAT_EQ(2.0f, filter.movingSum);
EXPECT_EQ(1, filter.index);
EXPECT_EQ(1, filter.count);
EXPECT_EQ(2.0f, firFilterCalcMovingAverage(&filter));
EXPECT_FLOAT_EQ(2.0f, firFilterCalcPartialAverage(&filter, 1));
EXPECT_FLOAT_EQ(2.0f, firFilterApply(&filter));
firFilterUpdateAverage(&filter, 3.0f);
EXPECT_FLOAT_EQ(3.0f, filter.buf[1]);
EXPECT_FLOAT_EQ(5.0f, filter.movingSum);
EXPECT_EQ(2, filter.index);
EXPECT_EQ(2, filter.count);
EXPECT_EQ(2.5f, firFilterCalcMovingAverage(&filter));
EXPECT_FLOAT_EQ(2.5f, firFilterCalcPartialAverage(&filter, 2));
EXPECT_FLOAT_EQ(5.0f, firFilterApply(&filter));
firFilterUpdateAverage(&filter, 4.0f);
EXPECT_FLOAT_EQ(4.0f, filter.buf[2]);
EXPECT_FLOAT_EQ(9.0f, filter.movingSum);
EXPECT_EQ(3, filter.index);
EXPECT_EQ(3, filter.count);
EXPECT_EQ(3.0f, firFilterCalcMovingAverage(&filter));
EXPECT_FLOAT_EQ(3.0f, firFilterCalcPartialAverage(&filter, 3));
EXPECT_FLOAT_EQ(9.0f, firFilterApply(&filter));
firFilterUpdateAverage(&filter, 5.0f);
EXPECT_FLOAT_EQ(5.0f, filter.buf[3]);
EXPECT_FLOAT_EQ(14.0f, filter.movingSum);
EXPECT_EQ(0, filter.index);
EXPECT_EQ(4, filter.count);
EXPECT_EQ(3.5f, firFilterCalcMovingAverage(&filter));
EXPECT_FLOAT_EQ(3.5f, firFilterCalcPartialAverage(&filter, 4));
EXPECT_FLOAT_EQ(14.0f, firFilterApply(&filter));
firFilterUpdateAverage(&filter, 6.0f);
EXPECT_FLOAT_EQ(6.0f, filter.buf[0]);
EXPECT_FLOAT_EQ(18.0f, filter.movingSum);
EXPECT_EQ(1, filter.index);
EXPECT_EQ(BUFLEN, filter.count);
EXPECT_EQ(4.5f, firFilterCalcMovingAverage(&filter));
EXPECT_FLOAT_EQ(4.5f, firFilterCalcPartialAverage(&filter, BUFLEN));
EXPECT_FLOAT_EQ(18.0f, firFilterApply(&filter));
firFilterUpdateAverage(&filter, 7.0f);
EXPECT_FLOAT_EQ(7.0f, filter.buf[1]);
EXPECT_FLOAT_EQ(22.0f, filter.movingSum);
EXPECT_EQ(2, filter.index);
EXPECT_EQ(BUFLEN, filter.count);
EXPECT_EQ(5.5f, firFilterCalcMovingAverage(&filter));
EXPECT_FLOAT_EQ(5.5f, firFilterCalcPartialAverage(&filter, BUFLEN));
EXPECT_FLOAT_EQ(22.0f, firFilterApply(&filter));
} }
TEST(FilterUnittest, TestFirFilterApply) TEST(FilterUnittest, TestPt1FilterApply)
{ {
#define BUFLEN 4 pt1Filter_t filter;
float buf[BUFLEN]; pt1FilterInit(&filter, pt1FilterGain(100, 31.25f));
firFilter_t filter; EXPECT_EQ(0, filter.state);
const float coeffs[BUFLEN] = {26.0f, 27.0f, 28.0f, 29.0f};
float expected = 0.0f; pt1FilterApply(&filter, 1800.0f);
firFilterInit(&filter, buf, BUFLEN, coeffs); EXPECT_FLOAT_EQ(1799.9083, filter.state);
firFilterUpdate(&filter, 2.0f); pt1FilterApply(&filter, -1800.0f);
expected = 2.0f * 26.0f; EXPECT_FLOAT_EQ(-1799.8165, filter.state);
EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
firFilterUpdate(&filter, 3.0f); pt1FilterApply(&filter, -200.0f);
expected = 3.0f * 26.0f + 2.0 * 27.0; EXPECT_FLOAT_EQ(-200.08142, filter.state);
EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
firFilterUpdate(&filter, 4.0f);
expected = 4.0f * 26.0f + 3.0 * 27.0 + 2.0 * 28.0;
EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
firFilterUpdate(&filter, 5.0f);
expected = 5.0f * 26.0f + 4.0 * 27.0 + 3.0 * 28.0 + 2.0f * 29.0f;
EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
firFilterUpdate(&filter, 6.0f);
expected = 6.0f * 26.0f + 5.0 * 27.0 + 4.0 * 28.0 + 3.0f * 29.0f;
EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
firFilterUpdate(&filter, 7.0f);
expected = 7.0f * 26.0f + 6.0 * 27.0 + 5.0 * 28.0 + 4.0f * 29.0f;
EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
} }
TEST(FilterUnittest, TestSlewFilterInit) TEST(FilterUnittest, TestSlewFilterInit)

2
src/test/unit/sensor_gyro_unittest.cc
View file

@ -117,8 +117,6 @@ TEST(SensorGyro, Update)
// turn off filters // turn off filters
gyroConfigMutable()->gyro_lowpass_hz = 0; gyroConfigMutable()->gyro_lowpass_hz = 0;
gyroConfigMutable()->gyro_lowpass2_hz = 0; gyroConfigMutable()->gyro_lowpass2_hz = 0;
gyroConfigMutable()->gyro_lma_depth = 0;
gyroConfigMutable()->gyro_lma_weight = 0;
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
gyroInit(); gyroInit();