From fefc7ef44a55866e5171246bcfc78151cfa56c8f Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 19 Nov 2024 10:13:16 +0000 Subject: [PATCH 1/5] Refactor RPM notch filter --- src/main/blackbox/blackbox.c | 38 +--- src/main/cli/settings.c | 15 +- src/main/config/config.c | 21 +- src/main/flight/rpm_filter.c | 398 +++++++++++++++++++++-------------- src/main/flight/rpm_filter.h | 2 + src/main/msp/msp.c | 62 ++++-- src/main/pg/rpm_filter.c | 7 +- src/main/pg/rpm_filter.h | 25 ++- 8 files changed, 326 insertions(+), 242 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 194ddf0302..cb22b26859 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1681,42 +1681,8 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DSHOT_BIDIR, "%d", motorConfig()->dev.useDshotTelemetry); #endif #ifdef USE_RPM_FILTER - BLACKBOX_PRINT_HEADER_LINE_CUSTOM( - char *ptr = buf; - for (int i=0; i 0) - *ptr++ = ','; - ptr += tfp_sprintf(ptr, "%d", rpmFilterConfig()->filter_bank_rpm_source[i]); - } - blackboxPrintfHeaderLine("gyro_rpm_filter_bank_rpm_source", buf); - ); - BLACKBOX_PRINT_HEADER_LINE_CUSTOM( - char *ptr = buf; - for (int i=0; i 0) - *ptr++ = ','; - ptr += tfp_sprintf(ptr, "%d", rpmFilterConfig()->filter_bank_rpm_ratio[i]); - } - blackboxPrintfHeaderLine("gyro_rpm_filter_bank_rpm_ratio", buf); - ); - BLACKBOX_PRINT_HEADER_LINE_CUSTOM( - char *ptr = buf; - for (int i=0; i 0) - *ptr++ = ','; - ptr += tfp_sprintf(ptr, "%d", rpmFilterConfig()->filter_bank_rpm_limit[i]); - } - blackboxPrintfHeaderLine("gyro_rpm_filter_bank_rpm_limit", buf); - ); - BLACKBOX_PRINT_HEADER_LINE_CUSTOM( - char *ptr = buf; - for (int i=0; i 0) - *ptr++ = ','; - ptr += tfp_sprintf(ptr, "%d", rpmFilterConfig()->filter_bank_notch_q[i]); - } - blackboxPrintfHeaderLine("gyro_rpm_filter_bank_notch_q", buf); - ); + BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_preset", "%d", rpmFilterConfig()->preset); + BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_min_hz", "%d", rpmFilterConfig()->min_hz); #endif #if defined(USE_ACC) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LPF_HZ, "%d", accelerometerConfig()->acc_lpf_hz * 100); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index a90c5c7d76..84e934354d 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1694,10 +1694,17 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_RPM_FILTER - { "gyro_rpm_filter_bank_rpm_source", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_source) }, - { "gyro_rpm_filter_bank_rpm_ratio", VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_ratio) }, - { "gyro_rpm_filter_bank_rpm_limit", VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_limit) }, - { "gyro_rpm_filter_bank_notch_q", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_notch_q) }, + { "gyro_rpm_notch_preset", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, preset) }, + { "gyro_rpm_notch_min_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 100 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, min_hz) }, + { "gyro_rpm_notch_source_pitch", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_source[PITCH]) }, + { "gyro_rpm_notch_center_pitch", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_center[PITCH]) }, + { "gyro_rpm_notch_q_pitch", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_q[PITCH]) }, + { "gyro_rpm_notch_source_roll", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_source[ROLL]) }, + { "gyro_rpm_notch_center_roll", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_center[ROLL]) }, + { "gyro_rpm_notch_q_roll", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_q[ROLL]) }, + { "gyro_rpm_notch_source_yaw", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_source[YAW]) }, + { "gyro_rpm_notch_center_yaw", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_center[YAW]) }, + { "gyro_rpm_notch_q_yaw", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_NOTCH_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, custom.notch_q[YAW]) }, #endif #ifdef USE_RX_FLYSKY diff --git a/src/main/config/config.c b/src/main/config/config.c index ce3a392020..66ec260401 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -428,6 +428,10 @@ static void validateAndFixConfig(void) featureDisableImmediate(FEATURE_RSSI_ADC); #endif +#ifdef USE_RPM_FILTER + validateAndFixRPMFilterConfig(); +#endif + #if defined(USE_BEEPER) #ifdef USE_TIMER if (beeperDevConfig()->frequency && !timerGetConfiguredByTag(beeperDevConfig()->ioTag)) { @@ -664,23 +668,6 @@ void validateAndFixGyroConfig(void) if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) { gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; } - -#ifdef USE_RPM_FILTER - if (featureIsConfigured(FEATURE_RPM_FILTER)) { - rpmFilterConfig_t *config = rpmFilterConfigMutable(); - for (int index = 0; index < RPM_FILTER_BANK_COUNT; index++) { - if (config->filter_bank_rpm_source[index] == 0 || - config->filter_bank_rpm_ratio[index] == 0 || - config->filter_bank_notch_q[index] == 0) - { - config->filter_bank_rpm_source[index] = 0; - config->filter_bank_rpm_ratio[index] = 0; - config->filter_bank_rpm_limit[index] = 0; - config->filter_bank_notch_q[index] = 0; - } - } - } -#endif } #ifdef USE_BLACKBOX diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 797569ff13..e345c391e3 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -40,213 +40,295 @@ #include "rpm_filter.h" // Number of banks to update in one cycle -#define RPM_UPDATE_BANK_COUNT 1 +#define RPM_UPDATE_BANK_COUNT 5 -typedef struct rpmFilterBank_s +// Number of predefined presets +#define RPM_FILTER_PRESET_COUNT 3 + +// RPM Filter Presets +static const rpmNotchConfig_t rpmFilterPreset[RPM_FILTER_PRESET_COUNT] = +{ + // Low vibration preset + { + .notch_source = { + { 11, 12, 21, }, + { 11, 12, 21, }, + { 11, 12, 21, }, + }, + .notch_center = { + { 0, 0, 0, }, + { 0, 0, 0, }, + { 0, 0, 0, }, + }, + .notch_q = { + { 100, 50, 50, }, + { 100, 50, 50, }, + { 100, 50, 50, }, + }, + }, + // Typical vibration preset + { + .notch_source = { + { 11, 12, 13, 14, 21, }, + { 11, 12, 13, 14, 21, }, + { 11, 12, 13, 14, 21, }, + }, + .notch_center = { + { 0, 0, 0, 0, 0, }, + { 0, 0, 0, 0, 0, }, + { 0, 0, 0, 0, 0, }, + }, + .notch_q = { + { 80, 50, 80, 80, 50, }, + { 80, 50, 80, 80, 50, }, + { 80, 50, 80, 80, 50, }, + }, + }, + // High vibration preset + { + .notch_source = { + { 11, 12, 13, 14, 15, 21, 22, 10 }, + { 11, 12, 13, 14, 15, 21, 22, 10 }, + { 11, 12, 13, 14, 15, 21, 22, 10 }, + }, + .notch_center = { + { 0, 0, 0, 0, 0, 0, 0, 0, }, + { 0, 0, 0, 0, 0, 0, 0, 0, }, + { 0, 0, 0, 0, 0, 0, 0, 0, }, + }, + .notch_q = { + { 50, 30, 80, 80, 80, 50, 80, 80 }, + { 50, 30, 80, 80, 80, 50, 80, 80 }, + { 80, 50, 80, 80, 80, 50, 80, 80 }, + }, + }, +}; + +// Internal data +typedef struct { uint8_t motor; + float fader; float ratio; - float minHz; - float maxHz; float notchQ; - biquadFilter_t notch[XYZ_AXIS_COUNT]; + biquadFilter_t notch; } rpmFilterBank_t; +FAST_DATA_ZERO_INIT static rpmFilterBank_t filterBank[XYZ_AXIS_COUNT][RPM_FILTER_NOTCH_COUNT]; -FAST_DATA_ZERO_INIT static rpmFilterBank_t filterBank[RPM_FILTER_BANK_COUNT]; - -FAST_DATA_ZERO_INIT static uint8_t activeBankCount; +FAST_DATA_ZERO_INIT static uint8_t updateAxisNumber; FAST_DATA_ZERO_INIT static uint8_t updateBankNumber; +FAST_DATA_ZERO_INIT static uint8_t updateBankCount; +FAST_DATA_ZERO_INIT static uint8_t totalBankCount; +FAST_DATA_ZERO_INIT static float notchMaxHz; +FAST_DATA_ZERO_INIT static float notchMinHz; +FAST_DATA_ZERO_INIT static float notchFadeHz; -INIT_CODE void rpmFilterInit(void) -{ - const rpmFilterConfig_t *config = rpmFilterConfig(); - - const int mainMotorIndex = 0; - const int tailMotorIndex = mixerMotorizedTail() ? 1 : 0; - - const float mainGearRatio = getMainGearRatio(); - const float tailGearRatio = getTailGearRatio(); - - const bool enable10 = mainGearRatio != 1.0f; - const bool enable20 = tailGearRatio != 1.0f && mixerMotorizedTail(); - int bankNumber = 0; - - #define CHECK_SOURCE(motor) if (!isMotorFastRpmSourceActive(motor)) goto error - - for (int index = 0; index < RPM_FILTER_BANK_COUNT; index++) - { - if (config->filter_bank_rpm_source[index] == 0 || - config->filter_bank_rpm_ratio[index] == 0 || - config->filter_bank_notch_q[index] == 0) - continue; - - /* - * NOTE! rpm_limit has different meaning depending on rpm_source - * - * 1-4 Minimum RPM of the MOTOR - * 10-18 Minimum RPM of the Main ROTOR - * 20-28 Minimum RPM of the Tail ROTOR - */ - - rpmFilterBank_t *bank = &filterBank[bankNumber]; - - // RPM source for this bank - const unsigned source = config->filter_bank_rpm_source[index]; - - // Ratio converts RPM to Hz - const float ratio = 10000.0f / (constrain(config->filter_bank_rpm_ratio[index], 1, 50000) * 60.0f); - - // Absolute limits - const float minHzLimit = 0.40f * gyro.filterRateHz; - const float maxHzLimit = 0.45f * gyro.filterRateHz; - - // Q value - const float notchQ = constrainf(config->filter_bank_notch_q[index], 5, 250) / 10; - - // Motor RPM based notches - if (source >= 1 && source <= getMotorCount()) { - CHECK_SOURCE(source - 1); - bank->motor = source - 1; - bank->ratio = ratio; - bank->minHz = constrainf(config->filter_bank_rpm_limit[index] * ratio, 10, minHzLimit); - bank->maxHz = maxHzLimit; - bank->notchQ = notchQ; - bankNumber++; - } - // Main Motor (M1) - else if (source == 10) { - if (enable10) { - CHECK_SOURCE(mainMotorIndex); - bank->motor = mainMotorIndex; - bank->ratio = ratio; - bank->minHz = constrainf((config->filter_bank_rpm_limit[index] / mainGearRatio) * ratio, 10, minHzLimit); - bank->maxHz = maxHzLimit; - bank->notchQ = notchQ; - bankNumber++; +INIT_CODE void validateAndFixRPMFilterConfig(void) +{ + rpmFilterConfig_t *config = rpmFilterConfigMutable(); + rpmNotchConfig_t *custom = &config->custom; + + if (config->preset == 0) { + for (int axis = 0; axis < RPM_FILTER_AXIS_COUNT; axis++) { + for (int bank = 0; bank < RPM_FILTER_NOTCH_COUNT; bank++) { + if (custom->notch_source[axis][bank] == 0 || + custom->notch_q[axis][bank] == 0) + { + custom->notch_source[axis][bank] = 0; + custom->notch_center[axis][bank] = 0; + custom->notch_q[axis][bank] = 0; + } } } - // Main Rotor harmonics - else if (source >= 11 && source <= 18) { - CHECK_SOURCE(mainMotorIndex); - const int harmonic = source - 10; - bank->motor = mainMotorIndex; - bank->ratio = mainGearRatio * harmonic * ratio; - bank->minHz = constrainf((config->filter_bank_rpm_limit[index] * harmonic) * ratio, 10, minHzLimit); - bank->maxHz = maxHzLimit; - bank->notchQ = notchQ; - bankNumber++; - } - // Tail Motor (M2) - else if (source == 20) { - if (enable20) { - CHECK_SOURCE(tailMotorIndex); - bank->motor = tailMotorIndex; - bank->ratio = ratio; - bank->minHz = constrainf((config->filter_bank_rpm_limit[index] / tailGearRatio) * ratio, 10, minHzLimit); - bank->maxHz = maxHzLimit; - bank->notchQ = notchQ; - bankNumber++; + } + else if (config->preset <= RPM_FILTER_PRESET_COUNT) { + const rpmNotchConfig_t *preset = &rpmFilterPreset[config->preset - 1]; + for (int axis = 0; axis < RPM_FILTER_AXIS_COUNT; axis++) { + for (int bank = 0; bank < RPM_FILTER_NOTCH_COUNT; bank++) { + custom->notch_source[axis][bank] = preset->notch_source[axis][bank]; + custom->notch_center[axis][bank] = preset->notch_center[axis][bank]; + custom->notch_q[axis][bank] = preset->notch_q[axis][bank]; } } - // Tail Rotor harmonics - else if (source >= 21 && source <= 28) { - CHECK_SOURCE(tailMotorIndex); - const int harmonic = source - 20; - bank->motor = tailMotorIndex; - bank->ratio = tailGearRatio * harmonic * ratio; - bank->minHz = constrainf((config->filter_bank_rpm_limit[index] * harmonic) * ratio, 10, minHzLimit); - bank->maxHz = maxHzLimit; - bank->notchQ = notchQ; - bankNumber++; - } - else { - goto error; - } } + else { + PG_RESET(rpmFilterConfig); + } +} - // Set activeBankCount to the number of configured notches - activeBankCount = bankNumber; +INIT_CODE void rpmFilterInit(void) +{ + if (featureIsEnabled(FEATURE_RPM_FILTER)) + { + const rpmFilterConfig_t *config = rpmFilterConfig(); + const rpmNotchConfig_t *notch = &config->custom; + + const int mainMotorIndex = 0; + const int tailMotorIndex = mixerMotorizedTail() ? 1 : 0; + + const float mainGearRatio = getMainGearRatio(); + const float tailGearRatio = getTailGearRatio(); + + // Main motor fundamental used if not direct-drive + const bool enable10 = mainGearRatio != 1.0f; + + // Tail motor fundamental used if geared motorised tail and RPM available + const bool enable20 = tailGearRatio != 1.0f && mixerMotorizedTail() && isMotorFastRpmSourceActive(tailMotorIndex); + + // Tail harmonics are not used if tail motor RPM unavailable + const bool enable2x = !mixerMotorizedTail() || isMotorFastRpmSourceActive(tailMotorIndex); + + notchMaxHz = 0.45f * gyro.filterRateHz; + notchMinHz = constrain(config->min_hz, 5, 0.5f * notchMaxHz); + notchFadeHz = 1.2f * notchMinHz; + + #define CHECK_SOURCE(motor) if (!isMotorFastRpmSourceActive(motor)) goto error + + for (int axis = 0; axis < RPM_FILTER_AXIS_COUNT; axis++) { + for (int bank = 0; bank < RPM_FILTER_NOTCH_COUNT; bank++) { + if (notch->notch_source[axis][bank] && notch->notch_q[axis][bank]) + { + rpmFilterBank_t *filter = &filterBank[axis][bank]; + + // RPM source for this bank + const unsigned source = notch->notch_source[axis][bank]; + + // Convert RPM to Hz plus center adjustment + const float ratio = (1.0f + notch->notch_center[axis][bank] / 10000.0f) / 60.0f; + + // Q value + const float notchQ = constrainf(notch->notch_q[axis][bank], 10, 250) / 10; + + // Main Motor (M1) + if (source == 10) { + if (enable10) { + CHECK_SOURCE(mainMotorIndex); + filter->motor = mainMotorIndex; + filter->ratio = ratio; + filter->notchQ = notchQ; + } + } + // Main Rotor harmonics + else if (source >= 11 && source <= 18) { + CHECK_SOURCE(mainMotorIndex); + const int harmonic = source - 10; + filter->motor = mainMotorIndex; + filter->ratio = mainGearRatio * harmonic * ratio; + filter->notchQ = notchQ; + } + // Tail Motor (M2) + else if (source == 20) { + if (enable20) { + CHECK_SOURCE(tailMotorIndex); + filter->motor = tailMotorIndex; + filter->ratio = ratio; + filter->notchQ = notchQ; + } + } + // Tail Rotor harmonics + else if (source >= 21 && source <= 28) { + if (enable2x) { + CHECK_SOURCE(tailMotorIndex); + const int harmonic = source - 20; + filter->motor = tailMotorIndex; + filter->ratio = tailGearRatio * harmonic * ratio; + filter->notchQ = notchQ; + } + } + else { + goto error; + } + } + } + } - // Init all filters @minHz. As soon as the motor is running, the filters are updated to the real RPM. - for (int index = 0; index < activeBankCount; index++) { - rpmFilterBank_t *bank = &filterBank[index]; + // Init all filters @minHz. As soon as the motor is running, the filters are updated to the real RPM. for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInit(&bank->notch[axis], bank->minHz, gyro.filterRateHz, bank->notchQ, BIQUAD_NOTCH); + for (int bank = 0; bank < RPM_FILTER_NOTCH_COUNT; bank++) { + rpmFilterBank_t *filter = &filterBank[axis][bank]; + if (filter->notchQ) { + biquadFilterInit(&filter->notch, notchMinHz, gyro.filterRateHz, filter->notchQ, BIQUAD_NOTCH); + totalBankCount++; + } + } } + + // Number of banks to update in one update cycle + updateBankCount = MIN(totalBankCount, RPM_UPDATE_BANK_COUNT); } return; error: - activeBankCount = 0; - setArmingDisabled(ARMING_DISABLED_RPMFILTER); } FAST_CODE float rpmFilterGyro(int axis, float value) { - for (int index = 0; index < activeBankCount; index++) { - value = biquadFilterApplyDF1(&filterBank[index].notch[axis], value); + if (totalBankCount) { + for (int bank = 0; bank < RPM_FILTER_NOTCH_COUNT; bank++) { + rpmFilterBank_t *filter = &filterBank[axis][bank]; + if (filter->notchQ) { + value += (biquadFilterApplyDF1(&filter->notch, value) - value) * filter->fader; + } + } } return value; } void rpmFilterUpdate() { - if (activeBankCount > 0) { - + if (totalBankCount) + { // Actual update rate - allow ±25% variation const float updateRate = gyro.filterRateHz * constrainf(schedulerGetCycleTimeMultiplier(), 0.75f, 1.25f); // Number of banks to update in one update cycle - for (int i = 0; i < RPM_UPDATE_BANK_COUNT; i++) { - - // Current filter bank - rpmFilterBank_t *bank = &filterBank[updateBankNumber]; - - // Calculate notch filter center frequency - const float rpm = getMotorRPMf(bank->motor); - const float freq = rpm * bank->ratio; - const float notch = constrainf(freq, bank->minHz, bank->maxHz); - - // Notch filters for Roll,Pitch,Yaw - biquadFilter_t *R = &bank->notch[0]; - biquadFilter_t *P = &bank->notch[1]; - biquadFilter_t *Y = &bank->notch[2]; - - // Update the filter coefficients - biquadFilterUpdate(R, notch, updateRate, bank->notchQ, BIQUAD_NOTCH); - - // Transfer the filter coefficients from Roll axis filter into Pitch and Yaw - P->b0 = Y->b0 = R->b0; - P->b1 = Y->b1 = R->b1; - P->b2 = Y->b2 = R->b2; - P->a1 = Y->a1 = R->a1; - P->a2 = Y->a2 = R->a2; - - // Set debug if bank number matches - if (updateBankNumber == debugAxis) { - DEBUG(RPM_FILTER, 0, rpm); - DEBUG(RPM_FILTER, 1, freq * 10); - DEBUG(RPM_FILTER, 2, notch * 10); - DEBUG(RPM_FILTER, 3, updateRate * 10); - DEBUG(RPM_FILTER, 4, bank->motor); - DEBUG(RPM_FILTER, 5, bank->minHz * 10); - DEBUG(RPM_FILTER, 6, bank->maxHz * 10); - DEBUG(RPM_FILTER, 7, bank->notchQ * 10); + for (int count = 0; count < updateBankCount;) { + + // Current filter + rpmFilterBank_t *filter = &filterBank[updateAxisNumber][updateBankNumber]; + + if (filter->notchQ) { + + // Calculate notch filter center frequency + const float rpm = getMotorRPMf(filter->motor); + const float freq = rpm * filter->ratio; + const float center = constrainf(freq, 1, notchMaxHz); + + // Calculate fading + filter->fader = transition(freq, notchMinHz, notchFadeHz, 0, 1); + + // Update the filter coefficients + biquadFilterUpdate(&filter->notch, center, updateRate, filter->notchQ, BIQUAD_NOTCH); + + // Set debug if bank number matches + if (debugAxis == updateAxisNumber * RPM_FILTER_NOTCH_COUNT + updateBankNumber) { + DEBUG(RPM_FILTER, 0, rpm); + DEBUG(RPM_FILTER, 1, freq * 10); + DEBUG(RPM_FILTER, 2, center * 10); + DEBUG(RPM_FILTER, 3, updateRate * 10); + DEBUG(RPM_FILTER, 4, filter->motor); + DEBUG(RPM_FILTER, 6, filter->fader * 1000); + DEBUG(RPM_FILTER, 7, filter->notchQ * 10); + } + + // Bank updated + count++; } // Next bank - updateBankNumber = (updateBankNumber + 1) % activeBankCount; + updateBankNumber = (updateBankNumber + 1) % RPM_FILTER_NOTCH_COUNT; + if (updateBankNumber == 0) + updateAxisNumber = (updateAxisNumber + 1) % RPM_FILTER_AXIS_COUNT; } } } #endif - diff --git a/src/main/flight/rpm_filter.h b/src/main/flight/rpm_filter.h index 875fa3a6a6..fbc59b4b95 100644 --- a/src/main/flight/rpm_filter.h +++ b/src/main/flight/rpm_filter.h @@ -23,3 +23,5 @@ void rpmFilterInit(void); float rpmFilterGyro(int axis, float values); void rpmFilterUpdate(void); + +void validateAndFixRPMFilterConfig(void); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 0a36efa9fb..df9a91f65c 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1007,7 +1007,7 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce } case MSP_EXPERIMENTAL: - /* + /* * Send your experimental parameters to LUA. Like: * * sbufWriteU8(dst, currentPidProfile->yourFancyParameterA); @@ -1783,18 +1783,14 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, 0); sbufWriteU16(dst, 0); #endif - break; - -#ifdef USE_RPM_FILTER - case MSP_RPM_FILTER: - for (int i = 0; i < RPM_FILTER_BANK_COUNT; i++) { - sbufWriteU8(dst, rpmFilterConfig()->filter_bank_rpm_source[i]); - sbufWriteU16(dst, rpmFilterConfig()->filter_bank_rpm_ratio[i]); - sbufWriteU16(dst, rpmFilterConfig()->filter_bank_rpm_limit[i]); - sbufWriteU8(dst, rpmFilterConfig()->filter_bank_notch_q[i]); - } - break; +#if defined(USE_RPM_FILTER) + sbufWriteU8(dst, rpmFilterConfig()->preset); + sbufWriteU8(dst, rpmFilterConfig()->min_hz); +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); #endif + break; case MSP_PID_PROFILE: sbufWriteU8(dst, currentPidProfile->pid_mode); @@ -2022,8 +2018,22 @@ void mspGetOptionalIndexRange(sbuf_t *src, const range_t *range, range_t *value) static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) { - switch (cmdMSP) { +#ifdef USE_RPM_FILTER + case MSP_RPM_FILTER: + { + const range_t axis_range = { 0, RPM_FILTER_AXIS_COUNT - 1 }; + const range_t bank_range = { 0, RPM_FILTER_NOTCH_COUNT - 1 }; + int axis = 0, bank = 0; + mspGetOptionalIndex(src, &axis_range, &axis); + mspGetOptionalIndex(src, &bank_range, &bank); + sbufWriteU8(dst, rpmFilterConfig()->custom.notch_source[axis][bank]); + sbufWriteU16(dst, rpmFilterConfig()->custom.notch_center[axis][bank]); + sbufWriteU8(dst, rpmFilterConfig()->custom.notch_q[axis][bank]); + } + break; +#endif + case MSP_BOXNAMES: { const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0; @@ -2544,22 +2554,34 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU8(src); sbufReadU16(src); sbufReadU16(src); +#endif +#if defined(USE_RPM_FILTER) + rpmFilterConfigMutable()->preset = sbufReadU8(src); + rpmFilterConfigMutable()->min_hz = sbufReadU8(src); +#else + sbufReadU8(src); + sbufReadU8(src); #endif // reinitialize the gyro filters with the new values validateAndFixGyroConfig(); + validateAndFixRPMFilterConfig(); gyroInitFilters(); break; #ifdef USE_RPM_FILTER case MSP_SET_RPM_FILTER: - i = sbufReadU8(src); - if (i >= RPM_FILTER_BANK_COUNT) { - return MSP_RESULT_ERROR; + { + uint8_t axis = sbufReadU8(src); + uint8_t bank = sbufReadU8(src); + if (axis >= RPM_FILTER_AXIS_COUNT || bank >= RPM_FILTER_NOTCH_COUNT) { + return MSP_RESULT_ERROR; + } + rpmFilterConfigMutable()->custom.notch_source[axis][bank] = sbufReadU8(src); + rpmFilterConfigMutable()->custom.notch_center[axis][bank] = sbufReadU16(src); + rpmFilterConfigMutable()->custom.notch_q[axis][bank] = sbufReadU8(src); } - rpmFilterConfigMutable()->filter_bank_rpm_source[i] = sbufReadU8(src); - rpmFilterConfigMutable()->filter_bank_rpm_ratio[i] = sbufReadU16(src); - rpmFilterConfigMutable()->filter_bank_rpm_limit[i] = sbufReadU16(src); - rpmFilterConfigMutable()->filter_bank_notch_q[i] = sbufReadU8(src); + validateAndFixRPMFilterConfig(); + gyroInitFilters(); break; #endif diff --git a/src/main/pg/rpm_filter.c b/src/main/pg/rpm_filter.c index 83a1e39b58..f36f243543 100644 --- a/src/main/pg/rpm_filter.c +++ b/src/main/pg/rpm_filter.c @@ -23,7 +23,12 @@ #include "pg/pg_ids.h" #include "pg/rpm_filter.h" -PG_REGISTER(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 0); + +PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, + .preset = 2, + .min_hz = 20, +); #endif diff --git a/src/main/pg/rpm_filter.h b/src/main/pg/rpm_filter.h index 28716e4d37..2be500403e 100644 --- a/src/main/pg/rpm_filter.h +++ b/src/main/pg/rpm_filter.h @@ -19,14 +19,27 @@ #include "pg/pg.h" -#define RPM_FILTER_BANK_COUNT 16 +#define RPM_FILTER_AXIS_COUNT 3 +#define RPM_FILTER_NOTCH_COUNT 10 -typedef struct rpmFilteConfig_s +typedef struct { - uint8_t filter_bank_rpm_source[RPM_FILTER_BANK_COUNT]; // RPM source - uint16_t filter_bank_rpm_ratio[RPM_FILTER_BANK_COUNT]; // RPM ratio *1000 - uint16_t filter_bank_rpm_limit[RPM_FILTER_BANK_COUNT]; // RPM minimum limit - uint8_t filter_bank_notch_q[RPM_FILTER_BANK_COUNT]; // Notch Q *10 + uint8_t notch_source[RPM_FILTER_AXIS_COUNT][RPM_FILTER_NOTCH_COUNT]; // RPM source index + int16_t notch_center[RPM_FILTER_AXIS_COUNT][RPM_FILTER_NOTCH_COUNT]; // Center correction *10000 + uint8_t notch_q[RPM_FILTER_AXIS_COUNT][RPM_FILTER_NOTCH_COUNT]; // Notch Q *10 + +} rpmNotchConfig_t; + +typedef struct +{ + // Filter preset: 0 = custom, 1 = low, 2 = normal, 3 = high + uint8_t preset; + + // Hz limit + uint8_t min_hz; + + // Custom preset + rpmNotchConfig_t custom; } rpmFilterConfig_t; From 29dd9e84e3b1b33617206da152881cd3f36e2227 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Wed, 18 Dec 2024 16:21:32 +0000 Subject: [PATCH 2/5] Add BLACKBOX_PRINT_HEADER_LINE_ARRAY in blackbox.c --- src/main/blackbox/blackbox.c | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index cb22b26859..586fbc32a9 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1506,9 +1506,17 @@ static char *blackboxGetStartDateTime(char *buf) #define BLACKBOX_PRINT_HEADER_LINE(name, format, ...) case __COUNTER__: \ blackboxPrintfHeaderLine(name, format, __VA_ARGS__); \ break; +#define BLACKBOX_PRINT_HEADER_LINE_ARRAY(name, count, array) case __COUNTER__: { \ + char *ptr = buf; \ + for (int i=0; i<(count); i++) { \ + if (i > 0) *ptr++ = ','; \ + ptr += tfp_sprintf(ptr, "%d", array[i]); \ + } \ + blackboxPrintfHeaderLine(name, buf); \ + break; } #define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) case __COUNTER__: \ - {__VA_ARGS__}; \ - break; + {__VA_ARGS__}; \ + break; #endif /** From 3d8f422b24238ff08a33e1050e3e4e143b196b9c Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 13 Dec 2024 18:13:44 +0000 Subject: [PATCH 3/5] Add Blackbox headers for RPM filter --- src/main/blackbox/blackbox.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 586fbc32a9..f4b9f90feb 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1691,6 +1691,24 @@ static bool blackboxWriteSysinfo(void) #ifdef USE_RPM_FILTER BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_preset", "%d", rpmFilterConfig()->preset); BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_min_hz", "%d", rpmFilterConfig()->min_hz); + BLACKBOX_PRINT_HEADER_LINE_ARRAY("gyro_rpm_notch_source_pitch", + RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_source[FD_PITCH]); + BLACKBOX_PRINT_HEADER_LINE_ARRAY("gyro_rpm_notch_center_pitch", + RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_center[FD_PITCH]); + BLACKBOX_PRINT_HEADER_LINE_ARRAY("gyro_rpm_notch_q_pitch", + RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_q[FD_PITCH]); + BLACKBOX_PRINT_HEADER_LINE_ARRAY("gyro_rpm_notch_source_roll", + RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_source[FD_ROLL]); + BLACKBOX_PRINT_HEADER_LINE_ARRAY("gyro_rpm_notch_center_roll", + RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_center[FD_ROLL]); + BLACKBOX_PRINT_HEADER_LINE_ARRAY("gyro_rpm_notch_q_roll", + RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_q[FD_ROLL]); + BLACKBOX_PRINT_HEADER_LINE_ARRAY("gyro_rpm_notch_source_yaw", + RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_source[FD_YAW]); + BLACKBOX_PRINT_HEADER_LINE_ARRAY("gyro_rpm_notch_center_yaw", + RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_center[FD_YAW]); + BLACKBOX_PRINT_HEADER_LINE_ARRAY("gyro_rpm_notch_q_yaw", + RPM_FILTER_NOTCH_COUNT, rpmFilterConfig()->custom.notch_q[FD_YAW]); #endif #if defined(USE_ACC) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LPF_HZ, "%d", accelerometerConfig()->acc_lpf_hz * 100); From 19e49708e6dcb786df6684332daa5288b063ef50 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 13 Dec 2024 13:45:53 +0000 Subject: [PATCH 4/5] Amend RPM filter presets --- src/main/flight/rpm_filter.c | 56 +++++++++++++++++++----------------- src/main/pg/rpm_filter.h | 2 +- 2 files changed, 31 insertions(+), 27 deletions(-) diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index e345c391e3..bf10a702b3 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -40,12 +40,16 @@ #include "rpm_filter.h" // Number of banks to update in one cycle -#define RPM_UPDATE_BANK_COUNT 5 +#define RPM_UPDATE_BANK_COUNT 3 // Number of predefined presets #define RPM_FILTER_PRESET_COUNT 3 -// RPM Filter Presets +// Notch side-bands: 10000 * 2% / Q(x10) +#define LNC(Q) (-2000 / (Q)) +#define RNC(Q) ( 2000 / (Q)) + +// RPM Filter Preset static const rpmNotchConfig_t rpmFilterPreset[RPM_FILTER_PRESET_COUNT] = { // Low vibration preset @@ -56,50 +60,50 @@ static const rpmNotchConfig_t rpmFilterPreset[RPM_FILTER_PRESET_COUNT] = { 11, 12, 21, }, }, .notch_center = { - { 0, 0, 0, }, - { 0, 0, 0, }, - { 0, 0, 0, }, + { 0, }, + { 0, }, + { 0, }, }, .notch_q = { - { 100, 50, 50, }, - { 100, 50, 50, }, - { 100, 50, 50, }, + { 80, 40, 50, }, + { 80, 40, 50, }, + { 80, 40, 50, }, }, }, // Typical vibration preset { .notch_source = { - { 11, 12, 13, 14, 21, }, - { 11, 12, 13, 14, 21, }, - { 11, 12, 13, 14, 21, }, + { 11, 12, 13, 14, 21, 22, }, + { 11, 12, 13, 14, 21, 22, }, + { 11, 12, 13, 14, 21, 22, }, }, .notch_center = { - { 0, 0, 0, 0, 0, }, - { 0, 0, 0, 0, 0, }, - { 0, 0, 0, 0, 0, }, + { 0, }, + { 0, }, + { 0, }, }, .notch_q = { - { 80, 50, 80, 80, 50, }, - { 80, 50, 80, 80, 50, }, - { 80, 50, 80, 80, 50, }, + { 80, 30, 80, 60, 60, 50, }, + { 80, 30, 80, 60, 60, 50, }, + { 80, 40, 80, 80, 60, 50, }, }, }, // High vibration preset { .notch_source = { - { 11, 12, 13, 14, 15, 21, 22, 10 }, - { 11, 12, 13, 14, 15, 21, 22, 10 }, - { 11, 12, 13, 14, 15, 21, 22, 10 }, + { 11, 12, 12, 12, 13, 14, 15, 16, 21, 22, 10, }, + { 11, 12, 12, 12, 13, 14, 15, 16, 21, 10, }, + { 11, 12, 13, 14, 21, 22, 10, }, }, .notch_center = { - { 0, 0, 0, 0, 0, 0, 0, 0, }, - { 0, 0, 0, 0, 0, 0, 0, 0, }, - { 0, 0, 0, 0, 0, 0, 0, 0, }, + { 0, 0, LNC(30), RNC(30), 0, }, + { 0, 0, LNC(30), RNC(30), 0, }, + { 0, }, }, .notch_q = { - { 50, 30, 80, 80, 80, 50, 80, 80 }, - { 50, 30, 80, 80, 80, 50, 80, 80 }, - { 80, 50, 80, 80, 80, 50, 80, 80 }, + { 60, 30, 30, 30, 80, 60, 80, 80, 60, 50, 80 }, + { 60, 30, 30, 30, 80, 60, 80, 80, 60, 80 }, + { 80, 40, 80, 80, 60, 40, 80 }, }, }, }; diff --git a/src/main/pg/rpm_filter.h b/src/main/pg/rpm_filter.h index 2be500403e..b66f62b5e3 100644 --- a/src/main/pg/rpm_filter.h +++ b/src/main/pg/rpm_filter.h @@ -20,7 +20,7 @@ #include "pg/pg.h" #define RPM_FILTER_AXIS_COUNT 3 -#define RPM_FILTER_NOTCH_COUNT 10 +#define RPM_FILTER_NOTCH_COUNT 16 typedef struct { From 670157c5ec53370145a47b0273444b2c4d0d1d67 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Wed, 27 Nov 2024 11:44:44 +0000 Subject: [PATCH 5/5] Nuke MSP changes --- src/main/msp/msp.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index df9a91f65c..b876ff0ee4 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -2021,6 +2021,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_ switch (cmdMSP) { #ifdef USE_RPM_FILTER case MSP_RPM_FILTER: +#if 0 { const range_t axis_range = { 0, RPM_FILTER_AXIS_COUNT - 1 }; const range_t bank_range = { 0, RPM_FILTER_NOTCH_COUNT - 1 }; @@ -2031,6 +2032,11 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_ sbufWriteU16(dst, rpmFilterConfig()->custom.notch_center[axis][bank]); sbufWriteU8(dst, rpmFilterConfig()->custom.notch_q[axis][bank]); } +#else + for (int i = 0; i < 16 * 6; i++) { + sbufWriteU8(dst, 0); + } +#endif break; #endif @@ -2570,6 +2576,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #ifdef USE_RPM_FILTER case MSP_SET_RPM_FILTER: +#if 0 { uint8_t axis = sbufReadU8(src); uint8_t bank = sbufReadU8(src); @@ -2582,6 +2589,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, } validateAndFixRPMFilterConfig(); gyroInitFilters(); +#endif break; #endif