From d6b55b2d09b50ae3b8b570dbf5ae0ee22338297b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 23 Aug 2016 20:21:04 +0900
Subject: [PATCH] Rover: set_guided_WP accepts location

---
 APMrover2/GCS_Mavlink.cpp | 12 +++---------
 APMrover2/Rover.h         |  4 +---
 APMrover2/commands.cpp    |  8 +++++---
 APMrover2/system.cpp      |  4 +---
 4 files changed, 10 insertions(+), 18 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index c5bf77cc09..d604908f83 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -705,14 +705,12 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
         // only accept position updates when in GUIDED mode
         return false;
     }
-    rover.guided_WP = cmd.content.location;
 
     // This method is only called when we are in Guided WP GUIDED mode
     rover.guided_mode = Guided_WP;
 
     // make any new wp uploaded instant (in case we are already in Guided mode)
-    rover.rtl_complete = false;
-    rover.set_guided_WP();
+    rover.set_guided_WP(cmd.content.location);
     return true;
 }
 
@@ -1290,9 +1288,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                     break;
                 }
 
-                rover.guided_WP = loc;
-                rover.rtl_complete = false;
-                rover.set_guided_WP();
+                rover.set_guided_WP(loc);
             }
 
             break;
@@ -1324,9 +1320,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                 Location loc = rover.current_loc;
                 loc.lat = packet.lat_int;
                 loc.lng = packet.lon_int;
-                rover.guided_WP = loc;
-                rover.rtl_complete = false;
-                rover.set_guided_WP();
+                rover.set_guided_WP(loc);
             }
 
             break;
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 4e0f971b78..45a338146a 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -338,8 +338,6 @@ private:
     struct Location prev_WP;
     // The location of the current/active waypoint.  Used for track following
     struct Location next_WP;
-    // The location of the active waypoint in Guided mode.
-    struct Location guided_WP;
 
     // IMU variables
     // The main loop execution time.  Seconds
@@ -472,7 +470,7 @@ private:
     void calc_nav_steer();
     void set_servos(void);
     void set_next_WP(const struct Location& loc);
-    void set_guided_WP(void);
+    void set_guided_WP(const struct Location& loc);
     void init_home();
     void restart_nav();
     void exit_mission();
diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp
index d52dba6a4e..2f1dfaacb4 100644
--- a/APMrover2/commands.cpp
+++ b/APMrover2/commands.cpp
@@ -37,7 +37,7 @@ void Rover::set_next_WP(const struct Location& loc)
     wp_distance      = wp_totalDistance;
 }
 
-void Rover::set_guided_WP(void)
+void Rover::set_guided_WP(const struct Location& loc)
 {
     // copy the current location into the OldWP slot
     // ---------------------------------------
@@ -45,11 +45,13 @@ void Rover::set_guided_WP(void)
 
     // Load the next_WP slot
     // ---------------------
-    next_WP = guided_WP;
+    next_WP = loc;
 
     // this is handy for the groundstation
     wp_totalDistance = get_distance(current_loc, next_WP);
     wp_distance      = wp_totalDistance;
+
+    rover.rtl_complete = false;
 }
 
 // run this at setup on the ground
@@ -77,7 +79,7 @@ void Rover::init_home()
 
     // Load home for a default guided_WP
     // -------------
-    guided_WP = home;
+    set_guided_WP(home);
 }
 
 void Rover::restart_nav()
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index e36ab20db6..0ef25adcce 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -333,13 +333,11 @@ void Rover::set_mode(enum mode mode)
 
     case GUIDED:
         auto_throttle_mode = true;
-        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();
+        set_guided_WP(current_loc);
         break;
 
     default: