Plane: prevent cross-track on some WP types

when finishing a takeoff or when a user forces a WP change don't do
any cross-tracking. This avoids a problem where the plane tries to
line up with a track completely unrelated to the track currently being
flown

ArduPlane/ArduPlane.pde

@@ -530,6 +530,12 @@ static struct {
     // should we fly inverted?
     bool inverted_flight:1;
 
+    // should we disable cross-tracking for the next waypoint?
+    bool next_wp_no_crosstrack:1;
+
+    // should we use cross-tracking for this waypoint?
+    bool no_crosstrack:1;
+
     // Altitude threshold to complete a takeoff command in autonomous modes.  Centimeters
     int32_t takeoff_altitude_cm;
 
@@ -549,6 +555,8 @@ static struct {
     takeoff_complete : true,
     land_complete : false,
     inverted_flight  : false,
+    next_wp_no_crosstrack : true,
+    no_crosstrack : true,
     takeoff_altitude_cm : 0,
     takeoff_pitch_cd : 0,
     highest_airspeed : 0,

ArduPlane/GCS_Mavlink.pde

@@ -1208,6 +1208,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
     {
+        // disable cross-track when user asks for WP change, to
+        // prevent unexpected flight paths
+        auto_state.next_wp_no_crosstrack = true;
         handle_mission_set_current(mission, msg);
         if (control_mode == AUTO && mission.state() == AP_Mission::MISSION_STOPPED) {
             mission.resume();

ArduPlane/commands.pde

@@ -6,11 +6,19 @@
 /*
  *  set_next_WP - sets the target location the vehicle should fly to
  */
-static void set_next_WP(const struct Location& loc)
+static void set_next_WP(const struct Location &loc)
 {
-    // copy the current WP into the OldWP slot
-    // ---------------------------------------
-    prev_WP_loc = next_WP_loc;
+    if (auto_state.next_wp_no_crosstrack) {
+        // we should not try to cross-track for this waypoint
+        prev_WP_loc = current_loc;
+        // use cross-track for the next waypoint
+        auto_state.next_wp_no_crosstrack = false;
+        auto_state.no_crosstrack = true;
+    } else {
+        // copy the current WP into the OldWP slot
+        prev_WP_loc = next_WP_loc;
+        auto_state.no_crosstrack = false;
+    }
 
     // Load the next_WP slot
     // ---------------------

ArduPlane/commands_logic.pde

@@ -250,6 +250,8 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
 
 static void do_RTL(void)
 {
+    auto_state.next_wp_no_crosstrack = true;
+    auto_state.no_crosstrack = true;
     prev_WP_loc = current_loc;
     next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
     setup_terrain_target_alt(next_WP_loc);
@@ -358,13 +360,18 @@ static bool verify_takeoff()
         }
 #endif
 
+        // don't cross-track on completion of takeoff, as otherwise we
+        // can end up doing too sharp a turn
+        auto_state.next_wp_no_crosstrack = true;
         return true;
     } else {
         return false;
     }
 }
 
-// we are executing a landing
+/*
+  update navigation for landing
+ */
 static bool verify_land()
 {
     // we don't 'verify' landing in the sense that it never completes,
@@ -412,11 +419,19 @@ static bool verify_land()
     return false;
 }
 
+/*
+  update navigation for normal mission waypoints. Return true when the
+  waypoint is complete
+ */
 static bool verify_nav_wp()
 {
     steer_state.hold_course_cd = -1;
 
-    nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
+    if (auto_state.no_crosstrack) {
+        nav_controller->update_waypoint(current_loc, next_WP_loc);
+    } else {
+        nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
+    }
 
     // see if the user has specified a maximum distance to waypoint
     if (g.waypoint_max_radius > 0 && wp_distance > (uint16_t)g.waypoint_max_radius) {

ArduPlane/system.pde

@@ -290,6 +290,9 @@ static void set_mode(enum FlightMode mode)
     // cancel inverted flight
     auto_state.inverted_flight = false;
 
+    // don't cross-track when starting a mission
+    auto_state.next_wp_no_crosstrack = true;
+
     // set mode
     previous_mode = control_mode;
     control_mode = mode;