From 3800c66f071cef8b7595fb776af9e6bb07b8a03a Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Tue, 5 Jan 2016 23:00:29 -0800
Subject: [PATCH] Copter: add RTL_CONE_SLOPE

---
 ArduCopter/Parameters.cpp  |  8 ++++++++
 ArduCopter/Parameters.h    |  4 +++-
 ArduCopter/config.h        | 12 ++++++++++++
 ArduCopter/control_rtl.cpp |  4 ++++
 4 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
index ed609c2cea..7d2f609b2c 100644
--- a/ArduCopter/Parameters.cpp
+++ b/ArduCopter/Parameters.cpp
@@ -127,6 +127,14 @@ const AP_Param::Info Copter::var_info[] = {
     // @Increment: 1
     // @User: Standard
     GSCALAR(rtl_altitude,   "RTL_ALT",     RTL_ALT),
+    
+    // @Param: RTL_CONE_SLOPE
+    // @DisplayName: RTL cone slope
+    // @Description: Defines a cone above home which determines maximum climb
+    // @Range: 0.5 10.0
+    // @Increment: .1
+    // @User: Standard
+    GSCALAR(rtl_cone_slope,   "RTL_CONE_SLOPE",     RTL_CONE_SLOPE),
 
     // @Param: RTL_SPEED
     // @DisplayName: RTL speed
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index c647d4482b..cf299cff23 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -217,7 +217,8 @@ public:
         // 135 : reserved for Solo until features merged with master
         //
         k_param_rtl_speed_cms = 135,
-        k_param_fs_batt_curr_rtl, // 136
+        k_param_fs_batt_curr_rtl,
+        k_param_rtl_cone_slope, // 137
 
         //
         // 140: Sensor parameters
@@ -380,6 +381,7 @@ public:
 
     AP_Int16        rtl_altitude;
     AP_Int16        rtl_speed_cms;
+    AP_Float        rtl_cone_slope;
     AP_Float        sonar_gain;
 
     AP_Int8         failsafe_battery_enabled;   // battery failsafe enabled
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 9d76f8c36d..d340e314e0 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -484,10 +484,22 @@
  # define RTL_ALT 				    1500    // default alt to return to home in cm, 0 = Maintain current altitude
 #endif
 
+#ifndef RTL_CONE_SLOPE
+ # define RTL_CONE_SLOPE            3.0f   // slope of RTL cone. 0 = No cone
+#endif
+
 #ifndef RTL_ALT_MIN
  # define RTL_ALT_MIN               200     // min height above ground for RTL (i.e 2m)
 #endif
 
+#ifndef RTL_ABS_MIN_CLIMB
+ # define RTL_ABS_MIN_CLIMB          250     // absolute minimum initial climb
+#endif
+
+#ifndef RTL_MIN_CONE_SLOPE
+ # define RTL_MIN_CONE_SLOPE          0.5f     // absolute minimum initial climb
+#endif
+
 #ifndef RTL_CLIMB_MIN_DEFAULT
  # define RTL_CLIMB_MIN_DEFAULT     0       // vehicle will always climb this many cm as first stage of RTL
 #endif
diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp
index 8b7ccc2787..ddc238e51d 100644
--- a/ArduCopter/control_rtl.cpp
+++ b/ArduCopter/control_rtl.cpp
@@ -430,6 +430,10 @@ float Copter::rtl_compute_return_alt_above_origin(float rtl_return_dist)
     // maximum of current altitude + climb_min and rtl altitude
     float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN));
 
+    if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { // don't allow really shallow slopes
+        ret = MAX(current_loc.alt, MIN(ret, MAX(rtl_return_dist*g.rtl_cone_slope, current_loc.alt+RTL_ABS_MIN_CLIMB)));
+    }
+
 #if AC_FENCE == ENABLED
     // ensure not above fence altitude if alt fence is enabled
     if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {