From e1b7e53c046049742ff40a6ddb0308de144fa1f6 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Wed, 14 May 2014 12:11:28 -0700 Subject: [PATCH] Copter: only acceept Guided waypoints in Guided mode Fix #1068. When receiving guided waypoints do not change to GUIDED mode. This serves as a safety precaution for GCS, since they must switch to guided mode before sending the waypoints. --- ArduCopter/commands_logic.pde | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index de7cbcda1d..71d0c0847b 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -777,12 +777,9 @@ static bool verify_yaw() // do_guided - start guided mode static bool do_guided(const AP_Mission::Mission_Command& cmd) { - // switch to guided mode if we're not already in guided mode + // only process guided waypoint if we are in guided mode if (control_mode != GUIDED) { - if (!set_mode(GUIDED)) { - // if we failed to enter guided mode return immediately - return false; - } + return false; } // set wp_nav's destination