Rover: set_guided_WP accepts location

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;

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();

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()

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: