rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #10757 from KarateBrot/biquadCrossfeed 

Add biquad crossfeeding + RPM filter fix
This commit is contained in:
J Blackman 2021-07-18 13:31:20 +10:00 committed by GitHub
parent 3d5a0e585c 48f14686d5
commit 6d286e25f1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 76 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/cli/settings.c
View File

@ -1659,6 +1659,7 @@ const clivalue_t valueTable[] = {
{ "gyro_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_harmonics) }, { "gyro_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_harmonics) },
{ "gyro_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) }, { "gyro_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) },
{ "gyro_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_min) }, { "gyro_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_min) },
{ "gyro_rpm_notch_fade_range_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_fade_range_hz) },
{ "rpm_notch_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_lpf) }, { "rpm_notch_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_lpf) },
#endif #endif

25
src/main/common/filter.c
View File

@ -165,19 +165,19 @@ float filterGetNotchQ(float centerFreq, float cutoffFreq)
/* sets up a biquad filter as a 2nd order butterworth LPF */ /* sets up a biquad filter as a 2nd order butterworth LPF */
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate) void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{ {
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f);
} }
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, float weight)
{ {
biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType); biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType, weight);
// zero initial samples // zero initial samples
filter->x1 = filter->x2 = 0; filter->x1 = filter->x2 = 0;
filter->y1 = filter->y2 = 0; filter->y1 = filter->y2 = 0;
} }
FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight)
{ {
// setup variables // setup variables
const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f; const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f;
@ -219,11 +219,14 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
filter->b2 /= a0; filter->b2 /= a0;
filter->a1 /= a0; filter->a1 /= a0;
filter->a2 /= a0; filter->a2 /= a0;
// update weight
filter->weight = weight;
} }
FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate) FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{ {
biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f);
} }
/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */ /* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
@ -243,12 +246,24 @@ FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
return result; return result;
} }
/* Computes a biquadFilter_t filter in df1 and crossfades input with output */
FAST_CODE float biquadFilterApplyDF1Weighted(biquadFilter_t* filter, float input)
{
// compute result
const float result = biquadFilterApplyDF1(filter, input);
// crossfading of input and output to turn filter on/off gradually
return filter->weight * result + (1 - filter->weight) * input;
}
/* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */ /* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input) FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
{ {
const float result = filter->b0 * input + filter->x1; const float result = filter->b0 * input + filter->x1;
filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2; filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
filter->x2 = filter->b2 * input - filter->a2 * result; filter->x2 = filter->b2 * input - filter->a2 * result;
return result; return result;
} }

6
src/main/common/filter.h
View File

@ -52,6 +52,7 @@ typedef struct slewFilter_s {
typedef struct biquadFilter_s { typedef struct biquadFilter_s {
float b0, b1, b2, a1, a2; float b0, b1, b2, a1, a2;
float x1, x2, y1, y2; float x1, x2, y1, y2;
float weight;
} biquadFilter_t; } biquadFilter_t;
typedef struct laggedMovingAverage_s { typedef struct laggedMovingAverage_s {
@ -80,11 +81,12 @@ 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, float weight);
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, float weight);
void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApplyDF1Weighted(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input); float biquadFilterApply(biquadFilter_t *filter, float input);
float filterGetNotchQ(float centerFreq, float cutoffFreq); float filterGetNotchQ(float centerFreq, float cutoffFreq);

2
src/main/flight/gyroanalyse.c
View File

@ -329,7 +329,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
for (int p = 0; p < gyro.notchFilterDynCount; p++) { for (int p = 0; p < gyro.notchFilterDynCount; p++) {
// Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step // Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) { if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH, 1.0f);
} }
} }

2
src/main/flight/pid_init.c
View File

@ -95,7 +95,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pidRuntime.dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply; pidRuntime.dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff); const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInit(&pidRuntime.dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(&pidRuntime.dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH, 1.0f);
} }
} else { } else {
pidRuntime.dtermNotchApplyFn = nullFilterApply; pidRuntime.dtermNotchApplyFn = nullFilterApply;

70
src/main/flight/rpm_filter.c
View File

@ -52,57 +52,63 @@
static pt1Filter_t rpmFilters[MAX_SUPPORTED_MOTORS]; static pt1Filter_t rpmFilters[MAX_SUPPORTED_MOTORS];
typedef struct rpmNotchFilter_s typedef struct rpmNotchFilter_s {
{
uint8_t harmonics; uint8_t harmonics;
float minHz; float minHz;
float maxHz; float maxHz;
float q; float fadeRangeHz;
float loopTime; float q;
uint32_t looptime;
biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS]; biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS];
} rpmNotchFilter_t; } rpmNotchFilter_t;
FAST_DATA_ZERO_INIT static float erpmToHz; FAST_DATA_ZERO_INIT static float erpmToHz;
FAST_DATA_ZERO_INIT static float filteredMotorErpm[MAX_SUPPORTED_MOTORS]; FAST_DATA_ZERO_INIT static float filteredMotorErpm[MAX_SUPPORTED_MOTORS];
FAST_DATA_ZERO_INIT static float motorFrequency[MAX_SUPPORTED_MOTORS];
FAST_DATA_ZERO_INIT static float minMotorFrequency; FAST_DATA_ZERO_INIT static float minMotorFrequency;
FAST_DATA_ZERO_INIT static uint8_t numberFilters; FAST_DATA_ZERO_INIT static uint8_t numberFilters;
FAST_DATA_ZERO_INIT static uint8_t numberRpmNotchFilters; FAST_DATA_ZERO_INIT static uint8_t numberRpmNotchFilters;
FAST_DATA_ZERO_INIT static uint8_t filterUpdatesPerIteration; FAST_DATA_ZERO_INIT static uint8_t filterUpdatesPerIteration;
FAST_DATA_ZERO_INIT static float pidLooptime; FAST_DATA_ZERO_INIT static float pidLooptime;
FAST_DATA_ZERO_INIT static rpmNotchFilter_t filters[2]; FAST_DATA_ZERO_INIT static rpmNotchFilter_t filters[2];
FAST_DATA_ZERO_INIT static rpmNotchFilter_t* gyroFilter; FAST_DATA_ZERO_INIT static rpmNotchFilter_t *gyroFilter;
FAST_DATA_ZERO_INIT static uint8_t currentMotor; FAST_DATA_ZERO_INIT static uint8_t currentMotor;
FAST_DATA_ZERO_INIT static uint8_t currentHarmonic; FAST_DATA_ZERO_INIT static uint8_t currentHarmonic;
FAST_DATA_ZERO_INIT static uint8_t currentFilterNumber; FAST_DATA_ZERO_INIT static uint8_t currentFilterNumber;
FAST_DATA static rpmNotchFilter_t* currentFilter = &filters[0]; FAST_DATA static rpmNotchFilter_t *currentFilter = &filters[0];
PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 4); PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 5);
void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config) void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
{ {
config->gyro_rpm_notch_harmonics = 3; config->gyro_rpm_notch_harmonics = 3;
config->gyro_rpm_notch_min = 100; config->gyro_rpm_notch_min = 100;
config->gyro_rpm_notch_fade_range_hz = 50;
config->gyro_rpm_notch_q = 500; config->gyro_rpm_notch_q = 500;
config->rpm_lpf = 150; config->rpm_lpf = 150;
} }
static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minHz, int q, float looptime) static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t *config, const uint32_t looptime)
{ {
filter->harmonics = harmonics; filter->harmonics = config->gyro_rpm_notch_harmonics;
filter->minHz = minHz; filter->minHz = config->gyro_rpm_notch_min;
filter->q = q / 100.0f; filter->maxHz = 0.48f * 1e6f / looptime; // don't go quite to nyquist to avoid oscillations
filter->loopTime = looptime; filter->fadeRangeHz = config->gyro_rpm_notch_fade_range_hz;
filter->q = config->gyro_rpm_notch_q / 100.0f;
filter->looptime = looptime;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int motor = 0; motor < getMotorCount(); motor++) { for (int motor = 0; motor < getMotorCount(); motor++) {
for (int i = 0; i < harmonics; i++) { for (int i = 0; i < filter->harmonics; i++) {
biquadFilterInit( biquadFilterInit(
&filter->notch[axis][motor][i], minHz * i, looptime, filter->q, FILTER_NOTCH); &filter->notch[axis][motor][i], filter->minHz * i, filter->looptime, filter->q, FILTER_NOTCH, 0.0f);
} }
} }
} }
@ -122,10 +128,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
pidLooptime = gyro.targetLooptime; pidLooptime = gyro.targetLooptime;
if (config->gyro_rpm_notch_harmonics) { if (config->gyro_rpm_notch_harmonics) {
gyroFilter = &filters[numberRpmNotchFilters++]; gyroFilter = &filters[numberRpmNotchFilters++];
rpmNotchFilterInit(gyroFilter, config->gyro_rpm_notch_harmonics, rpmNotchFilterInit(gyroFilter, config, pidLooptime);
config->gyro_rpm_notch_min, config->gyro_rpm_notch_q, gyro.targetLooptime);
// don't go quite to nyquist to avoid oscillations
gyroFilter->maxHz = 0.48f / (gyro.targetLooptime * 1e-6f);
} else { } else {
gyroFilter = NULL; gyroFilter = NULL;
} }
@ -142,26 +145,24 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f); filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f);
} }
static float applyFilter(rpmNotchFilter_t* filter, int axis, float value) static float applyFilter(rpmNotchFilter_t *filter, const int axis, float value)
{ {
if (filter == NULL) { if (filter == NULL) {
return value; return value;
} }
for (int motor = 0; motor < getMotorCount(); motor++) { for (int motor = 0; motor < getMotorCount(); motor++) {
for (int i = 0; i < filter->harmonics; i++) { for (int i = 0; i < filter->harmonics; i++) {
value = biquadFilterApplyDF1(&filter->notch[axis][motor][i], value); value = biquadFilterApplyDF1Weighted(&filter->notch[axis][motor][i], value);
} }
} }
return value; return value;
} }
float rpmFilterGyro(int axis, float value) float rpmFilterGyro(const int axis, float value)
{ {
return applyFilter(gyroFilter, axis, value); return applyFilter(gyroFilter, axis, value);
} }
FAST_DATA_ZERO_INIT static float motorFrequency[MAX_SUPPORTED_MOTORS];
FAST_CODE_NOINLINE void rpmFilterUpdate() FAST_CODE_NOINLINE void rpmFilterUpdate()
{ {
if (gyroFilter == NULL) { if (gyroFilter == NULL) {
@ -177,23 +178,33 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
} }
for (int i = 0; i < filterUpdatesPerIteration; i++) { for (int i = 0; i < filterUpdatesPerIteration; i++) {
float frequency = constrainf( float frequency = constrainf(
(currentHarmonic + 1) * motorFrequency[currentMotor], currentFilter->minHz, currentFilter->maxHz); (currentHarmonic + 1) * motorFrequency[currentMotor], currentFilter->minHz, currentFilter->maxHz);
biquadFilter_t* template = &currentFilter->notch[0][currentMotor][currentHarmonic]; biquadFilter_t *template = &currentFilter->notch[0][currentMotor][currentHarmonic];
// uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above // uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above
/* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */ /* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */ /* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */ /* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 3, frequency) */ /* DEBUG_SET(DEBUG_RPM_FILTER, 3, frequency) */
// fade out notch when approaching minHz (turn it off)
float weight = 1.0f;
if (frequency < currentFilter->minHz + currentFilter->fadeRangeHz) {
weight = (frequency - currentFilter->minHz) / currentFilter->fadeRangeHz;
}
biquadFilterUpdate( biquadFilterUpdate(
template, frequency, currentFilter->loopTime, currentFilter->q, FILTER_NOTCH); template, frequency, currentFilter->looptime, currentFilter->q, FILTER_NOTCH, weight);
for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilter_t* clone = &currentFilter->notch[axis][currentMotor][currentHarmonic]; biquadFilter_t *clone = &currentFilter->notch[axis][currentMotor][currentHarmonic];
clone->b0 = template->b0; clone->b0 = template->b0;
clone->b1 = template->b1; clone->b1 = template->b1;
clone->b2 = template->b2; clone->b2 = template->b2;
clone->a1 = template->a1; clone->a1 = template->a1;
clone->a2 = template->a2; clone->a2 = template->a2;
clone->weight = template->weight;
} }
if (++currentHarmonic == currentFilter->harmonics) { if (++currentHarmonic == currentFilter->harmonics) {
@ -207,7 +218,6 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
} }
currentFilter = &filters[currentFilterNumber]; currentFilter = &filters[currentFilterNumber];
} }
} }
} }

