output assignment: firmware-authoritative MSP2 READ/QUERY API

src/main/drivers/pwm_mapping.c

@@ -51,13 +51,6 @@ enum {
     MAP_TO_LED_OUTPUT
 };
 
-typedef struct {
-    int maxTimMotorCount;
-    int maxTimServoCount;
-    const timerHardware_t * timMotors[MAX_PWM_OUTPUTS];
-    const timerHardware_t * timServos[MAX_PWM_OUTPUTS];
-} timMotorServoHardware_t;
-
 static pwmInitError_e pwmInitError = PWM_INIT_ERROR_NONE;
 
 static const char * pwmInitErrorMsg[] = {
@@ -302,12 +295,29 @@ static void pwmAssignOutput(timMotorServoHardware_t *timOutputs, timerHardware_t
 
 void pwmBuildTimerOutputList(timMotorServoHardware_t *timOutputs, bool isMixerUsingServos)
 {
-    UNUSED(isMixerUsingServos);
     timOutputs->maxTimMotorCount = 0;
     timOutputs->maxTimServoCount = 0;
 
     const uint8_t motorCount = getMotorCount();
 
+    // Count servo outputs needed from customServoMixers — the same PG array
+    // that the Configurator writes via MSP2_INAV_SET_SERVO_MIXER and that
+    // loadCustomServoMixer() uses at runtime.  getServoCount() reads from
+    // mixerProfiles->ServoMixers, a separate PG that the Configurator does
+    // not clear, which causes phantom servo outputs after settings-preserving
+    // firmware upgrades.
+    // Servo count comes directly from customServoMixers — the PG that the
+    // Configurator writes via MSP2_INAV_SET_SERVO_MIXER and that
+    // loadCustomServoMixer() uses.  Do not gate on isMixerUsingServos():
+    // that flag uses mixerProfiles->ServoMixers (a separate PG) which can
+    // disagree with customServoMixers due to stale data.
+    uint8_t servoCount = 0;
+    for (int i = 0; i < MAX_SERVO_RULES; i++) {
+        if (customServoMixers(i)->rate == 0) break;
+        uint8_t ch = customServoMixers(i)->targetChannel;
+        if (ch + 1 > servoCount) servoCount = ch + 1;
+    }
+
     // Apply all timerOverrides upfront so flag state is stable for both passes
     for (int idx = 0; idx < timerHardwareCount; idx++) {
         timerHardwareOverride(&timerHardware[idx]);
@@ -341,7 +351,7 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t *timOutputs, bool isMixerUs
             }
 
             // Servos: dedicated (OUTPUT_MODE_SERVOS) first, then auto
-            if (TIM_IS_SERVO(timHw->usageFlags)
+            if (TIM_IS_SERVO(timHw->usageFlags) && timOutputs->maxTimServoCount < servoCount
                     && !pwmHasMotorOnTimer(timOutputs, timHw->tim)
                     && (isDedicated ? mode == OUTPUT_MODE_SERVOS : mode != OUTPUT_MODE_SERVOS)) {
                 pwmAssignOutput(timOutputs, timHw, MAP_TO_SERVO_OUTPUT);
@@ -459,19 +469,61 @@ static void pwmInitServos(timMotorServoHardware_t * timOutputs)
 }
 
 
+static timMotorServoHardware_t timOutputsStatic;
+
 bool pwmMotorAndServoInit(void)
 {
-    timMotorServoHardware_t timOutputs;
+    pwmBuildTimerOutputList(&timOutputsStatic, isMixerUsingServos());
+    pwmInitMotors(&timOutputsStatic);
+    pwmInitServos(&timOutputsStatic);
+    return (pwmInitError == PWM_INIT_ERROR_NONE);
+}
 
-    // Build temporary timer mappings for motor and servo
-    pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos());
+const timMotorServoHardware_t *pwmGetOutputAssignment(void)
+{
+    return &timOutputsStatic;
+}
 
-    // At this point we have built tables of timers suitable for motor and servo mappings
-    // Now we can actually initialize them according to motor/servo count from mixer
-    pwmInitMotors(&timOutputs);
-    pwmInitServos(&timOutputs);
+// Upper bound for timerHardware[] size across all supported targets.
+// timerHardwareCount is a runtime value; this constant prevents a VLA on the MSP
+// task stack. Targets with 22+ entries (e.g. OMNIBUSF4) need more than MAX_PWM_OUTPUTS.
+#define TIMER_HW_MAX 64
 
-    return (pwmInitError == PWM_INIT_ERROR_NONE);
+// Simulate pwmBuildTimerOutputList() with proposed overrides without modifying live state.
+// IMPORTANT: Must only be called from the main loop / MSP task — not ISR-safe.
+// timerHardware[].usageFlags are modified in-place during the simulation; this function
+// saves and restores them so hardware state is identical on exit.
+void pwmCalculateAssignment(timMotorServoHardware_t *out, const uint8_t *proposedModes)
+{
+    if (timerHardwareCount > TIMER_HW_MAX) {
+        return; // Safety guard: target exceeds the buffer — increase TIMER_HW_MAX
+    }
+
+    // Snapshot timerHardware flags — pwmBuildTimerOutputList() modifies them in-place
+    // via timerHardwareOverride() and pwmClaimTimer().
+    uint32_t savedFlags[TIMER_HW_MAX];
+    for (int i = 0; i < timerHardwareCount; i++) {
+        savedFlags[i] = timerHardware[i].usageFlags;
+    }
+
+    // Snapshot timerOverrides config so the proposed values can be applied temporarily.
+    uint8_t savedModes[HARDWARE_TIMER_DEFINITION_COUNT];
+    for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
+        savedModes[i] = timerOverrides(i)->outputMode;
+    }
+
+    for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
+        timerOverridesMutable(i)->outputMode = proposedModes[i];
+    }
+
+    pwmBuildTimerOutputList(out, isMixerUsingServos());
+
+    for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
+        timerOverridesMutable(i)->outputMode = savedModes[i];
+    }
+    for (int i = 0; i < timerHardwareCount; i++) {
+        timerHardware[i].usageFlags = savedFlags[i];
+    }
 }
 
 #endif

