From ef5569fab3bfc196b18d4e290df5c6afe2b3dfe4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 21 Mar 2024 20:14:03 +0100 Subject: [PATCH] px4iofirmware: reuse existing disarmed logic for lockdown and `should_always_enable_pwm` The existing disarmed logic already handles disabled outputs it makes sense to reuse it and not have lockdown handled differently resulting in unexpeced corner cases. --- src/modules/px4iofirmware/mixer.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 82d3320327..936c126505 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -176,6 +176,14 @@ mixer_tick() atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE)); } + const bool armed_output = should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE); + const bool disarmed_output = (!armed_output && should_always_enable_pwm) + || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN); + + if (disarmed_output) { + source = MIX_DISARMED; + } + /* * Run the mixers. */ @@ -219,17 +227,7 @@ mixer_tick() isr_debug(5, "> PWM disabled"); } - const bool armed_output = should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE); - const bool disarmed_output = (!armed_output && should_always_enable_pwm) - || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN); - if (mixer_servos_armed && (armed_output || disarmed_output)) { - if (disarmed_output) { - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - r_page_servos[i] = r_page_servo_disarmed[i]; - } - } - /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]);