From 4ddef54b142f20eea5a605fa9b6c5797987ba331 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Thu, 3 Oct 2024 12:26:08 +0100 Subject: [PATCH 1/3] Use separate RPM filters banks for RPY --- src/main/cli/settings.c | 8 +- src/main/flight/rpm_filter.c | 290 ++++++++++++++++++----------------- src/main/pg/rpm_filter.h | 11 +- 3 files changed, 160 insertions(+), 149 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index a5d3c26b0d..1aae685d4b 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1689,10 +1689,10 @@ 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_filter_bank_rpm_source", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_PARAM_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_PARAM_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_PARAM_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_PARAM_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_notch_q) }, #endif #ifdef USE_RX_FLYSKY diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 797569ff13..477874f446 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -40,9 +40,9 @@ #include "rpm_filter.h" // Number of banks to update in one cycle -#define RPM_UPDATE_BANK_COUNT 1 +#define RPM_UPDATE_BANK_COUNT 4 -typedef struct rpmFilterBank_s +typedef struct { uint8_t motor; @@ -51,15 +51,17 @@ typedef struct rpmFilterBank_s float maxHz; float notchQ; - biquadFilter_t notch[XYZ_AXIS_COUNT]; + biquadFilter_t notch; } rpmFilterBank_t; -FAST_DATA_ZERO_INIT static rpmFilterBank_t filterBank[RPM_FILTER_BANK_COUNT]; +FAST_DATA_ZERO_INIT static rpmFilterBank_t filterBank[XYZ_AXIS_COUNT][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; INIT_CODE void rpmFilterInit(void) @@ -75,178 +77,186 @@ INIT_CODE void rpmFilterInit(void) const bool enable10 = mainGearRatio != 1.0f; const bool enable20 = tailGearRatio != 1.0f && mixerMotorizedTail(); - int bankNumber = 0; + bool useRPYconfig = false; + + /* Check if a separate R/P/Y config is used */ + for (int index = RPM_FILTER_BANK_COUNT; index < RPM_FILTER_PARAM_COUNT; index++) { + if (config->filter_bank_rpm_source[index] != 0 && + config->filter_bank_rpm_ratio[index] != 0 && + config->filter_bank_notch_q[index] != 0) { + useRPYconfig = true; + break; + } + } #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) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int bank = 0; bank < RPM_FILTER_BANK_COUNT; bank++) + { + const int index = useRPYconfig ? axis * RPM_FILTER_BANK_COUNT + bank : bank; + + 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 *filter = &filterBank[axis][bank]; + + // 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); + filter->motor = source - 1; + filter->ratio = ratio; + filter->minHz = constrainf(config->filter_bank_rpm_limit[index] * ratio, 10, minHzLimit); + filter->maxHz = maxHzLimit; + filter->notchQ = notchQ; + } + // Main Motor (M1) + else if (source == 10) { + if (enable10) { + CHECK_SOURCE(mainMotorIndex); + filter->motor = mainMotorIndex; + filter->ratio = ratio; + filter->minHz = constrainf((config->filter_bank_rpm_limit[index] / mainGearRatio) * ratio, 10, minHzLimit); + filter->maxHz = maxHzLimit; + filter->notchQ = notchQ; + } + } + // Main Rotor harmonics + else if (source >= 11 && source <= 18) { 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++; + const int harmonic = source - 10; + filter->motor = mainMotorIndex; + filter->ratio = mainGearRatio * harmonic * ratio; + filter->minHz = constrainf((config->filter_bank_rpm_limit[index] * harmonic) * ratio, 10, minHzLimit); + filter->maxHz = maxHzLimit; + filter->notchQ = notchQ; } - } - // 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) { + // Tail Motor (M2) + else if (source == 20) { + if (enable20) { + CHECK_SOURCE(tailMotorIndex); + filter->motor = tailMotorIndex; + filter->ratio = ratio; + filter->minHz = constrainf((config->filter_bank_rpm_limit[index] / tailGearRatio) * ratio, 10, minHzLimit); + filter->maxHz = maxHzLimit; + filter->notchQ = notchQ; + } + } + // Tail Rotor harmonics + else if (source >= 21 && source <= 28) { 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++; + const int harmonic = source - 20; + filter->motor = tailMotorIndex; + filter->ratio = tailGearRatio * harmonic * ratio; + filter->minHz = constrainf((config->filter_bank_rpm_limit[index] * harmonic) * ratio, 10, minHzLimit); + filter->maxHz = maxHzLimit; + filter->notchQ = notchQ; + } + else { + goto error; } - } - // 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; } } - // Set activeBankCount to the number of configured notches - activeBankCount = bankNumber; - // 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]; - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInit(&bank->notch[axis], bank->minHz, gyro.filterRateHz, bank->notchQ, BIQUAD_NOTCH); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int bank = 0; bank < RPM_FILTER_BANK_COUNT; bank++) { + rpmFilterBank_t *filter = &filterBank[axis][bank]; + if (filter->notchQ) { + biquadFilterInit(&filter->notch, filter->minHz, 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); + for (int bank = 0; bank < RPM_FILTER_BANK_COUNT; bank++) { + rpmFilterBank_t *filter = &filterBank[axis][bank]; + if (filter->notchQ) { + value = biquadFilterApplyDF1(&filter->notch, value); + } } 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 notch = constrainf(freq, filter->minHz, filter->maxHz); + + // Update the filter coefficients + biquadFilterUpdate(&filter->notch, notch, updateRate, filter->notchQ, BIQUAD_NOTCH); + + // Set debug if bank number matches + if (debugAxis == updateAxisNumber * RPM_FILTER_BANK_COUNT + updateBankNumber) { + 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, filter->motor); + DEBUG(RPM_FILTER, 5, filter->minHz * 10); + DEBUG(RPM_FILTER, 6, filter->maxHz * 10); + DEBUG(RPM_FILTER, 7, filter->notchQ * 10); + } + + // Bank updated + count++; } // Next bank - updateBankNumber = (updateBankNumber + 1) % activeBankCount; + updateBankNumber = (updateBankNumber + 1) % RPM_FILTER_BANK_COUNT; + if (updateBankNumber == 0) + updateAxisNumber = (updateAxisNumber + 1) % XYZ_AXIS_COUNT; } } } #endif - diff --git a/src/main/pg/rpm_filter.h b/src/main/pg/rpm_filter.h index 28716e4d37..42d405ad66 100644 --- a/src/main/pg/rpm_filter.h +++ b/src/main/pg/rpm_filter.h @@ -19,14 +19,15 @@ #include "pg/pg.h" -#define RPM_FILTER_BANK_COUNT 16 +#define RPM_FILTER_BANK_COUNT 16 +#define RPM_FILTER_PARAM_COUNT (3 * RPM_FILTER_BANK_COUNT) typedef struct rpmFilteConfig_s { - 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 filter_bank_rpm_source[RPM_FILTER_PARAM_COUNT]; // RPM source index + uint16_t filter_bank_rpm_ratio[RPM_FILTER_PARAM_COUNT]; // RPM ratio *1000 + uint16_t filter_bank_rpm_limit[RPM_FILTER_PARAM_COUNT]; // RPM low limit + uint8_t filter_bank_notch_q[RPM_FILTER_PARAM_COUNT]; // Notch Q *10 } rpmFilterConfig_t; From 40e692b24a7979eb7c2d437cdc75ef16e54ea80f Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 4 Oct 2024 15:34:32 +0100 Subject: [PATCH 2/3] Fix RPM_FILTER MSP --- src/main/common/types.h | 6 ++++++ src/main/msp/msp.c | 48 ++++++++++++++++++++++++++++++----------- 2 files changed, 41 insertions(+), 13 deletions(-) diff --git a/src/main/common/types.h b/src/main/common/types.h index 195a93042b..6090398192 100644 --- a/src/main/common/types.h +++ b/src/main/common/types.h @@ -27,3 +27,9 @@ typedef unsigned char uchar; typedef uint32_t bitmap_t; typedef uint64_t bitmap64_t; + +typedef struct { + int min; + int max; +} range_t; + diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index f0596b8c1b..aff2f0098e 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1764,17 +1764,6 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) #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; -#endif - case MSP_PID_PROFILE: sbufWriteU8(dst, currentPidProfile->pid_mode); sbufWriteU8(dst, currentPidProfile->error_decay_time_ground); @@ -1980,10 +1969,43 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) return !unsupportedCommand; } -static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) +void mspGetOptionalIndex(sbuf_t *src, const range_t *range, int *value) +{ + if (sbufBytesRemaining(src) > 0) { + *value = constrain(sbufReadU8(src), range->min, range->max); + } +} + +void mspGetOptionalIndexRange(sbuf_t *src, const range_t *range, range_t *value) { + if (sbufBytesRemaining(src) > 0) { + value->min = value->max = constrain(sbufReadU8(src), range->min, range->max); + if (sbufBytesRemaining(src) > 0) { + value->max = constrain(sbufReadU8(src), value->min, range->max); + } + } +} +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 range = { 0, 2 }; + int axis = 0; + mspGetOptionalIndex(src, &range, &axis); + for (int i = 0; i < RPM_FILTER_BANK_COUNT; i++) { + const int index = axis * RPM_FILTER_BANK_COUNT + i; + sbufWriteU8(dst, rpmFilterConfig()->filter_bank_rpm_source[index]); + sbufWriteU16(dst, rpmFilterConfig()->filter_bank_rpm_ratio[index]); + sbufWriteU16(dst, rpmFilterConfig()->filter_bank_rpm_limit[index]); + sbufWriteU8(dst, rpmFilterConfig()->filter_bank_notch_q[index]); + } + } + break; +#endif + case MSP_BOXNAMES: { const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0; @@ -2513,7 +2535,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #ifdef USE_RPM_FILTER case MSP_SET_RPM_FILTER: i = sbufReadU8(src); - if (i >= RPM_FILTER_BANK_COUNT) { + if (i >= RPM_FILTER_PARAM_COUNT) { return MSP_RESULT_ERROR; } rpmFilterConfigMutable()->filter_bank_rpm_source[i] = sbufReadU8(src); From e27cfb0ab6a6f3b2b5e4539287763209090ee611 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 11 Oct 2024 10:19:16 +0100 Subject: [PATCH 3/3] Fix RPM filter parameter check --- src/main/config/config.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index ce3a392020..8d36fb7353 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -668,9 +668,10 @@ void validateAndFixGyroConfig(void) #ifdef USE_RPM_FILTER if (featureIsConfigured(FEATURE_RPM_FILTER)) { rpmFilterConfig_t *config = rpmFilterConfigMutable(); - for (int index = 0; index < RPM_FILTER_BANK_COUNT; index++) { + for (int index = 0; index < RPM_FILTER_PARAM_COUNT; index++) { if (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) { config->filter_bank_rpm_source[index] = 0;