src/main/drivers/pwm_mapping.h

@@ -18,6 +18,8 @@
 #pragma once
 
 #include "drivers/io_types.h"
+#include "drivers/timer.h"
+#include "drivers/pwm_output.h"
 #include "flight/mixer.h"
 #include "flight/mixer_profile.h"
 #include "flight/servos.h"
@@ -78,7 +80,26 @@ typedef struct {
     bool isDSHOT;
 } motorProtocolProperties_t;
 
+#ifndef SITL_BUILD
+typedef struct {
+    int maxTimMotorCount;
+    int maxTimServoCount;
+    const timerHardware_t * timMotors[MAX_PWM_OUTPUTS];
+    const timerHardware_t * timServos[MAX_PWM_OUTPUTS];
+} timMotorServoHardware_t;
+
+// Output assignment types for MSP2_INAV_OUTPUT_ASSIGNMENT response
+// LED outputs are not reported here; they are already identified by TIM_USE_LED
+// in the MSP2_INAV_OUTPUT_MAPPING_EXT2 usageFlags response.
+#define OUTPUT_ASSIGNMENT_TYPE_MOTOR 1
+#define OUTPUT_ASSIGNMENT_TYPE_SERVO 2
+#endif // SITL_BUILD
+
 bool pwmMotorAndServoInit(void);
 const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto);
 pwmInitError_e getPwmInitError(void);
 const char * getPwmInitErrorMessage(void);
+#ifndef SITL_BUILD
+const timMotorServoHardware_t *pwmGetOutputAssignment(void);
+void pwmCalculateAssignment(timMotorServoHardware_t *out, const uint8_t *proposedModes);
+#endif // SITL_BUILD

src/main/fc/fc_msp.c

