diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 893f2f8d2..b5d311a05 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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_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_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) }, #endif diff --git a/src/main/common/filter.c b/src/main/common/filter.c index f8a80a700..3967c83ec 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -165,19 +165,19 @@ float filterGetNotchQ(float centerFreq, float cutoffFreq) /* sets up a biquad filter as a 2nd order butterworth LPF */ 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 filter->x1 = filter->x2 = 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 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->a1 /= a0; filter->a2 /= a0; + + // update weight + filter->weight = weight; } 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) */ @@ -243,12 +246,24 @@ FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input) 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 */ FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input) { const float result = filter->b0 * input + filter->x1; + filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2; filter->x2 = filter->b2 * input - filter->a2 * result; + return result; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 9c3a8f3d3..f218b3071 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -52,6 +52,7 @@ typedef struct slewFilter_s { typedef struct biquadFilter_s { float b0, b1, b2, a1, a2; float x1, x2, y1, y2; + float weight; } biquadFilter_t; typedef struct laggedMovingAverage_s { @@ -80,11 +81,12 @@ typedef float (*filterApplyFnPtr)(filter_t *filter, float input); float nullFilterApply(filter_t *filter, float input); 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 biquadFilterUpdate(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, 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); diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index 0ed47a34a..0855cd179 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -329,7 +329,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) 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 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); } } diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index d623b0dcd..458a37803 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -95,7 +95,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) pidRuntime.dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply; const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff); 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 { pidRuntime.dtermNotchApplyFn = nullFilterApply; diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 2c08b71a0..a93f8cc05 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -52,57 +52,63 @@ static pt1Filter_t rpmFilters[MAX_SUPPORTED_MOTORS]; -typedef struct rpmNotchFilter_s -{ - uint8_t harmonics; - float minHz; - float maxHz; - float q; - float loopTime; +typedef struct rpmNotchFilter_s { + + uint8_t harmonics; + float minHz; + float maxHz; + float fadeRangeHz; + float q; + uint32_t looptime; biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS]; + } rpmNotchFilter_t; FAST_DATA_ZERO_INIT static float erpmToHz; 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 uint8_t numberFilters; FAST_DATA_ZERO_INIT static uint8_t numberRpmNotchFilters; FAST_DATA_ZERO_INIT static uint8_t filterUpdatesPerIteration; FAST_DATA_ZERO_INIT static float pidLooptime; 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 currentHarmonic; 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) { config->gyro_rpm_notch_harmonics = 3; config->gyro_rpm_notch_min = 100; + config->gyro_rpm_notch_fade_range_hz = 50; config->gyro_rpm_notch_q = 500; 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->minHz = minHz; - filter->q = q / 100.0f; - filter->loopTime = looptime; + filter->harmonics = config->gyro_rpm_notch_harmonics; + filter->minHz = config->gyro_rpm_notch_min; + filter->maxHz = 0.48f * 1e6f / looptime; // don't go quite to nyquist to avoid oscillations + 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 motor = 0; motor < getMotorCount(); motor++) { - for (int i = 0; i < harmonics; i++) { + for (int i = 0; i < filter->harmonics; i++) { 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; if (config->gyro_rpm_notch_harmonics) { gyroFilter = &filters[numberRpmNotchFilters++]; - rpmNotchFilterInit(gyroFilter, config->gyro_rpm_notch_harmonics, - 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); + rpmNotchFilterInit(gyroFilter, config, pidLooptime); } else { gyroFilter = NULL; } @@ -142,26 +145,24 @@ void rpmFilterInit(const rpmFilterConfig_t *config) 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) { return value; } for (int motor = 0; motor < getMotorCount(); motor++) { 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; } -float rpmFilterGyro(int axis, float value) +float rpmFilterGyro(const int axis, float value) { return applyFilter(gyroFilter, axis, value); } -FAST_DATA_ZERO_INIT static float motorFrequency[MAX_SUPPORTED_MOTORS]; - FAST_CODE_NOINLINE void rpmFilterUpdate() { if (gyroFilter == NULL) { @@ -177,23 +178,33 @@ FAST_CODE_NOINLINE void rpmFilterUpdate() } for (int i = 0; i < filterUpdatesPerIteration; i++) { + float frequency = constrainf( (currentHarmonic + 1) * motorFrequency[currentMotor], currentFilter->minHz, currentFilter->maxHz); - biquadFilter_t* template = ¤tFilter->notch[0][currentMotor][currentHarmonic]; + biquadFilter_t *template = ¤tFilter->notch[0][currentMotor][currentHarmonic]; // 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, 1, motor); */ /* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */ /* 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( - 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++) { - biquadFilter_t* clone = ¤tFilter->notch[axis][currentMotor][currentHarmonic]; + biquadFilter_t *clone = ¤tFilter->notch[axis][currentMotor][currentHarmonic]; clone->b0 = template->b0; clone->b1 = template->b1; clone->b2 = template->b2; clone->a1 = template->a1; clone->a2 = template->a2; + clone->weight = template->weight; } if (++currentHarmonic == currentFilter->harmonics) { @@ -207,7 +218,6 @@ FAST_CODE_NOINLINE void rpmFilterUpdate() } currentFilter = &filters[currentFilterNumber]; } - } } diff --git a/src/main/flight/rpm_filter.h b/src/main/flight/rpm_filter.h index 0ead8bfbf..476172c8e 100644 --- a/src/main/flight/rpm_filter.h +++ b/src/main/flight/rpm_filter.h @@ -25,17 +25,18 @@ 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_min; // minimum frequency of the notches - uint16_t gyro_rpm_notch_q; // q of the notches + 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 + 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; PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig); void rpmFilterInit(const rpmFilterConfig_t *config); -float rpmFilterGyro(int axis, float values); +float rpmFilterGyro(const int axis, float value); void rpmFilterUpdate(); bool isRpmFilterEnabled(void); float rpmMinMotorFrequency(); diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index c69c23a2a..7e9f0c4fc 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -109,7 +109,7 @@ static void gyroInitFilterNotch1(uint16_t notchHz, uint16_t notchCutoffHz) gyro.notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); 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; const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); 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 for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { 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); } } }