From 178bbb080b0a108f9fbb51f94623140206ce85df Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Wed, 16 Sep 2015 02:03:28 -0700
Subject: [PATCH] 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 |  4 +++-
 ArduPlane/navigation.cpp     | 20 +++++++++++++++++++-
 ArduPlane/system.cpp         |  3 +++
 3 files changed, 25 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index 318d869d0c..142f2eeb76 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/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);
 }
diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp
index 58d5f85a11..a4fce858f4 100644
--- a/ArduPlane/navigation.cpp
+++ b/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();
+        }
+    }
 }
 
 /*
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 742aa472e2..4fbb518b50 100644
--- a/ArduPlane/system.cpp
+++ b/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: