diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 329678e1b889ca..ace8cc43f2edc3 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 7c74838a3fe742..9047e7975e7794 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -490,7 +490,7 @@ void ModeRTL::compute_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, MAX(RTL_ALT_MIN, curr_alt + MAX(0, g.rtl_climb_min)))); } // set returned target alt to new target_alt (don't change altitude type)