From e48b7cea12216be0423a21b2bd09aa8e62a565b8 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 6 Dec 2023 10:37:33 +1030 Subject: [PATCH] Copter: Use RTL_CLIMB_MIN in cone slope. --- ArduCopter/config.h | 4 ---- ArduCopter/mode_rtl.cpp | 6 +++--- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 329678e1b8..ace8cc43f2 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -433,10 +433,6 @@ # define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL #endif -#ifndef RTL_ABS_MIN_CLIMB - # define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb -#endif - #ifndef RTL_CONE_SLOPE_DEFAULT # define RTL_CONE_SLOPE_DEFAULT 3.0f // slope of RTL cone (height / distance). 0 = No cone #endif diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 7c74838a3f..a8421d9009 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -483,14 +483,14 @@ void ModeRTL::compute_return_target() int32_t target_alt = MAX(rtl_path.return_target.alt, 0); // increase target to maximum of current altitude + climb_min and rtl altitude - target_alt = MAX(target_alt, curr_alt + MAX(0, g.rtl_climb_min)); - target_alt = MAX(target_alt, MAX(g.rtl_altitude, RTL_ALT_MIN)); + const float min_rtl_alt = MAX(RTL_ALT_MIN, curr_alt + MAX(0, g.rtl_climb_min)); + target_alt = MAX(target_alt, MAX(g.rtl_altitude, min_rtl_alt)); // reduce climb if close to return target float rtl_return_dist_cm = rtl_path.return_target.get_distance(rtl_path.origin_point) * 100.0f; // don't allow really shallow slopes if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { - target_alt = MAX(curr_alt, MIN(target_alt, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB))); + target_alt = MIN(target_alt, MAX(rtl_return_dist_cm * g.rtl_cone_slope, min_rtl_alt)); } // set returned target alt to new target_alt (don't change altitude type)