From 9d6a5c6f0428387a41ad25708973bb9eb0777310 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 24 Oct 2016 13:35:39 +1100 Subject: [PATCH] HAL_PX4: make ready_to_arm state latching this fixes a race condition in the handling of the px4io safety switch --- libraries/AP_HAL_PX4/RCOutput.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp index 4ada16dcb4..583932572e 100644 --- a/libraries/AP_HAL_PX4/RCOutput.cpp +++ b/libraries/AP_HAL_PX4/RCOutput.cpp @@ -385,7 +385,11 @@ void PX4RCOutput::_arm_actuators(bool arm) _armed.timestamp = hrt_absolute_time(); _armed.armed = arm; - _armed.ready_to_arm = arm; + if (arm) { + // this latches ready_to_arm to true once set once. Otherwise + // we have a race condition with px4io safety switch update + _armed.ready_to_arm = true; + } _armed.lockdown = false; _armed.force_failsafe = false;