diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 040e6549e9..772b7c8e3f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1518,6 +1518,9 @@ static bool blackboxWriteSysinfo(void) #endif #ifdef USE_RPM_FILTER BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_HARMONICS, "%d", rpmFilterConfig()->rpm_filter_harmonics); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_WEIGHTS, "%d,%d,%d", rpmFilterConfig()->rpm_filter_weights[0], + rpmFilterConfig()->rpm_filter_weights[1], + rpmFilterConfig()->rpm_filter_weights[2]); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_Q, "%d", rpmFilterConfig()->rpm_filter_q); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_MIN_HZ, "%d", rpmFilterConfig()->rpm_filter_min_hz); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, "%d", rpmFilterConfig()->rpm_filter_fade_range_hz); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index b6ca858c75..5eaaacd1ea 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1721,6 +1721,7 @@ const clivalue_t valueTable[] = { #ifdef USE_RPM_FILTER { PARAM_NAME_RPM_FILTER_HARMONICS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_harmonics) }, + { PARAM_NAME_RPM_FILTER_WEIGHTS, VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_HARMONICS_MAX, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_weights) }, { PARAM_NAME_RPM_FILTER_Q, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_q) }, { PARAM_NAME_RPM_FILTER_MIN_HZ, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_min_hz) }, { PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_fade_range_hz) }, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 0c7d7db180..2a6336fc02 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -116,6 +116,7 @@ #define PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER "simplified_gyro_filter_multiplier" #define PARAM_NAME_DEBUG_MODE "debug_mode" #define PARAM_NAME_RPM_FILTER_HARMONICS "rpm_filter_harmonics" +#define PARAM_NAME_RPM_FILTER_WEIGHTS "rpm_filter_weights" #define PARAM_NAME_RPM_FILTER_Q "rpm_filter_q" #define PARAM_NAME_RPM_FILTER_MIN_HZ "rpm_filter_min_hz" #define PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ "rpm_filter_fade_range_hz" diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index e4f5266c2d..54b8aade00 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -44,7 +44,6 @@ #include "rpm_filter.h" -#define RPM_FILTER_HARMONICS_MAX 3 #define RPM_FILTER_DURATION_S 0.001f // Maximum duration allowed to update all RPM notches once #define SECONDS_PER_MINUTE 60.0f #define ERPM_PER_LSB 100.0f @@ -53,6 +52,7 @@ typedef struct rpmFilter_s { int numHarmonics; + float weights[RPM_FILTER_HARMONICS_MAX]; float minHz; float maxHz; float fadeRangeHz; @@ -109,6 +109,10 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs) rpmFilter.q = config->rpm_filter_q / 100.0f; rpmFilter.looptimeUs = looptimeUs; + for (int n = 0; n < RPM_FILTER_HARMONICS_MAX; n++) { + rpmFilter.weights[n] = constrainf(config->rpm_filter_weights[n] / 100.0f, 0.0f, 1.0f); + } + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int motor = 0; motor < getMotorCount(); motor++) { for (int i = 0; i < rpmFilter.numHarmonics; i++) { @@ -145,30 +149,37 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void) // update RPM notches for (int i = 0; i < notchUpdatesPerIteration; i++) { - // select current notch on ROLL - biquadFilter_t *template = &rpmFilter.notch[0][motorIndex][harmonicIndex]; + // Only bother updating notches which can have an effect on filtered output + if (rpmFilter.weights[harmonicIndex] > 0.0f) { - const float frequencyHz = constrainf((harmonicIndex + 1) * motorFrequencyHz[motorIndex], rpmFilter.minHz, rpmFilter.maxHz); - const float marginHz = frequencyHz - rpmFilter.minHz; - - // fade out notch when approaching minHz (turn it off) - float weight = 1.0f; - if (marginHz < rpmFilter.fadeRangeHz) { - weight = marginHz / rpmFilter.fadeRangeHz; - } + // select current notch on ROLL + biquadFilter_t *template = &rpmFilter.notch[0][motorIndex][harmonicIndex]; - // update notch - biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight); + const float frequencyHz = constrainf((harmonicIndex + 1) * motorFrequencyHz[motorIndex], rpmFilter.minHz, rpmFilter.maxHz); + const float marginHz = frequencyHz - rpmFilter.minHz; + float weight = 1.0f; - // copy notch properties to corresponding notches on PITCH and YAW - for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilter_t *dest = &rpmFilter.notch[axis][motorIndex][harmonicIndex]; - dest->b0 = template->b0; - dest->b1 = template->b1; - dest->b2 = template->b2; - dest->a1 = template->a1; - dest->a2 = template->a2; - dest->weight = template->weight; + // fade out notch when approaching minHz (turn it off) + if (marginHz < rpmFilter.fadeRangeHz) { + weight *= marginHz / rpmFilter.fadeRangeHz; + } + + // attenuate notches per harmonics group + weight *= rpmFilter.weights[harmonicIndex]; + + // update notch + biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight); + + // copy notch properties to corresponding notches on PITCH and YAW + for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilter_t *dest = &rpmFilter.notch[axis][motorIndex][harmonicIndex]; + dest->b0 = template->b0; + dest->b1 = template->b1; + dest->b2 = template->b2; + dest->a1 = template->a1; + dest->a2 = template->a2; + dest->weight = template->weight; + } } // cycle through all notches on ROLL (takes RPM_FILTER_DURATION_S at max.) @@ -184,6 +195,11 @@ FAST_CODE float rpmFilterApply(const int axis, float value) // Iterate over all notches on axis and apply each one to value. // Order of application doesn't matter because biquads are linear time-invariant filters. for (int i = 0; i < rpmFilter.numHarmonics; i++) { + + if (rpmFilter.weights[i] <= 0.0f) { + continue; // skip harmonics which have no effect on filtered output + } + for (int motor = 0; motor < getMotorCount(); motor++) { value = biquadFilterApplyDF1Weighted(&rpmFilter.notch[axis][motor][i], value); } diff --git a/src/main/pg/rpm_filter.c b/src/main/pg/rpm_filter.c index 15dd1753e1..6fef789f6b 100644 --- a/src/main/pg/rpm_filter.c +++ b/src/main/pg/rpm_filter.c @@ -34,7 +34,8 @@ PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, .rpm_filter_min_hz = 100, .rpm_filter_fade_range_hz = 50, .rpm_filter_q = 500, - .rpm_filter_lpf_hz = 150 + .rpm_filter_lpf_hz = 150, + .rpm_filter_weights = { 100, 100, 100 }, ); #endif // USE_RPM_FILTER diff --git a/src/main/pg/rpm_filter.h b/src/main/pg/rpm_filter.h index cd3f5008c4..2b199ed21a 100644 --- a/src/main/pg/rpm_filter.h +++ b/src/main/pg/rpm_filter.h @@ -24,9 +24,12 @@ #include "pg/pg.h" +#define RPM_FILTER_HARMONICS_MAX 3 + 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_weights[RPM_FILTER_HARMONICS_MAX]; // effect or "weight" (0% - 100%) of each RPM filter harmonic 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