From 168f955e0cd4133417b2f917ac123df71d4924fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Feb 2016 13:06:19 +0100 Subject: [PATCH] IO firmware: Ensure that in case of lockdown only the lockdown code writes servo outputs --- src/modules/px4iofirmware/mixer.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 845f398480..dbb8499da6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -235,7 +235,8 @@ mixer_tick(void) } - } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) + && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; @@ -286,7 +287,8 @@ mixer_tick(void) isr_debug(5, "> PWM disabled"); } - if (mixer_servos_armed && (should_arm || should_arm_nothrottle)) { + if (mixer_servos_armed && (should_arm || should_arm_nothrottle) + && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); @@ -306,6 +308,8 @@ mixer_tick(void) /* 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 */