Fix RPM_FILTER MSP

rotorflight · Oct 4, 2024 · 9875fcc · 9875fcc
1 parent f68d7b6
commit 9875fcc
Showing 1 changed file with 19 additions and 12 deletions.
diff --git 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);