Copter: add RTL_CONE_SLOPE

3drobotics · Oct 9, 2015 · 9698e78 · 9698e78
1 parent 5f3991e
commit 9698e78
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 12 deletions.
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
@@ -197,6 +197,7 @@ class Parameters {
         //solo params 135..139
         k_param_rtl_speed_cms=135,
         k_param_fs_batt_curr_rtl,
+        k_param_rtl_cone_slope,
 
         //
         // 140: Sensor parameters
@@ -356,6 +357,7 @@ class Parameters {
 
     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/Parameters.pde b/ArduCopter/Parameters.pde
@@ -117,6 +117,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @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
     // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home

diff --git a/ArduCopter/config.h b/ArduCopter/config.h
@@ -485,6 +485,10 @@
  # 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             2.0    // default alt to return to home in cm, 0 = Maintain current altitude
+#endif
+
 #ifndef RTL_SPEED
 # define RTL_SPEED                  500    // default speed at which to return home in cm/s
 #endif

diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde
@@ -397,24 +397,24 @@ static void rtl_build_path()
     pos_control.get_stopping_point_xy(rtl_path.origin_point);
     pos_control.get_stopping_point_z(rtl_path.origin_point);
 
-    // compute return altitude
-    float return_alt_above_home = rtl_compute_alt();
-
-    // climb target is above our origin point
-    rtl_path.climb_target.x = rtl_path.origin_point.x;
-    rtl_path.climb_target.y = rtl_path.origin_point.y;
-    rtl_path.climb_target.z = pv_alt_above_origin(return_alt_above_home);
-
     // set return target to nearest rally point or home position
 #if AC_RALLY == ENABLED
-    Location rally_point = rally.calc_best_rally_or_home_location(current_loc, return_alt_above_home+ahrs.get_home().alt);
+    Location rally_point = rally.calc_best_rally_or_home_location(current_loc, 0);
     rtl_path.return_target = pv_location_to_vector(rally_point);
 #else
     rtl_path.return_target = pv_location_to_vector(ahrs.get_home());
 #endif
 
-    // return target is at same altitude as climb target
-    rtl_path.return_target.z = rtl_path.climb_target.z;
+    float rtl_return_dist = sqrtf(sq(rtl_path.return_target.x-rtl_path.origin_point.x)+sq(rtl_path.return_target.y-rtl_path.origin_point.y));
+
+    // compute return altitude
+    float return_alt_above_home = rtl_compute_alt(rtl_return_dist);
+    rtl_path.return_target.z = pv_alt_above_origin(return_alt_above_home);
+
+    // climb target is above our origin point at the return altitude
+    rtl_path.climb_target.x = rtl_path.origin_point.x;
+    rtl_path.climb_target.y = rtl_path.origin_point.y;
+    rtl_path.climb_target.z = rtl_path.return_target.z;
 
     rtl_path.land = g.rtl_alt_final <= 0;
 
@@ -426,11 +426,15 @@ static void rtl_build_path()
 
 // return altitude which vehicle should return home at
 //      altitude is in cm above home
-static float rtl_compute_alt()
+static float rtl_compute_alt(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), g.rtl_altitude);
 
+    if (g.rtl_cone_slope >= 0.5f) { // don't allow really shallow slopes
+        ret = max(current_loc.alt, min(ret, rtl_return_dist*g.rtl_cone_slope));
+    }
+
     ret = max(ret, RTL_ALT_MIN);
 
 #if AC_FENCE == ENABLED