dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/modules/px4iofirmware/mixer.cpp
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);
}
}
}