From 024b3d27acd8543a30882be9fa2d25b18f9ddd92 Mon Sep 17 00:00:00 2001 From: Silvan Date: Tue, 6 Jan 2026 11:48:06 +0100 Subject: [PATCH] RTL: add new RTL_TYPE to only allow returns to safe points or last link position Do not allow RTL to Home or mission landings. Signed-off-by: Silvan --- src/modules/navigator/rtl.cpp | 174 ++++++++++++++++++++++------- src/modules/navigator/rtl.h | 19 +++- src/modules/navigator/rtl_params.c | 1 + 3 files changed, 149 insertions(+), 45 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 8d551e87bd..448f16eaa5 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -310,17 +310,71 @@ bool RTL::isLanding() void RTL::setRtlTypeAndDestination() { - init_rtl_mission_type(); - uint8_t safe_point_index{0U}; + uint8_t safe_point_index{UINT8_MAX}; - if (_param_rtl_type.get() != 2 && _param_rtl_type.get() != 4) { + if (_param_rtl_type.get() == 5) { + PositionYawSetpoint rtl_position; + findClosestSafePoint(rtl_position, safe_point_index); + DestinationType destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; + + if (safe_point_index == UINT8_MAX) { + // no safe point found, set destination to last position with valid data link + for (auto &telemetry_status : _telemetry_status_subs) { + telemetry_status_s telemetry; + + if (telemetry_status.update(&telemetry)) { + + if (telemetry.heartbeat_type_gcs) { + _last_position_before_link_loss.alt = _global_pos_sub.get().alt; + _last_position_before_link_loss.lat = _global_pos_sub.get().lat; + _last_position_before_link_loss.lon = _global_pos_sub.get().lon; + break; + } + } + } + + if (!PX4_ISFINITE(_last_position_before_link_loss.lat) || !PX4_ISFINITE(_last_position_before_link_loss.lon)) { + // if we never had a valid data link position, fallback to current position + rtl_position.alt = _global_pos_sub.get().alt; + rtl_position.lat = _global_pos_sub.get().lat; + rtl_position.lon = _global_pos_sub.get().lon; + + } else { + rtl_position = _last_position_before_link_loss; + } + + destination_type = DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION; + } + + // set rtl altitude to the destination from the beginning for DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION + const float rtl_alt = destination_type == DestinationType::DESTINATION_TYPE_SAFE_POINT ? computeReturnAltitude(rtl_position, + (float)_param_rtl_cone_half_angle_deg.get()) : rtl_position.alt; + + loiter_point_s landing_loiter; + landing_loiter.lat = rtl_position.lat; + landing_loiter.lon = rtl_position.lon; + landing_loiter.height_m = NAN; + + land_approaches_s rtl_land_approaches{readVtolLandApproaches(rtl_position)}; + + if (_vehicle_status_sub.get().is_vtol + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) + && rtl_land_approaches.isAnyApproachValid()) { + landing_loiter = chooseBestLandingApproach(rtl_land_approaches); + } + + _rtl_type = RtlType::RTL_DIRECT; + _rtl_direct.setRtlAlt(rtl_alt); + _rtl_direct.setRtlPosition(rtl_position, landing_loiter); + + } else if (_param_rtl_type.get() != 2 && _param_rtl_type.get() != 4) { // check the closest allowed destination. DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME}; PositionYawSetpoint rtl_position; - float rtl_alt; - findRtlDestination(destination_type, rtl_position, rtl_alt, safe_point_index); + findRtlDestination(destination_type, rtl_position, safe_point_index); + const float rtl_alt = computeReturnAltitude(rtl_position, (float)_param_rtl_cone_half_angle_deg.get()); switch (destination_type) { case DestinationType::DESTINATION_TYPE_MISSION_LAND: @@ -366,7 +420,48 @@ void RTL::setRtlTypeAndDestination() } -void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt, +void RTL::findClosestSafePoint(PositionYawSetpoint &rtl_position, uint8_t &safe_point_index) +{ + float min_dist = FLT_MAX; + + if (_safe_points_updated) { + + _one_rally_point_has_land_approach = false; + + for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) { + mission_item_s mission_safe_point; + + const bool success = _dataman_cache_safepoint.loadWait(static_cast(_stats.dataman_id), current_seq, + reinterpret_cast(&mission_safe_point), + sizeof(mission_item_s), 500_ms); + + if (!success) { + PX4_ERR("dm_read failed"); + continue; + } + + if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT) { + const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)}; + + PositionYawSetpoint safepoint_position; + setSafepointAsDestination(safepoint_position, mission_safe_point); + + const bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)}; + + _one_rally_point_has_land_approach |= current_safe_point_has_approaches; + + if (((dist + MIN_DIST_THRESHOLD) < min_dist) && ((_param_rtl_approach_force.get() == 0) + || current_safe_point_has_approaches)) { + min_dist = dist; + rtl_position = safepoint_position; + safe_point_index = current_seq; + } + } + } + } +} + +void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, uint8_t &safe_point_index) { // set destination to home per default, then check if other valid landing spot is closer @@ -448,7 +543,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo const float dist_to_home = get_distance_to_next_waypoint(_home_pos_sub.get().lat, _home_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon); - if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { + if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { // remove! float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)}; PositionYawSetpoint safepoint_position; @@ -468,14 +563,6 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo } } } - - if (_param_rtl_cone_half_angle_deg.get() > 0 - && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - rtl_alt = calculate_return_alt_from_cone_half_angle(rtl_position, (float)_param_rtl_cone_half_angle_deg.get()); - - } else { - rtl_alt = max(_global_pos_sub.get().alt, rtl_position.alt + _param_rtl_return_alt.get()); - } } void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_item_s &land_mission_item) const @@ -487,8 +574,7 @@ void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_ite rtl_position.yaw = _home_pos_sub.get().yaw; } -void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, - const mission_item_s &mission_safe_point) const +void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const { // There is a safe point closer than home/mission landing // TODO: handle all possible mission_safe_point.frame cases @@ -515,41 +601,46 @@ void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, } } -float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position, - float cone_half_angle_deg) const +float RTL::computeReturnAltitude(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const { - // horizontal distance to destination - const float destination_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, - rtl_position.lat, rtl_position.lon); + if (_param_rtl_cone_half_angle_deg.get() > 0 && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + // horizontal distance to destination + const float destination_dist = + get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon); - // minium rtl altitude to use when outside of horizontal acceptance radius of target position. - // We choose the minimum height to be two times the distance from the land position in order to - // avoid the vehicle touching the ground while still moving horizontally. - const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get(); + // minium rtl altitude to use when outside of horizontal acceptance radius of target position. + // We choose the minimum height to be two times the distance from the land position in order to + // avoid the vehicle touching the ground while still moving horizontally. + const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get(); - const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get(); + const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get(); - float return_altitude_amsl = max_return_altitude; + float return_altitude_amsl = max_return_altitude; - if (destination_dist <= _param_nav_acc_rad.get()) { - return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist; + if (destination_dist <= _param_nav_acc_rad.get()) { + return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist; - } else { - if (destination_dist <= _param_rtl_min_dist.get()) { + } else { + if (destination_dist <= _param_rtl_min_dist.get()) { - // constrain cone half angle to meaningful values. All other cases are already handled above. - const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f)); + // constrain cone half angle to meaningful values. All other cases are already handled above. + const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f)); - // minimum altitude we need in order to be within the user defined cone - const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + rtl_position.alt; + // minimum altitude we need in order to be within the user defined cone + const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + rtl_position.alt; - return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl); + return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl); + } + + return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); } - return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); - } + return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude); - return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude); + } else { + // standard behaviour: return altitude above rtl destination + return max(_global_pos_sub.get().alt, rtl_position.alt + _param_rtl_return_alt.get()); + } } void RTL::init_rtl_mission_type() @@ -571,6 +662,9 @@ void RTL::init_rtl_mission_type() } else { new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE; } + + } else if (_param_rtl_type.get() == 5) { + new_rtl_mission_type = RtlType::RTL_DIRECT; } if (_set_rtl_mission_type == new_rtl_mission_type) { diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 207af7ceb5..4d437d17bd 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -55,11 +55,13 @@ #include #include #include +#include #include #include #include #include #include +#include class Navigator; @@ -95,6 +97,7 @@ private: DESTINATION_TYPE_HOME, DESTINATION_TYPE_MISSION_LAND, DESTINATION_TYPE_SAFE_POINT, + DESTINATION_TYPE_LAST_LINK_POSITION }; private: @@ -130,8 +133,13 @@ private: * @brief Find RTL destination. * */ - void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt, - uint8_t &safe_point_index); + void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, uint8_t &safe_point_index); + + /** + * @brief Find RTL destination if only safe points are considered + * + */ + void findClosestSafePoint(PositionYawSetpoint &rtl_position, uint8_t &safe_point_index); /** * @brief Set the position of the land start marker in the planned mission as destination. @@ -147,14 +155,13 @@ private: void setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const; /** - * @brief calculate return altitude from cone half angle + * @brief calculate return altitude from return altitude parameter, current altitude and cone angle * * @param[in] rtl_position landing position of the rtl * @param[in] cone_half_angle_deg half angle of the cone [deg] * @return return altitude */ - float calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position, - float cone_half_angle_deg) const; + float computeReturnAltitude(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const; /** * @brief initialize RTL mission type @@ -223,6 +230,7 @@ private: mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; uint32_t _mission_id = 0u; uint32_t _safe_points_id = 0u; + PositionYawSetpoint _last_position_before_link_loss; mission_stats_entry_s _stats; @@ -246,6 +254,7 @@ private: uORB::SubscriptionData _mission_sub{ORB_ID(mission)}; uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; + uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; uORB::PublicationData _rtl_status_pub{ORB_ID(rtl_status)}; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index af02033c3a..7d6e7dc8de 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -113,6 +113,7 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 10.0f); * @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path. Do not consider rally points. * @value 3 Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land. * @value 4 Return to the planned mission landing, or to home via the reverse mission path, whichever is closer by counting waypoints. Do not consider rally points. + * @value 5 Return directly to safe landing point (do not consider mission landing and Home) * @group Return Mode */ PARAM_DEFINE_INT32(RTL_TYPE, 0);