fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 14:34:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

px4iofirmware: refactor to only have one PWM output code path

Browse Source 
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 committed by Daniel Agar
parent ab1087906d
commit 7f0ec2305c
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);
}
}
}