Plane: improve crosstrack error while navigating to loiter point

There is no crosstrack concept in the loiter navigation so when going from waypoint to loiter you will not converge onto the line between those two points. This commit adds crosstracking by performing normal waypoint navigation until you get near it.

ArduPlane/commands_logic.cpp

@@ -31,6 +31,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         // once landed, post some landing statistics to the GCS
         auto_state.post_landing_stats = false;
 
+        // reset loiter start time. New command is a new loiter
+        loiter.start_time_ms = 0;
+
         gcs_send_text_fmt("Executing nav command ID #%i",cmd.id);
     } else {
         gcs_send_text_fmt("Executing command ID #%i",cmd.id);
@@ -405,7 +408,6 @@ void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd)
 {
     set_next_WP(cmd.content.location);
     // we set start_time_ms when we reach the waypoint
-    loiter.start_time_ms = 0;
     loiter.time_max_ms = cmd.p1 * (uint32_t)1000;     // units are seconds
     loiter_set_direction_wp(cmd);
 }

ArduPlane/navigation.cpp

@@ -128,7 +128,25 @@ void Plane::calc_gndspeed_undershoot()
 
 void Plane::update_loiter()
 {
-    nav_controller->update_loiter(next_WP_loc, abs(g.loiter_radius), loiter.direction);
+    int16_t radius = abs(g.loiter_radius);
+
+    if (loiter.start_time_ms == 0 &&
+        control_mode == AUTO &&
+        !auto_state.no_crosstrack &&
+        get_distance(current_loc, next_WP_loc) > radius*2) {
+        // if never reached loiter point and using crosstrack and somewhat far away from loiter point
+        // navigate to it like in auto-mode for normal crosstrack behavior
+        nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
+    } else {
+        nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);
+    }
+
+    if (loiter.start_time_ms == 0) {
+        if (nav_controller->reached_loiter_target()) {
+            // we've reached the target, start the timer
+            loiter.start_time_ms = millis();
+        }
+    }
 }
 
 /*

ArduPlane/system.cpp

@@ -360,6 +360,9 @@ void Plane::set_mode(enum FlightMode mode)
     // start with previous WP at current location
     prev_WP_loc = current_loc;
 
+    // new mode means new loiter
+    loiter.start_time_ms = 0;
+
     switch(control_mode)
     {
     case INITIALISING: