diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 16eda8d7ab..bb9e8af0ee 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -359,7 +359,16 @@ static void rtl_land_run() // altitude is in cm above home static float get_RTL_alt() { - // return maximum of current altitude and rtl altitude - return max(current_loc.alt, g.rtl_altitude); + // maximum of current altitude and rtl altitude + float rtl_alt = max(current_loc.alt, g.rtl_altitude); + +#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) { + rtl_alt = min(rtl_alt, fence.get_safe_alt()*100.0f); + } +#endif + + return rtl_alt; }