From 20de5b3006b4f2fc408db6657d208a36da1b36ce Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 25 Apr 2014 14:38:11 +0900 Subject: [PATCH] Copter: Loiter clears out pilot acceleration when failsafe occurs This ensures invalid pilot desired accelerations are cleared from the loiter controller. This is probably not strictly necessary because the vehicle should switch out of Loiter and into RTL during failsafe. --- ArduCopter/control_loiter.pde | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 01fe79fc58..77fba105b1 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -45,7 +45,6 @@ static void loiter_run() update_simple_mode(); // process pilot's roll and pitch input - // To-Do: do we need to clear out feed forward if this is not called? wp_nav.set_pilot_desired_acceleration(g.rc_1.control_in, g.rc_2.control_in); // get pilot's desired yaw rate @@ -61,6 +60,9 @@ static void loiter_run() // clear i term when we're taking off set_throttle_takeoff(); } + } else { + // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason + wp_nav.clear_pilot_desired_acceleration(); } // when landed reset targets and output zero throttle