From 0ed3061d32c801fe0d7c18b8adb0734ee020d0c4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 29 Mar 2013 10:24:59 +1100
Subject: [PATCH] Rover: added waypoint passed logic to RTL

this prevents circling around home
---
 APMrover2/commands_logic.pde | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde
index 8a1fc53432..4aef464f16 100644
--- a/APMrover2/commands_logic.pde
+++ b/APMrover2/commands_logic.pde
@@ -222,9 +222,16 @@ static bool verify_RTL()
 		gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
                 rtl_complete = true;
 		return true;
-	}else{
-		return false;
 	}
+
+    // have we gone past the waypoint?
+    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));
+        return true;
+    }
+
+    return false;
 }
 
 /********************************************************************************/