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..f2bd3e5482 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -46,7 +46,7 @@ typedef struct rpmFilter_s { - int numHarmonics; + int activeHarmonicsCount; float weights[RPM_FILTER_HARMONICS_MAX]; float minHz; float maxHz; @@ -70,46 +70,48 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs) { motorIndex = 0; harmonicIndex = 0; - rpmFilter.numHarmonics = 0; // disable RPM Filtering + rpmFilter.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) { + rpmFilter.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() * rpmFilter.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 rpmFilter.activeHarmonicsCount > 0; } #endif // USE_RPM_FILTER diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 15cab1dec6..9bd942d385 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1911,10 +1911,15 @@ case MSP_NAME: sbufWriteU16(dst, 0); #endif #if defined(USE_RPM_FILTER) - sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_harmonics); + 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) @@ -3107,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 @@ -3121,10 +3126,15 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU16(src); #endif #if defined(USE_RPM_FILTER) - rpmFilterConfigMutable()->rpm_filter_harmonics = sbufReadU8(src); + 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 } 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