From 81ee40eac8f68ea3699d962f0d0fd7d465a47169 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 1 Jul 2019 17:17:48 +0200 Subject: [PATCH] rtl: implemented RTL based on cone shape - user defines a cone half angle which defines the RTL altitude behavior - if vehicle is outside the cone it will climb until it intersects the cone and then return at this altitude. - if vehicle is inside the cone or above the predefined return altitude (RTL_RETURN_ALT) then it will return at the current altitude Signed-off-by: RomanBapst --- src/modules/navigator/rtl.cpp | 59 ++++++++++++++++++++++++------ src/modules/navigator/rtl.h | 7 +++- src/modules/navigator/rtl_params.c | 13 +++++++ 3 files changed, 67 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index bd06a9a09a..bd2f763153 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -67,6 +67,9 @@ RTL::rtl_type() const void RTL::on_activation() { + + _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); + if (_navigator->get_land_detected()->landed) { // For safety reasons don't go into RTL if landed. _rtl_state = RTL_STATE_LANDED; @@ -133,14 +136,6 @@ RTL::set_rtl_item() // Check if we are pretty close to home already. const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon); - // Compute the return altitude. - float return_alt = math::max(home.alt + _param_rtl_return_alt.get(), gpos.alt); - - // We are close to home, limit climb to min. - if (home_dist < _param_rtl_min_dist.get()) { - return_alt = home.alt + _param_rtl_descend_alt.get(); - } - // Compute the loiter altitude. const float loiter_altitude = math::min(home.alt + _param_rtl_descend_alt.get(), gpos.alt); @@ -150,7 +145,7 @@ RTL::set_rtl_item() _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.lat = gpos.lat; _mission_item.lon = gpos.lon; - _mission_item.altitude = return_alt; + _mission_item.altitude = _rtl_alt; _mission_item.altitude_is_relative = false; _mission_item.yaw = _navigator->get_local_position()->yaw; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); @@ -159,7 +154,7 @@ RTL::set_rtl_item() _mission_item.origin = ORIGIN_ONBOARD; mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)", - (int)ceilf(return_alt), (int)ceilf(return_alt - _navigator->get_home_position()->alt)); + (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _navigator->get_home_position()->alt)); break; } @@ -169,7 +164,7 @@ RTL::set_rtl_item() _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.lat = home.lat; _mission_item.lon = home.lon; - _mission_item.altitude = return_alt; + _mission_item.altitude = _rtl_alt; _mission_item.altitude_is_relative = false; // Use home yaw if close to home. @@ -353,3 +348,45 @@ RTL::advance_rtl() break; } } + + +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(); + + float rtl_altitude; + + 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)); + + // minimum height above home position required + float height_above_home_min = home_dist / tanf(cone_half_angle_rad); + + // minimum altitude we need in order to be within the user defined cone + const float altitude_min = math::constrain(height_above_home_min + home.alt, home.alt, + home.alt + _param_rtl_return_alt.get()); + + if (gpos.alt < altitude_min) { + rtl_altitude = altitude_min; + + } else { + rtl_altitude = gpos.alt; + } + } + + // always demand altitude which is higher or equal the RTL descend altitude + rtl_altitude = math::max(rtl_altitude, home.alt + _param_rtl_descend_alt.get()); + + return rtl_altitude; +} diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index d852232d51..c8777a41ca 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -80,6 +80,9 @@ private: */ void advance_rtl(); + + float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg); + enum RTLState { RTL_STATE_NONE = 0, RTL_STATE_CLIMB, @@ -91,6 +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 bool _rtl_alt_min{false}; DEFINE_PARAMETERS( @@ -98,6 +102,7 @@ private: (ParamFloat) _param_rtl_descend_alt, (ParamFloat) _param_rtl_land_delay, (ParamFloat) _param_rtl_min_dist, - (ParamInt) _param_rtl_type + (ParamInt) _param_rtl_type, + (ParamInt) _param_rtl_cone_half_angle_deg ) }; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index f5654df7cd..59d09c54ae 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -116,3 +116,16 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 5.0f); * @group Return Mode */ PARAM_DEFINE_INT32(RTL_TYPE, 0); + +/** + * Half-angle of the RTL cone. + * + * Defines the half-angle of the cone which defines the vehicle RTL behavior. + * + * @unit degrees + * @min 0 + * @max 90 + * @increment 1 + * @group Return Mode + */ +PARAM_DEFINE_INT32(RTL_CONE_ANG, 30);