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.
This commit is contained in:
Matthias Grob 2024-03-21 19:54:04 +01:00
parent de89777b6a
commit 74ff65a12e
1 changed files with 12 additions and 20 deletions

View File

@ -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);
}
}
}