From 43ad1f372d47008a34bf9c736f48857529b31499 Mon Sep 17 00:00:00 2001
From: Francisco Ferreira <francisco-ferreira@mail.telepac.pt>
Date: Thu, 16 Jun 2016 16:13:57 +0100
Subject: [PATCH] Copter: change function from computing return altitude to
 computing return target

Also fix altitude for rally points
---
 ArduCopter/Copter.h        |  2 +-
 ArduCopter/control_rtl.cpp | 37 ++++++++++++++++++++++++++-----------
 2 files changed, 27 insertions(+), 12 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index ce1c3b09df..39ecc6fb9a 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -868,7 +868,7 @@ private:
     void rtl_land_start();
     void rtl_land_run();
     void rtl_build_path(bool terrain_following_allowed);
-    void rtl_compute_return_alt(bool terrain_following_allowed);
+    void rtl_compute_return_target(bool terrain_following_allowed);
     bool sport_init(bool ignore_checks);
     void sport_run();
     bool stabilize_init(bool ignore_checks);
diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp
index 0e3047aa04..f229b1e391 100644
--- a/ArduCopter/control_rtl.cpp
+++ b/ArduCopter/control_rtl.cpp
@@ -404,15 +404,8 @@ void Copter::rtl_build_path(bool terrain_following_allowed)
     rtl_path.origin_point = Location_Class(stopping_point);
     rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME);
 
-    // set return target to nearest rally point or home position
-#if AC_RALLY == ENABLED
-    rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt);
-#else
-    rtl_path.return_target = ahrs.get_home();
-#endif
-
-    // compute return altitude
-    rtl_compute_return_alt(terrain_following_allowed);
+    // compute return target
+    rtl_compute_return_target(terrain_following_allowed);
 
     // climb target is above our origin point at the return altitude
     rtl_path.climb_target = Location_Class(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame());
@@ -424,10 +417,26 @@ void Copter::rtl_build_path(bool terrain_following_allowed)
     rtl_path.land = g.rtl_alt_final <= 0;
 }
 
-// return altitude in cm above home at which vehicle should return home
+// compute the return target - home or rally point
+//   return altitude in cm above home at which vehicle should return home
 //   return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
-void Copter::rtl_compute_return_alt(bool terrain_following_allowed)
+void Copter::rtl_compute_return_target(bool terrain_following_allowed)
 {
+    // set return target to nearest rally point or home position
+#if AC_RALLY == ENABLED
+    RallyLocation rallyLoc;
+    Location_Class home(ahrs.get_home());
+    bool use_home = !rally.find_nearest_rally_point(current_loc, rallyLoc);
+
+    if (!use_home) {
+        rtl_path.return_target = Location_Class(rallyLoc.lat, rallyLoc.lng, home.alt, home.get_alt_frame());
+    } else {
+        rtl_path.return_target = home;
+    }
+#else
+    rtl_path.return_target = ahrs.get_home();
+#endif
+
     float rtl_return_dist_cm = rtl_path.return_target.get_distance(rtl_path.origin_point) * 100.0f;
 
     // curr_alt is current altitude above home or above terrain depending upon use_terrain
@@ -454,6 +463,12 @@ void Copter::rtl_compute_return_alt(bool terrain_following_allowed)
         ret = MAX(curr_alt, MIN(ret, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB)));
     }
 
+#if AC_RALLY == ENABLED
+    if (!use_home) {
+        ret = rallyLoc.alt * 100.0f;
+    }
+#endif
+
 #if AC_FENCE == ENABLED
     // ensure not above fence altitude if alt fence is enabled
     // Note: we are assuming the fence alt is the same frame as ret