From 9875fcc0d3d477fa0a0c79f570c2008f0a556c0f Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 4 Oct 2024 15:34:32 +0100 Subject: [PATCH] Fix RPM_FILTER MSP --- src/main/msp/msp.c | 31 +++++++++++++++++++------------ 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index f0596b8c1b..31a80d0d7f 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); @@ -1984,6 +1973,24 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_ { switch (cmdMSP) { +#ifdef USE_RPM_FILTER + case MSP_RPM_FILTER: + { + int axis = 0; + if (sbufBytesRemaining(src) > 0) { + axis = constrain(sbufReadU8(src), 0, 2); + } + 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 +2520,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);