From d243eed57595947f866d1f9f44ffd1ccd8bbb29e Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 15 May 2026 20:32:05 -0500 Subject: [PATCH 01/10] output assignment: firmware-authoritative MSP2 READ/QUERY API MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Exposes the firmware's post-boot output assignment as an MSP message so the Configurator no longer needs to mirror the assignment algorithm in JS. ## New MSP messages MSP2_INAV_OUTPUT_ASSIGNMENT (0x210E) — READ Response: [(u8 outputIndex, u8 type, u8 number)] per assigned output type: 1=MOTOR 2=SERVO 3=LED; number: 1-indexed ordinal Sent once on Mixer tab load; Configurator uses this for display instead of running getTimerMap() locally. MSP2_INAV_QUERY_OUTPUT_ASSIGNMENT (0x210F) — QUERY/preview Request: (u8 timerCount, [u8 timerId, u8 outputMode] × N) Response: same format as READ Lets Configurator preview assignments for proposed timer overrides without save+reboot. Firmware runs its own algorithm on the proposed overrides (save/restore approach, no hardware side-effects) and returns the result. ## Implementation - pwm_mapping.h: expose timMotorServoHardware_t; add OUTPUT_ASSIGNMENT_TYPE_* defines; declare pwmGetOutputAssignment() and pwmCalculateAssignment() - pwm_mapping.c: promote timOutputs to static; add getter and pure-function calculator (saves/restores timerHardware flags and timerOverrides config) - fc_msp.c: READ handler in mspFcProcessOutCommand (#ifndef SITL_BUILD); QUERY handler in mspFCProcessInOutCommand (#ifndef SITL_BUILD) - msp_protocol_v2_inav.h: reserve 0x210E and 0x210F ## Fallback Old firmware returns error/no-data; Configurator falls back to getTimerMap() (the existing JS algorithm). No behaviour change for users on older firmware. Companion configurator PR: iNavFlight/inav-configurator#XXXX --- src/main/drivers/pwm_mapping.c | 65 ++++++++++++++++++++++------- src/main/drivers/pwm_mapping.h | 17 ++++++++ src/main/fc/fc_msp.c | 61 +++++++++++++++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 2 + 4 files changed, 130 insertions(+), 15 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index c37ae1775a7..3adf04baed7 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/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[] = { @@ -459,19 +452,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); +} + +const timMotorServoHardware_t *pwmGetOutputAssignment(void) +{ + return &timOutputsStatic; +} - // Build temporary timer mappings for motor and servo - pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos()); +// 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 - // 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); +// 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 + } - return (pwmInitError == PWM_INIT_ERROR_NONE); + // 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 diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index c860166bc74..9851abc4009 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/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,22 @@ typedef struct { bool isDSHOT; } motorProtocolProperties_t; +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 + bool pwmMotorAndServoInit(void); const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto); pwmInitError_e getPwmInitError(void); const char * getPwmInitErrorMessage(void); +const timMotorServoHardware_t *pwmGetOutputAssignment(void); +void pwmCalculateAssignment(timMotorServoHardware_t *out, const uint8_t *proposedModes); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bef7c54de40..7e8823f983d 100644 --- a/src/main/fc/fc_msp.c +++ b/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,49 @@ 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++) { + ptrdiff_t idx = tempOut.timMotors[m] - timerHardware; + ASSERT(idx >= 0 && idx < timerHardwareCount); + sbufWriteU8(dst, (uint8_t)idx); + sbufWriteU8(dst, OUTPUT_ASSIGNMENT_TYPE_MOTOR); + sbufWriteU8(dst, (uint8_t)(m + 1)); + } + for (int s = 0; s < tempOut.maxTimServoCount; s++) { + ptrdiff_t idx = tempOut.timServos[s] - timerHardware; + ASSERT(idx >= 0 && idx < timerHardwareCount); + sbufWriteU8(dst, (uint8_t)idx); + sbufWriteU8(dst, OUTPUT_ASSIGNMENT_TYPE_SERVO); + sbufWriteU8(dst, (uint8_t)(s + 1)); + } + *ret = MSP_RESULT_ACK; + } + break; #endif case MSP_VTXTABLE_POWERLEVEL: { diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index b9aeb6b879c..f945d87d82f 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/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 From f860c811d5bab2594dcce59b8447f70dba7baf59 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 15 May 2026 23:43:45 -0500 Subject: [PATCH 02/10] fix: guard timMotorServoHardware_t in pwm_mapping.h for SITL builds MAX_PWM_OUTPUTS expands to MAX_PWM_OUTPUT_PORTS which is not defined in SITL targets. The struct and its dependent function declarations are hardware-only; wrap them in #ifndef SITL_BUILD to match the existing pattern used in fc_msp.c for the MSP handlers. --- src/main/drivers/pwm_mapping.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 9851abc4009..4a6722046c5 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -80,6 +80,7 @@ typedef struct { bool isDSHOT; } motorProtocolProperties_t; +#ifndef SITL_BUILD typedef struct { int maxTimMotorCount; int maxTimServoCount; @@ -92,10 +93,13 @@ typedef struct { // 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 From be6b3995e1840a69da82d3e4d80d4f3b2d75988b Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 15 May 2026 23:44:10 -0500 Subject: [PATCH 03/10] fix: remove ASSERT calls from MSP QUERY handler in fc_msp.c MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit INAV has no runtime ASSERT macro — STATIC_ASSERT is compile-time only. The ptrdiff_t bounds checks were unnecessary since timMotors/timServos pointers are guaranteed to point within timerHardware[] by construction in pwmBuildTimerOutputList(). Replace with the direct cast used in the READ handler. --- src/main/fc/fc_msp.c | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7e8823f983d..79adfca4abd 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4755,16 +4755,12 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu pwmCalculateAssignment(&tempOut, proposedModes); for (int m = 0; m < tempOut.maxTimMotorCount; m++) { - ptrdiff_t idx = tempOut.timMotors[m] - timerHardware; - ASSERT(idx >= 0 && idx < timerHardwareCount); - sbufWriteU8(dst, (uint8_t)idx); + 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++) { - ptrdiff_t idx = tempOut.timServos[s] - timerHardware; - ASSERT(idx >= 0 && idx < timerHardwareCount); - sbufWriteU8(dst, (uint8_t)idx); + sbufWriteU8(dst, (uint8_t)(tempOut.timServos[s] - timerHardware)); sbufWriteU8(dst, OUTPUT_ASSIGNMENT_TYPE_SERVO); sbufWriteU8(dst, (uint8_t)(s + 1)); } From 2353382f52b84fbe69e48fe1f69054e85497e298 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 16 May 2026 08:57:29 -0500 Subject: [PATCH 04/10] fix: honor isMixerUsingServos and servo count in pwmBuildTimerOutputList MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The isMixerUsingServos parameter was marked UNUSED; the function populated timServos[] with all servo-capable hardware pins regardless of how many servos the mixer actually needs. This caused MSP2_INAV_OUTPUT_ASSIGNMENT to report phantom servo assignments — e.g., reporting 4 servo outputs on a quadcopter with only 1 servo in the mixer. Fix mirrors the existing motor pattern: only add a servo slot when maxTimServoCount < servoCount, and skip all servo slots when isMixerUsingServos is false. --- src/main/drivers/pwm_mapping.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 3adf04baed7..2662525f2a3 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -295,11 +295,11 @@ 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(); + const uint8_t servoCount = isMixerUsingServos ? (uint8_t)getServoCount() : 0; // Apply all timerOverrides upfront so flag state is stable for both passes for (int idx = 0; idx < timerHardwareCount; idx++) { @@ -334,7 +334,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); From 6492483310a4f08b2fd7e27256a250aab4fdf942 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 16 May 2026 10:25:25 -0500 Subject: [PATCH 05/10] fix: count servo outputs from customServoMixers, not mixerProfiles MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit pwmBuildTimerOutputList() previously used getServoCount() which reads mixerProfiles->ServoMixers via computeServoCount(). That PG array is never cleared by the Configurator's MSP2_INAV_SET_SERVO_MIXER handler (which writes only to customServoMixers). Stale rules in mixerProfiles->ServoMixers survive settings-preserving firmware flashes and cause phantom servo outputs even with an empty servo mixer. Fix: count servo outputs directly from customServoMixers — the same PG source that loadCustomServoMixer() and the CLI smix command use for the current profile. The inactive profile (dual-profile setups) continues to use mixerServoMixersByIndex(nextMixerProfileIndex) which is populated when the user saves a profile via the Configurator. --- src/main/drivers/pwm_mapping.c | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 2662525f2a3..fda98b16b78 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -299,7 +299,29 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t *timOutputs, bool isMixerUs timOutputs->maxTimServoCount = 0; const uint8_t motorCount = getMotorCount(); - const uint8_t servoCount = isMixerUsingServos ? (uint8_t)getServoCount() : 0; + + // 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. + uint8_t servoCount = 0; + if (isMixerUsingServos) { + 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; + } + // Also include the inactive profile's rules (dual-profile setups) + if (MAX_MIXER_PROFILE_COUNT > 1) { + for (int i = 0; i < MAX_SERVO_RULES; i++) { + if (mixerServoMixersByIndex(nextMixerProfileIndex)[i].rate == 0) break; + uint8_t ch = mixerServoMixersByIndex(nextMixerProfileIndex)[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++) { From 2e92dde937dd9c497c89ecb6ac3c8a56ab809c12 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 16 May 2026 10:35:01 -0500 Subject: [PATCH 06/10] fix: use only customServoMixers for servo count in pwmBuildTimerOutputList MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The inactive profile's mixerProfiles(j)->ServoMixers is a separate PG that the Configurator never writes to via MSP2_INAV_SET_SERVO_MIXER, so it can contain stale data from old configurations that survives settings-preserving firmware upgrades. Checking it caused phantom servo outputs whenever that stale data had any non-zero rate rule. Use only customServoMixers — the Configurator-facing PG — to determine how many servo outputs to allocate. Dual-profile output reallocation is handled at runtime by outputProfileHotSwitch() when the user switches mixer profiles. --- src/main/drivers/pwm_mapping.c | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index fda98b16b78..b56c727125f 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -313,14 +313,6 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t *timOutputs, bool isMixerUs uint8_t ch = customServoMixers(i)->targetChannel; if (ch + 1 > servoCount) servoCount = ch + 1; } - // Also include the inactive profile's rules (dual-profile setups) - if (MAX_MIXER_PROFILE_COUNT > 1) { - for (int i = 0; i < MAX_SERVO_RULES; i++) { - if (mixerServoMixersByIndex(nextMixerProfileIndex)[i].rate == 0) break; - uint8_t ch = mixerServoMixersByIndex(nextMixerProfileIndex)[i].targetChannel; - if (ch + 1 > servoCount) servoCount = ch + 1; - } - } } // Apply all timerOverrides upfront so flag state is stable for both passes From f75c421c2f46244b440bb13b83b8d0dceee5b8c4 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 16 May 2026 11:39:16 -0500 Subject: [PATCH 07/10] fix: scan customServoMixers unconditionally for servo count MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Remove the isMixerUsingServos gate: that flag reads mixerProfiles->ServoMixers which can disagree with customServoMixers in both directions (stale non-zero data → phantom servos; clean data when real rules exist → zero servos). Scanning customServoMixers directly and unconditionally is always correct — an empty mixer (all rate=0) naturally produces servoCount=0 without needing the flag. --- src/main/drivers/pwm_mapping.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index b56c727125f..6291dccf39c 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -306,13 +306,16 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t *timOutputs, bool isMixerUs // 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; - if (isMixerUsingServos) { - 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; - } + 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 From 1df6a5b9b8e7c61880982eea555b26f3606825b1 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 16 May 2026 23:33:23 -0500 Subject: [PATCH 08/10] fix: remove unused parameter from pwmBuildTimerOutputList MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The isMixerUsingServos parameter was never read inside the function body — only the isMixerUsingServos() function was called. Remove the dead parameter and update both call sites to fix -Werror=unused-parameter CI failure. --- src/main/drivers/pwm_mapping.c | 21 +++++++-------------- 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 6291dccf39c..cb537b4a6fe 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -293,24 +293,17 @@ static void pwmAssignOutput(timMotorServoHardware_t *timOutputs, timerHardware_t } } -void pwmBuildTimerOutputList(timMotorServoHardware_t *timOutputs, bool isMixerUsingServos) +void pwmBuildTimerOutputList(timMotorServoHardware_t *timOutputs) { 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. + // Servo count from customServoMixers — the PG that the Configurator writes via + // MSP2_INAV_SET_SERVO_MIXER and that loadCustomServoMixer() uses at runtime. + // Do NOT use getServoCount() / isMixerUsingServos(): those read mixerProfiles->ServoMixers, + // a separate PG that can hold stale data after settings-preserving firmware upgrades. uint8_t servoCount = 0; for (int i = 0; i < MAX_SERVO_RULES; i++) { if (customServoMixers(i)->rate == 0) break; @@ -473,7 +466,7 @@ static timMotorServoHardware_t timOutputsStatic; bool pwmMotorAndServoInit(void) { - pwmBuildTimerOutputList(&timOutputsStatic, isMixerUsingServos()); + pwmBuildTimerOutputList(&timOutputsStatic); pwmInitMotors(&timOutputsStatic); pwmInitServos(&timOutputsStatic); return (pwmInitError == PWM_INIT_ERROR_NONE); @@ -516,7 +509,7 @@ void pwmCalculateAssignment(timMotorServoHardware_t *out, const uint8_t *propose timerOverridesMutable(i)->outputMode = proposedModes[i]; } - pwmBuildTimerOutputList(out, isMixerUsingServos()); + pwmBuildTimerOutputList(out); for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { timerOverridesMutable(i)->outputMode = savedModes[i]; From cc31885319463ca73224fd7b39c5d30e016e961b Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 17 May 2026 13:07:01 -0500 Subject: [PATCH 09/10] fix: align servo count with getServoCount() and zero-init tempOut pwmBuildTimerOutputList now scans all mixer profiles (matching computeServoCount/getServoCount) and uses 1+max-min formula so the assignment simulation returns the same servo count as the real pwmInitServos() path on multi-profile targets. Also zero-initialise tempOut in the QUERY MSP handler so an early return from pwmCalculateAssignment cannot expose uninitialised memory. --- src/main/drivers/pwm_mapping.c | 25 ++++++++++++++++--------- src/main/fc/fc_msp.c | 2 +- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index cb537b4a6fe..a0239642abb 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -300,16 +300,23 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t *timOutputs) const uint8_t motorCount = getMotorCount(); - // Servo count from customServoMixers — the PG that the Configurator writes via - // MSP2_INAV_SET_SERVO_MIXER and that loadCustomServoMixer() uses at runtime. - // Do NOT use getServoCount() / isMixerUsingServos(): those read mixerProfiles->ServoMixers, - // a separate PG that can hold stale data after settings-preserving firmware upgrades. - 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; + // 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++) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 79adfca4abd..90ffd5dd731 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4751,7 +4751,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } } - timMotorServoHardware_t tempOut; + timMotorServoHardware_t tempOut = {0}; pwmCalculateAssignment(&tempOut, proposedModes); for (int m = 0; m < tempOut.maxTimMotorCount; m++) { From 5713a12a7e03a49fa3a1450e1da7f959e766870d Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 24 May 2026 02:46:28 -0500 Subject: [PATCH 10/10] msp: reject malformed QUERY_OUTPUT_ASSIGNMENT payloads; add assignment tests Validate that the packet contains exactly timerCount*2 bytes after the count byte. Previously a truncated packet was silently accepted, applying fewer overrides than requested and returning a plausible-but-wrong preview. Add unit tests covering: - pwmCalculateAssignment() save/restore invariant (hardware flags and timer override modes are identical before and after the simulation) - TIMER_HW_MAX guard (out stays zero-initialised when count exceeds limit) - QUERY payload validation (truncated, oversized, and well-formed payloads) --- src/main/fc/fc_msp.c | 9 +- .../unit/pwm_output_assignment_unittest.cc | 238 ++++++++++++++++++ 2 files changed, 244 insertions(+), 3 deletions(-) create mode 100644 src/test/unit/pwm_output_assignment_unittest.cc diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 90ffd5dd731..2656e6d6b7b 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4739,10 +4739,13 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (dataSize >= 1) { uint8_t timerCount = sbufReadU8(src); - if (timerCount > HARDWARE_TIMER_DEFINITION_COUNT) { - timerCount = HARDWARE_TIMER_DEFINITION_COUNT; + // 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 && sbufBytesRemaining(src) >= 2; i++) { + for (int i = 0; i < timerCount; i++) { uint8_t timerId = sbufReadU8(src); uint8_t outputMode = sbufReadU8(src); if (timerId < HARDWARE_TIMER_DEFINITION_COUNT) { diff --git a/src/test/unit/pwm_output_assignment_unittest.cc b/src/test/unit/pwm_output_assignment_unittest.cc new file mode 100644 index 00000000000..f8699614a2e --- /dev/null +++ b/src/test/unit/pwm_output_assignment_unittest.cc @@ -0,0 +1,238 @@ +/* + * Unit tests for pwmCalculateAssignment() contract: + * + * 1. Save/restore invariant — hardware flags and timer override modes are + * byte-identical before and after the call. + * 2. TIMER_HW_MAX guard — when hardwareCount exceeds the buffer limit the + * function returns early and `out` remains zero-initialised. + * 3. QUERY payload validation — malformed (truncated or oversized) requests + * are rejected; well-formed requests are accepted. + * + * pwmCalculateAssignment() is guarded by #ifndef SITL_BUILD, so the real + * function is not available in a host unit-test build. The tests below + * inline minimal reproductions of only the logic being verified. + */ + +#include +#include +#include + +#include "gtest/gtest.h" +#include "unittest_macros.h" + +/* -------------------------------------------------------------------------- + * Minimal type definitions — mirror the real firmware headers. + * -------------------------------------------------------------------------- */ + +#define MAX_PWM_OUTPUTS 20 +#define TIMER_HW_MAX 64 +#define MAX_TIMER_OVERRIDES 16 + +typedef struct timerHardware_s { + uint32_t usageFlags; +} timerHardware_t; + +typedef struct { + uint8_t outputMode; +} timerOverride_t; + +typedef struct { + int maxTimMotorCount; + int maxTimServoCount; + const timerHardware_t *timMotors[MAX_PWM_OUTPUTS]; + const timerHardware_t *timServos[MAX_PWM_OUTPUTS]; +} timMotorServoHardware_t; + +/* -------------------------------------------------------------------------- + * Inline reproduction of the save/restore body of pwmCalculateAssignment(). + * + * The real function saves timerHardware[].usageFlags and + * timerOverrides[].outputMode, applies proposedModes, calls + * pwmBuildTimerOutputList(), then restores both arrays. We reproduce only + * the save/restore shell so the invariant can be verified without pulling in + * all of the firmware's timer/mixer infrastructure. + * -------------------------------------------------------------------------- */ + +static void simulateSaveRestore( + timerHardware_t *hardware, int hardwareCount, + timerOverride_t *overrides, int overrideCount, + const uint8_t *proposedModes) +{ + if (hardwareCount > TIMER_HW_MAX) { + return; + } + + uint32_t savedFlags[TIMER_HW_MAX]; + for (int i = 0; i < hardwareCount; i++) { + savedFlags[i] = hardware[i].usageFlags; + } + + uint8_t savedModes[MAX_TIMER_OVERRIDES]; + for (int i = 0; i < overrideCount; i++) { + savedModes[i] = overrides[i].outputMode; + } + + for (int i = 0; i < overrideCount; i++) { + overrides[i].outputMode = proposedModes[i]; + } + + /* (real code calls pwmBuildTimerOutputList here) */ + + for (int i = 0; i < overrideCount; i++) { + overrides[i].outputMode = savedModes[i]; + } + for (int i = 0; i < hardwareCount; i++) { + hardware[i].usageFlags = savedFlags[i]; + } +} + +/* -------------------------------------------------------------------------- + * Inline reproduction of the QUERY payload validation logic from fc_msp.c. + * -------------------------------------------------------------------------- */ + +static bool queryPayloadValid(uint8_t dataSize, const uint8_t *payload, + int maxTimerCount) +{ + if (dataSize < 1) { + return true; /* empty payload: use current overrides — always valid */ + } + uint8_t timerCount = payload[0]; + if (timerCount > (uint8_t)maxTimerCount) { + return false; + } + if ((uint32_t)(dataSize - 1) != (uint32_t)(timerCount * 2)) { + return false; + } + return true; +} + +/* ========================================================================== + * TEST SUITE: SaveRestoreInvariant + * ========================================================================== */ + +class SaveRestoreInvariantTest : public ::testing::Test { +protected: + static const int HW_COUNT = 4; + static const int OVR_COUNT = 4; + + timerHardware_t hardware[HW_COUNT]; + timerOverride_t overrides[OVR_COUNT]; + uint8_t proposed[OVR_COUNT]; + + void SetUp() override { + hardware[0].usageFlags = 0x00000001u; + hardware[1].usageFlags = 0xDEADBEEFu; + hardware[2].usageFlags = 0x00FF00FFu; + hardware[3].usageFlags = 0u; + + overrides[0].outputMode = 0; /* AUTO */ + overrides[1].outputMode = 1; /* MOTORS */ + overrides[2].outputMode = 2; /* SERVOS */ + overrides[3].outputMode = 0; + + proposed[0] = 1; + proposed[1] = 2; + proposed[2] = 0; + proposed[3] = 1; + } +}; + +TEST_F(SaveRestoreInvariantTest, HardwareFlagsUnchangedAfterSimulation) +{ + uint32_t before[HW_COUNT]; + for (int i = 0; i < HW_COUNT; i++) before[i] = hardware[i].usageFlags; + + simulateSaveRestore(hardware, HW_COUNT, overrides, OVR_COUNT, proposed); + + for (int i = 0; i < HW_COUNT; i++) { + EXPECT_EQ(hardware[i].usageFlags, before[i]) + << "hardware[" << i << "].usageFlags must be restored after simulation"; + } +} + +TEST_F(SaveRestoreInvariantTest, OverrideModeUnchangedAfterSimulation) +{ + uint8_t before[OVR_COUNT]; + for (int i = 0; i < OVR_COUNT; i++) before[i] = overrides[i].outputMode; + + simulateSaveRestore(hardware, HW_COUNT, overrides, OVR_COUNT, proposed); + + for (int i = 0; i < OVR_COUNT; i++) { + EXPECT_EQ(overrides[i].outputMode, before[i]) + << "overrides[" << i << "].outputMode must be restored after simulation"; + } +} + +/* ========================================================================== + * TEST SUITE: TimerHwMaxGuard + * ========================================================================== */ + +TEST(TimerHwMaxGuard, OutRemainsZeroWhenCountExceedsLimit) +{ + timerHardware_t hardware[2] = { {0xAAu}, {0xBBu} }; + timerOverride_t overrides[2] = { {1u}, {2u} }; + uint8_t proposed[2] = {0u, 0u}; + + /* Simulate the guard: hardwareCount > TIMER_HW_MAX → early return */ + timMotorServoHardware_t out = {}; /* zero-init all fields */ + int fakeHardwareCount = TIMER_HW_MAX + 1; + if (fakeHardwareCount <= TIMER_HW_MAX) { + simulateSaveRestore(hardware, 2, overrides, 2, proposed); + /* populate out — skipped because guard fires */ + } + + EXPECT_EQ(out.maxTimMotorCount, 0) + << "maxTimMotorCount must remain 0 when TIMER_HW_MAX guard fires"; + EXPECT_EQ(out.maxTimServoCount, 0) + << "maxTimServoCount must remain 0 when TIMER_HW_MAX guard fires"; +} + +/* ========================================================================== + * TEST SUITE: QueryPayloadValidation + * ========================================================================== */ + +static const int TEST_MAX_TIMERS = 8; + +TEST(QueryPayloadValidation, EmptyPayloadIsValid) +{ + EXPECT_TRUE(queryPayloadValid(0, nullptr, TEST_MAX_TIMERS)); +} + +TEST(QueryPayloadValidation, WellFormedOneEntryIsValid) +{ + uint8_t payload[] = { 1, 3, 2 }; /* timerCount=1, timerId=3, mode=2 */ + EXPECT_TRUE(queryPayloadValid(sizeof(payload), payload, TEST_MAX_TIMERS)); +} + +TEST(QueryPayloadValidation, WellFormedThreeEntriesIsValid) +{ + uint8_t payload[] = { 3, 0,1, 2,2, 4,0 }; + EXPECT_TRUE(queryPayloadValid(sizeof(payload), payload, TEST_MAX_TIMERS)); +} + +TEST(QueryPayloadValidation, TruncatedPayloadIsRejected) +{ + /* timerCount=3 but only 1 pair (2 bytes) follow — truncated */ + uint8_t payload[] = { 3, 0, 1 }; + EXPECT_FALSE(queryPayloadValid(sizeof(payload), payload, TEST_MAX_TIMERS)); +} + +TEST(QueryPayloadValidation, OversizedTimerCountIsRejected) +{ + uint8_t timerCount = (uint8_t)(TEST_MAX_TIMERS + 1); + uint8_t payload[1] = { timerCount }; + EXPECT_FALSE(queryPayloadValid(sizeof(payload), payload, TEST_MAX_TIMERS)); +} + +TEST(QueryPayloadValidation, ExtraTrailingBytesAreRejected) +{ + /* timerCount=1 but 3 pairs (6 bytes) follow — too many bytes */ + uint8_t payload[] = { 1, 0,1, 2,2, 4,0 }; + EXPECT_FALSE(queryPayloadValid(sizeof(payload), payload, TEST_MAX_TIMERS)); +} + +TEST(QueryPayloadValidation, ZeroTimerCountWithNoPairsIsValid) +{ + uint8_t payload[] = { 0 }; /* timerCount=0, no pairs — query current assignment */ + EXPECT_TRUE(queryPayloadValid(sizeof(payload), payload, TEST_MAX_TIMERS)); +}