Merge pull request #11564 from sensei-hacker/feature/output-assignment-api-v2

output assignment: firmware-authoritative MSP2 READ/QUERY API

Author: sensei-hacker

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[] = {
@@ -300,14 +293,31 @@ static void pwmAssignOutput(timMotorServoHardware_t *timOutputs, timerHardware_t
     }
 }
 
-void pwmBuildTimerOutputList(timMotorServoHardware_t *timOutputs, bool isMixerUsingServos)
+void pwmBuildTimerOutputList(timMotorServoHardware_t *timOutputs)
 {
-    UNUSED(isMixerUsingServos);
     timOutputs->maxTimMotorCount = 0;
     timOutputs->maxTimServoCount = 0;
 
     const uint8_t motorCount = getMotorCount();
 
+    // Count servo outputs needed across all mixer profiles, matching
+    // computeServoCount() / getServoCount() which also walk all profiles.
+    // Using only the active profile would under-count on targets with a second
+    // profile that has more servos, causing the simulation to diverge from
+    // the real pwmInitServos() result.
+    uint8_t minServo = 255, maxServo = 0;
+    bool anyServo = false;
+    for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) {
+        for (int i = 0; i < MAX_SERVO_RULES; i++) {
+            if (mixerServoMixersByIndex(j)[i].rate == 0) break;
+            uint8_t ch = mixerServoMixersByIndex(j)[i].targetChannel;
+            if (ch < minServo) minServo = ch;
+            if (ch > maxServo) maxServo = ch;
+            anyServo = true;
+        }
+    }
+    uint8_t servoCount = anyServo ? (uint8_t)(1 + maxServo - minServo) : 0;
+
     // 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);
+    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);
+
+    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

@@ -1771,6 +1771,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);
@@ -4754,6 +4772,48 @@ 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);
+                // Reject malformed payloads: must be exactly timerCount pairs.
+                if (timerCount > HARDWARE_TIMER_DEFINITION_COUNT ||
+                    sbufBytesRemaining(src) != (uint32_t)(timerCount * 2)) {
+                    *ret = MSP_RESULT_ERROR;
+                    break;
+                }
+                for (int i = 0; i < timerCount; i++) {
+                    uint8_t timerId = sbufReadU8(src);
+                    uint8_t outputMode = sbufReadU8(src);
+                    if (timerId < HARDWARE_TIMER_DEFINITION_COUNT) {
+                        proposedModes[timerId] = outputMode;
+                    }
+                }
+            }
+
+            timMotorServoHardware_t tempOut = {0};
+            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