diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index bd2f763153..2a05d21784 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -355,17 +355,21 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) const home_position_s &home = *_navigator->get_home_position(); const vehicle_global_position_s &gpos = *_navigator->get_global_position(); + // horizontal distance to home position + const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon); + float rtl_altitude; - if (gpos.alt > home.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) { + if (home_dist <= _param_rtl_min_dist.get()) { + rtl_altitude = home.alt + _param_rtl_descend_alt.get(); + + } else if (gpos.alt > home.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) { rtl_altitude = gpos.alt; } else if (cone_half_angle_deg <= 0) { rtl_altitude = home.alt + _param_rtl_return_alt.get(); } else { - // horizontal distance to home position - const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon); // constrain cone half angle to meaningful values. All other cases are already handled above. const float cone_half_angle_rad = math::radians(math::constrain(cone_half_angle_deg, 1.0f, 89.0f)); diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index c8777a41ca..ea6cf59dd1 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -94,7 +94,7 @@ private: RTL_STATE_LANDED, } _rtl_state{RTL_STATE_NONE}; - float _rtl_alt; // AMSL altitude at which the vehicle should return to the home position + float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position bool _rtl_alt_min{false}; DEFINE_PARAMETERS( diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 59d09c54ae..7f6d95e5c1 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -125,7 +125,12 @@ PARAM_DEFINE_INT32(RTL_TYPE, 0); * @unit degrees * @min 0 * @max 90 - * @increment 1 + * @value 0 No cone, always climb to RTL_RETURN_ALT above home. + * @value 25 25 degrees half cone angle. + * @value 45 45 degrees half cone angle. + * @value 65 65 degrees half cone angle. + * @value 80 80 degrees half cone angle. + * @value 90 Only climb to at least RTL_DESCEND_ALT above home. * @group Return Mode */ -PARAM_DEFINE_INT32(RTL_CONE_ANG, 30); +PARAM_DEFINE_INT32(RTL_CONE_ANG, 0);