From 7ecc87a787dbef03a1647e724a4159ea0a2016f7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 18 Oct 2014 12:36:08 +1100
Subject: [PATCH] Plane: improved check for reaching RTL point for auto-land

---
 ArduPlane/ArduPlane.pde | 19 +++++++++++--------
 1 file changed, 11 insertions(+), 8 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index cee8f84768..061eb9d7c8 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -1419,8 +1419,18 @@ static void update_navigation()
         update_commands();
         break;
             
-    case LOITER:
     case RTL:
+        if (g.rtl_autoland && 
+            nav_controller->reached_loiter_target() && 
+            labs(altitude_error_cm) < 1000) {
+            if (mission.jump_to_landing_sequence()) {
+                gcs_send_text_P(PSTR("Starting auto landing"));
+                set_mode(AUTO);
+            }
+        }
+        // fall through to LOITER
+
+    case LOITER:
     case GUIDED:
         // allow loiter direction to be changed in flight
         if (g.loiter_radius < 0) {
@@ -1429,13 +1439,6 @@ static void update_navigation()
             loiter.direction = 1;
         }
         update_loiter();
-        
-        if (g.rtl_autoland && control_mode == RTL && get_distance(current_loc, next_WP_loc) < fabs(g.loiter_radius) + 20.0f && fabs(current_loc.alt - next_WP_loc.alt) < 10.0f) {
-            if (mission.jump_to_landing_sequence()) {
-                set_mode(AUTO);
-            }
-        }
-
         break;
 
     case CRUISE: