diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index c37ae1775a7..a0239642abb 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[] = { @@ -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 diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index c860166bc74..4a6722046c5 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,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 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bef7c54de40..90ffd5dd731 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,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 = {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: { 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