11
src/main/flight/rpm_filter.h
View File

@ -25,17 +25,18 @@
typedef struct rpmFilterConfig_s typedef struct rpmFilterConfig_s
{ {
uint8_t gyro_rpm_notch_harmonics; // how many harmonics should be covered with notches? 0 means filter off uint8_t gyro_rpm_notch_harmonics; // how many harmonics should be covered with notches? 0 means filter off
uint8_t gyro_rpm_notch_min; // minimum frequency of the notches uint8_t gyro_rpm_notch_min; // minimum frequency of the notches
uint16_t gyro_rpm_notch_q; // q of the notches uint16_t gyro_rpm_notch_fade_range_hz; // range in which to gradually turn off notches down to minHz
uint16_t gyro_rpm_notch_q; // q of the notches
uint16_t rpm_lpf; // the cutoff of the lpf on reported motor rpm uint16_t rpm_lpf; // the cutoff of the lpf on reported motor rpm
} rpmFilterConfig_t; } rpmFilterConfig_t;
PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig); PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);
void rpmFilterInit(const rpmFilterConfig_t *config); void rpmFilterInit(const rpmFilterConfig_t *config);
float rpmFilterGyro(int axis, float values); float rpmFilterGyro(const int axis, float value);
void rpmFilterUpdate(); void rpmFilterUpdate();
bool isRpmFilterEnabled(void); bool isRpmFilterEnabled(void);
float rpmMinMotorFrequency(); float rpmMinMotorFrequency();

6
src/main/sensors/gyro_init.c
View File

@ -109,7 +109,7 @@ static void gyroInitFilterNotch1(uint16_t notchHz, uint16_t notchCutoffHz)
gyro.notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; gyro.notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyro.notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(&gyro.notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
} }
} }
} }
@ -124,7 +124,7 @@ static void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz)
gyro.notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; gyro.notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyro.notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(&gyro.notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
} }
} }
} }
@ -141,7 +141,7 @@ static void gyroInitFilterDynamicNotch()
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
biquadFilterInit(&gyro.notchFilterDyn[axis][p], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(&gyro.notchFilterDyn[axis][p], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
} }
} }
} }