@@ -1728,6 +1728,24 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
 
 
+#ifndef SITL_BUILD
+    case MSP2_INAV_OUTPUT_ASSIGNMENT:
+        {
+            const timMotorServoHardware_t *hw = pwmGetOutputAssignment();
+            for (int m = 0; m < hw->maxTimMotorCount; m++) {
+                sbufWriteU8(dst, (uint8_t)(hw->timMotors[m] - timerHardware));
+                sbufWriteU8(dst, OUTPUT_ASSIGNMENT_TYPE_MOTOR);
+                sbufWriteU8(dst, (uint8_t)(m + 1));
+            }
+            for (int s = 0; s < hw->maxTimServoCount; s++) {
+                sbufWriteU8(dst, (uint8_t)(hw->timServos[s] - timerHardware));
+                sbufWriteU8(dst, OUTPUT_ASSIGNMENT_TYPE_SERVO);
+                sbufWriteU8(dst, (uint8_t)(s + 1));
+            }
+        }
+        break;
+#endif
+
     case MSP2_INAV_MC_BRAKING:
 #ifdef USE_MR_BRAKING_MODE
         sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold);
@@ -4710,6 +4728,45 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
             *ret = MSP_RESULT_ERROR;
         }
         break;
+
+    case MSP2_INAV_QUERY_OUTPUT_ASSIGNMENT:
+        {
+            // Build proposed overrides array (defaults to current stored overrides)
+            uint8_t proposedModes[HARDWARE_TIMER_DEFINITION_COUNT];
+            for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
+                proposedModes[i] = timerOverrides(i)->outputMode;
+            }
+
+            if (dataSize >= 1) {
+                uint8_t timerCount = sbufReadU8(src);
+                if (timerCount > HARDWARE_TIMER_DEFINITION_COUNT) {
+                    timerCount = HARDWARE_TIMER_DEFINITION_COUNT;
+                }
+                for (int i = 0; i < timerCount && sbufBytesRemaining(src) >= 2; i++) {
+                    uint8_t timerId = sbufReadU8(src);
+                    uint8_t outputMode = sbufReadU8(src);
+                    if (timerId < HARDWARE_TIMER_DEFINITION_COUNT) {
+                        proposedModes[timerId] = outputMode;
+                    }
+                }
+            }
+
+            timMotorServoHardware_t tempOut;
+            pwmCalculateAssignment(&tempOut, proposedModes);
+
+            for (int m = 0; m < tempOut.maxTimMotorCount; m++) {
+                sbufWriteU8(dst, (uint8_t)(tempOut.timMotors[m] - timerHardware));
+                sbufWriteU8(dst, OUTPUT_ASSIGNMENT_TYPE_MOTOR);
+                sbufWriteU8(dst, (uint8_t)(m + 1));
+            }
+            for (int s = 0; s < tempOut.maxTimServoCount; s++) {
+                sbufWriteU8(dst, (uint8_t)(tempOut.timServos[s] - timerHardware));
+                sbufWriteU8(dst, OUTPUT_ASSIGNMENT_TYPE_SERVO);
+                sbufWriteU8(dst, (uint8_t)(s + 1));
+            }
+            *ret = MSP_RESULT_ACK;
+        }
+        break;
 #endif
 
     case MSP_VTXTABLE_POWERLEVEL: {

src/main/msp/msp_protocol_v2_inav.h

@@ -35,6 +35,8 @@
 #define MSP2_INAV_TIMER_OUTPUT_MODE             0x200E
 #define MSP2_INAV_SET_TIMER_OUTPUT_MODE         0x200F
 #define MSP2_INAV_OUTPUT_MAPPING_EXT2           0x210D
+#define MSP2_INAV_OUTPUT_ASSIGNMENT             0x210E  // Read finalized post-boot output assignments
+#define MSP2_INAV_QUERY_OUTPUT_ASSIGNMENT       0x210F  // Preview assignments for proposed timer overrides
 
 #define MSP2_INAV_MIXER                         0x2010
 #define MSP2_INAV_SET_MIXER                     0x2011