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..2656e6d6b7b 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,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: { 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 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)); +}