From 7f0ec2305c8b1efa239eeda5aed24f0b324ea5a7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 21 Mar 2024 19:54:04 +0100 Subject: [PATCH] px4iofirmware: refactor to only have one PWM output code path This removes the duplication with unexpected differences and allows to consistently handle the output instead of overriding the output for some specific cases which leads to unexpected corner cases. E.g. disabled outputs suddenly outputing PWM in lockdown. --- src/modules/px4iofirmware/mixer.cpp | 32 +++++++++++------------------ 1 file changed, 12 insertions(+), 20 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index db6e14844a..3b02242c8a 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -219,9 +219,18 @@ mixer_tick() isr_debug(5, "> PWM disabled"); } - if (mixer_servos_armed - && (should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE)) - && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { + const bool armed_output = (should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE)) + && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN); + 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]); @@ -234,22 +243,5 @@ mixer_tick() } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); } - - } else if (mixer_servos_armed && (should_always_enable_pwm || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) { - /* set the disarmed servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - up_pwm_servo_set(i, r_page_servo_disarmed[i]); - /* copy values into reporting register */ - r_page_servos[i] = r_page_servo_disarmed[i]; - } - - /* set S.BUS1 or S.BUS2 outputs */ - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { - sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); - } - - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { - sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); - } } }