diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index eb9a20e928..e4d477f119 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -514,9 +514,9 @@ static const struct	Location &home = ahrs.get_home();
 // Flag for if we have g_gps lock and have set the home location
 static bool	home_is_set;
 // The location of the previous waypoint.  Used for track following and altitude ramp calculations
-static struct 	AP_Mission::Mission_Command prev_WP;
+static struct 	Location prev_WP;
 // The location of the current/active waypoint.  Used for track following
-static struct 	AP_Mission::Mission_Command next_WP;
+static struct 	Location next_WP;
 // The location of the active waypoint in Guided mode.
 static struct  	Location guided_WP;
 
diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde
index 52ae50a800..e84bd3f5ab 100644
--- a/APMrover2/Steering.pde
+++ b/APMrover2/Steering.pde
@@ -138,13 +138,13 @@ static void calc_lateral_acceleration()
 {
     switch (control_mode) {
     case AUTO:
-        nav_controller->update_waypoint(prev_WP.content.location, next_WP.content.location);
+        nav_controller->update_waypoint(prev_WP, next_WP);
         break;
 
     case RTL:
     case GUIDED:
     case STEERING:
-        nav_controller->update_waypoint(current_loc, next_WP.content.location);
+        nav_controller->update_waypoint(current_loc, next_WP);
         break;
     default:
         return;
diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde
index 079e77aeb8..ad67e4a7bb 100644
--- a/APMrover2/commands.pde
+++ b/APMrover2/commands.pde
@@ -10,10 +10,9 @@
 
 
 /*
-This function stores waypoint commands
-It looks to see what the next command type is and finds the last command.
-*/
-static void set_next_WP(const AP_Mission::Mission_Command& cmd)
+ *  set_next_WP - sets the target location the vehicle should fly to
+ */
+static void set_next_WP(const struct Location& loc)
 {
 	// copy the current WP into the OldWP slot
 	// ---------------------------------------
@@ -21,20 +20,20 @@ static void set_next_WP(const AP_Mission::Mission_Command& cmd)
 
 	// Load the next_WP slot
 	// ---------------------
-	next_WP = cmd;
+	next_WP = loc;
 
     // are we already past the waypoint? This happens when we jump
     // waypoints, and it can cause us to skip a waypoint. If we are
     // past the waypoint when we start on a leg, then use the current
     // location as the previous waypoint, to prevent immediately
     // considering the waypoint complete
-    if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) {
+    if (location_passed_point(current_loc, prev_WP, next_WP)) {
         gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP"));
-        prev_WP.content.location = current_loc;
+        prev_WP = current_loc;
     }
 
 	// this is handy for the groundstation
-	wp_totalDistance 	= get_distance(current_loc, next_WP.content.location);
+	wp_totalDistance 	= get_distance(current_loc, next_WP);
 	wp_distance 		= wp_totalDistance;
 }
 
@@ -42,14 +41,14 @@ static void set_guided_WP(void)
 {
 	// copy the current location into the OldWP slot
 	// ---------------------------------------
-	prev_WP.content.location = current_loc;
+	prev_WP = current_loc;
 
 	// Load the next_WP slot
 	// ---------------------
-	next_WP.content.location = guided_WP;
+	next_WP = guided_WP;
 
 	// this is handy for the groundstation
-	wp_totalDistance 	= get_distance(current_loc, next_WP.content.location);
+	wp_totalDistance 	= get_distance(current_loc, next_WP);
 	wp_distance 		= wp_totalDistance;
 }
 
@@ -72,7 +71,7 @@ void init_home()
 
 	// Save prev loc
 	// -------------
-	next_WP.content.location = prev_WP.content.location = home;
+	next_WP = prev_WP = home;
 
 	// Load home for a default guided_WP
 	// -------------
@@ -82,6 +81,6 @@ void init_home()
 static void restart_nav()
 {  
     g.pidSpeedThrottle.reset_I();
-    prev_WP.content.location = current_loc;
+    prev_WP = current_loc;
     mission.resume();
 }
diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde
index 9e06a62952..729dd648ce 100644
--- a/APMrover2/commands_logic.pde
+++ b/APMrover2/commands_logic.pde
@@ -183,19 +183,19 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)
 
 static void do_RTL(void)
 {
-    prev_WP.content.location = current_loc;
+    prev_WP = current_loc;
 	control_mode 	= RTL;
-	next_WP.content.location = home;
+	next_WP = home;
 }
 
 static void do_takeoff(const AP_Mission::Mission_Command& cmd)
 {
-	set_next_WP(cmd);
+	set_next_WP(cmd.content.location);
 }
 
 static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
-	set_next_WP(cmd);
+	set_next_WP(cmd.content.location);
 }
 
 /********************************************************************************/
@@ -210,15 +210,15 @@ static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
     if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
         gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
                           (unsigned)cmd.index,
-                          (unsigned)get_distance(current_loc, next_WP.content.location));
+                          (unsigned)get_distance(current_loc, next_WP));
         return true;
     }
 
     // have we gone past the waypoint?
-    if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) {
+    if (location_passed_point(current_loc, prev_WP, next_WP)) {
         gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
                           (unsigned)cmd.index,
-                          (unsigned)get_distance(current_loc, next_WP.content.location));
+                          (unsigned)get_distance(current_loc, next_WP));
         return true;
     }
 
@@ -234,9 +234,9 @@ static bool verify_RTL()
 	}
 
     // have we gone past the waypoint?
-    if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) {
+    if (location_passed_point(current_loc, prev_WP, next_WP)) {
         gcs_send_text_fmt(PSTR("Reached Home dist %um"),
-                          (unsigned)get_distance(current_loc, next_WP.content.location));
+                          (unsigned)get_distance(current_loc, next_WP));
         return true;
     }
 
@@ -258,7 +258,7 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd)
 	condition_rate		= abs((int)cmd.content.location.lat);
 	condition_value 	= cmd.content.location.alt;
 	if(condition_value < current_loc.alt) condition_rate = -condition_rate;
-	next_WP.content.location.alt = condition_value;								// For future nav calculations
+	next_WP.alt = condition_value;								// For future nav calculations
 }
 
 static void do_within_distance(const AP_Mission::Mission_Command& cmd)
diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde
index dd897e699d..e9bdbca600 100644
--- a/APMrover2/control_modes.pde
+++ b/APMrover2/control_modes.pde
@@ -27,7 +27,7 @@ static void read_control_switch()
 		set_mode((enum mode)modes[switchPosition].get());
 
 		oldSwitchPosition = switchPosition;
-		prev_WP.content.location = current_loc;
+		prev_WP = current_loc;
 
 		// reset speed integrator
         g.pidSpeedThrottle.reset_I();
diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde
index 95cfeb83ad..c3ab4b608e 100644
--- a/APMrover2/navigation.pde
+++ b/APMrover2/navigation.pde
@@ -11,13 +11,13 @@ static void navigate()
 		return;
 	}
 
-	if ((next_WP.content.location.lat == 0)||(home_is_set==false)){
+	if ((next_WP.lat == 0)||(home_is_set==false)){
 		return;
 	}
 
 	// waypoint distance from rover
 	// ----------------------------
-	wp_distance = get_distance(current_loc, next_WP.content.location);
+	wp_distance = get_distance(current_loc, next_WP);
 
 	if (wp_distance < 0){
 		gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));