From ce95faf31a384556f22da63e016db26f8a44699e Mon Sep 17 00:00:00 2001 From: mituritsyn Date: Wed, 25 Jun 2025 18:28:10 +0200 Subject: [PATCH 1/3] remove rpm_filter_harmonics --- src/main/blackbox/blackbox.c | 1 - src/main/cli/settings.c | 1 - src/main/fc/parameter_names.h | 1 - src/main/flight/rpm_filter.c | 53 +++++++++++++++++------------------ src/main/msp/msp.c | 2 -- src/main/pg/rpm_filter.c | 1 - src/main/pg/rpm_filter.h | 1 - 7 files changed, 26 insertions(+), 34 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1492282d83..214ff05083 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1700,7 +1700,6 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_POLES, "%d", motorConfig()->motorPoleCount); #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]); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 9e6c0ddbf2..c42392ad78 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1936,7 +1936,6 @@ const clivalue_t valueTable[] = { #endif #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) }, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 9dd99ed845..bfd3b074a4 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -150,7 +150,6 @@ #define PARAM_NAME_SIMPLIFIED_GYRO_FILTER "simplified_gyro_filter" #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" diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index a95cab2555..71bcf232a8 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -46,7 +46,6 @@ typedef struct rpmFilter_s { - int numHarmonics; float weights[RPM_FILTER_HARMONICS_MAX]; float minHz; float maxHz; @@ -65,51 +64,54 @@ FAST_DATA_ZERO_INIT static rpmFilter_t rpmFilter; FAST_DATA_ZERO_INIT static int notchUpdatesPerIteration; FAST_DATA_ZERO_INIT static int motorIndex; FAST_DATA_ZERO_INIT static int harmonicIndex; +FAST_DATA_ZERO_INIT static int activeHarmonicsCount; void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs) { motorIndex = 0; harmonicIndex = 0; - rpmFilter.numHarmonics = 0; // disable RPM Filtering + activeHarmonicsCount = 0; // disable RPM Filtering // if bidirectional DShot is not available if (!useDshotTelemetry) { return; } + + 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); + if (rpmFilter.weights[n] > 0.0f) { + activeHarmonicsCount++; + } + } // if RPM Filtering is configured to be off - if (!config->rpm_filter_harmonics) { + if (!isRpmFilterEnabled()) { return; } // if we get to this point, enable and init RPM filtering - rpmFilter.numHarmonics = config->rpm_filter_harmonics; rpmFilter.minHz = config->rpm_filter_min_hz; rpmFilter.maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations rpmFilter.fadeRangeHz = config->rpm_filter_fade_range_hz; 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++) { - biquadFilterInit(&rpmFilter.notch[axis][motor][i], rpmFilter.minHz * i, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, 0.0f); + for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) { + biquadFilterInit(&rpmFilter.notch[axis][motor][i], rpmFilter.minHz * (i + 1), rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, rpmFilter.weights[i]); } } } const float loopIterationsPerUpdate = RPM_FILTER_DURATION_S / (looptimeUs * 1e-6f); - const float numNotchesPerAxis = getMotorCount() * rpmFilter.numHarmonics; + const float numNotchesPerAxis = getMotorCount() * activeHarmonicsCount; notchUpdatesPerIteration = ceilf(numNotchesPerAxis / loopIterationsPerUpdate); // round to ceiling } FAST_CODE_NOINLINE void rpmFilterUpdate(void) { - if (!useDshotTelemetry) { + if (!isRpmFilterEnabled()) { return; } @@ -161,34 +163,31 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void) } // cycle through all notches on ROLL (takes RPM_FILTER_DURATION_S at max.) - harmonicIndex = (harmonicIndex + 1) % rpmFilter.numHarmonics; - if (harmonicIndex == 0) { - motorIndex = (motorIndex + 1) % getMotorCount(); + //not sure if it correct + motorIndex++; + if (motorIndex >= getMotorCount()) { + // All motors processed for this harmonic, move to next harmonic + motorIndex = 0; + harmonicIndex = (harmonicIndex + 1) % RPM_FILTER_HARMONICS_MAX; } } } 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); + for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) { + if (rpmFilter.weights[i] > 0.0f) { + for (int motor = 0; motor < getMotorCount(); motor++) { + value = biquadFilterApplyDF1Weighted(&rpmFilter.notch[axis][motor][i], value); + } } } - return value; } bool isRpmFilterEnabled(void) { - return rpmFilter.numHarmonics > 0; + return activeHarmonicsCount > 0; } #endif // USE_RPM_FILTER diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index b5fe879f1a..e38b88244e 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1911,7 +1911,6 @@ case MSP_NAME: sbufWriteU16(dst, 0); #endif #if defined(USE_RPM_FILTER) - sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_harmonics); sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_min_hz); #else sbufWriteU8(dst, 0); @@ -3121,7 +3120,6 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU16(src); #endif #if defined(USE_RPM_FILTER) - rpmFilterConfigMutable()->rpm_filter_harmonics = sbufReadU8(src); rpmFilterConfigMutable()->rpm_filter_min_hz = sbufReadU8(src); #else sbufReadU8(src); diff --git a/src/main/pg/rpm_filter.c b/src/main/pg/rpm_filter.c index 2c08953e64..2e7a591a0a 100644 --- a/src/main/pg/rpm_filter.c +++ b/src/main/pg/rpm_filter.c @@ -30,7 +30,6 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 6); PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, - .rpm_filter_harmonics = 3, .rpm_filter_min_hz = 100, .rpm_filter_fade_range_hz = 50, .rpm_filter_q = 500, diff --git a/src/main/pg/rpm_filter.h b/src/main/pg/rpm_filter.h index 2b199ed21a..6401684d0b 100644 --- a/src/main/pg/rpm_filter.h +++ b/src/main/pg/rpm_filter.h @@ -28,7 +28,6 @@ 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 From 9a1593b78e336583b61954869a89e15b1fdf3bab Mon Sep 17 00:00:00 2001 From: mituritsyn Date: Wed, 25 Jun 2025 19:03:32 +0200 Subject: [PATCH 2/3] move activeHarmonicsCount into rpmFilter_s --- src/main/flight/rpm_filter.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 71bcf232a8..f2bd3e5482 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -46,6 +46,7 @@ typedef struct rpmFilter_s { + int activeHarmonicsCount; float weights[RPM_FILTER_HARMONICS_MAX]; float minHz; float maxHz; @@ -64,13 +65,12 @@ FAST_DATA_ZERO_INIT static rpmFilter_t rpmFilter; FAST_DATA_ZERO_INIT static int notchUpdatesPerIteration; FAST_DATA_ZERO_INIT static int motorIndex; FAST_DATA_ZERO_INIT static int harmonicIndex; -FAST_DATA_ZERO_INIT static int activeHarmonicsCount; void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs) { motorIndex = 0; harmonicIndex = 0; - activeHarmonicsCount = 0; // disable RPM Filtering + rpmFilter.activeHarmonicsCount = 0; // disable RPM Filtering // if bidirectional DShot is not available if (!useDshotTelemetry) { @@ -80,7 +80,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t 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); if (rpmFilter.weights[n] > 0.0f) { - activeHarmonicsCount++; + rpmFilter.activeHarmonicsCount++; } } @@ -105,7 +105,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs) } const float loopIterationsPerUpdate = RPM_FILTER_DURATION_S / (looptimeUs * 1e-6f); - const float numNotchesPerAxis = getMotorCount() * activeHarmonicsCount; + const float numNotchesPerAxis = getMotorCount() * rpmFilter.activeHarmonicsCount; notchUpdatesPerIteration = ceilf(numNotchesPerAxis / loopIterationsPerUpdate); // round to ceiling } @@ -187,7 +187,7 @@ FAST_CODE float rpmFilterApply(const int axis, float value) bool isRpmFilterEnabled(void) { - return activeHarmonicsCount > 0; + return rpmFilter.activeHarmonicsCount > 0; } #endif // USE_RPM_FILTER From bfff81889cb093bf45ccc1644d484522d5e98cc7 Mon Sep 17 00:00:00 2001 From: mituritsyn Date: Sun, 29 Jun 2025 11:05:25 +0200 Subject: [PATCH 3/3] add rpm_filter_weights to msp --- src/main/msp/msp.c | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index e38b88244e..a5cae16503 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1911,9 +1911,15 @@ case MSP_NAME: sbufWriteU16(dst, 0); #endif #if defined(USE_RPM_FILTER) + for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) { + // Added in MSP API 1.46 + sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_weights[i]); + } sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_min_hz); #else - sbufWriteU8(dst, 0); + for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) { + sbufWriteU8(dst, 0); + } sbufWriteU8(dst, 0); #endif #if defined(USE_DYN_NOTCH_FILTER) @@ -3106,7 +3112,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU16(src); #endif } - if (sbufBytesRemaining(src) >= 8) { + if (sbufBytesRemaining(src) >= 10) { // Added in MSP API 1.42 #if defined(USE_DYN_NOTCH_FILTER) sbufReadU8(src); // DEPRECATED 1.43: dyn_notch_range @@ -3120,9 +3126,15 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU16(src); #endif #if defined(USE_RPM_FILTER) + for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) { + // Added in MSP API 1.46 + rpmFilterConfigMutable()->rpm_filter_weights[i] = sbufReadU8(src); + } rpmFilterConfigMutable()->rpm_filter_min_hz = sbufReadU8(src); #else - sbufReadU8(src); + for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) { + sbufReadU8(src); + } sbufReadU8(src); #endif }