From f314b243eeefd91583339fb67126e0cc42520064 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Wed, 3 Jun 2015 22:31:31 +1000 Subject: [PATCH] Rover: Make guided operate the same way as Plane/Copter. This is a safety change. Lets say you have a GCS which is in followme mode which is really GUIDED mode with continually updated waypoints. If the user then changes mode with the RC transmitter to HOLD or anything else then the Rover should STOP listening to the updated guided mode waypoints. This is how Plane/Copter work. --- APMrover2/GCS_Mavlink.cpp | 7 +++++-- APMrover2/system.cpp | 6 ++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 07ae654079..f3e0262431 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -789,10 +789,13 @@ GCS_MAVLINK::data_stream_send(void) void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd) { + if (rover.control_mode != GUIDED) { + // only accept position updates when in GUIDED mode + return; + } + rover.guided_WP = cmd.content.location; - rover.set_mode(GUIDED); - // make any new wp uploaded instant (in case we are already in Guided mode) rover.rtl_complete = false; rover.set_guided_WP(); diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index a07242c7a9..9aba30288d 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -306,6 +306,11 @@ void Rover::set_mode(enum mode mode) case GUIDED: rtl_complete = false; + /* + when entering guided mode we set the target as the current + location. This matches the behaviour of the copter code. + */ + guided_WP = current_loc; set_guided_WP(); break; @@ -329,6 +334,7 @@ bool Rover::mavlink_set_mode(uint8_t mode) case HOLD: case LEARNING: case STEERING: + case GUIDED: case AUTO: case RTL: set_mode((enum mode)mode);