From 3a2477c00a96f78e91a620b1063d2631c76bf142 Mon Sep 17 00:00:00 2001 From: Jonas Perolini Date: Wed, 18 Nov 2026 06:21:06 +0100 Subject: [PATCH] refactor(navigator): split Return destination selection per RTL_TYPE (#29682) Assisted-by: Claude:claude-opus-5-5 --- src/modules/navigator/rtl.cpp | 343 ++++++++++++++++++---------------- src/modules/navigator/rtl.h | 51 +++-- 2 files changed, 227 insertions(+), 167 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 691ba3d7624..97a3f2e7f71 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -56,15 +56,14 @@ using namespace math; static constexpr float MIN_DIST_THRESHOLD = 2.f; // Named constants for RTL_TYPE parameter values (must match value tags in rtl_params.yaml). +static constexpr int RTL_TYPE_CLOSEST_HOME_OR_SAFE_POINT = 0; +static constexpr int RTL_TYPE_MISSION_LAND_OR_SAFE_POINT = 1; static constexpr int RTL_TYPE_MISSION_FAST = 2; static constexpr int RTL_TYPE_DIRECT_WITH_MISSION_LAND = 3; static constexpr int RTL_TYPE_MISSION_FAST_OR_REVERSE = 4; static constexpr int RTL_TYPE_SAFE_POINT_DIRECT = 5; static constexpr int RTL_TYPE_HOME_OR_SAFE_POINT_DIRECT = 6; static constexpr hrt_abstime RTL_REPLAN_INTERVAL{2_s}; -static constexpr uint8_t RTL_STATUS_NO_SAFE_POINT{UINT8_MAX}; -static_assert(DM_KEY_SAFE_POINTS_MAX < RTL_STATUS_NO_SAFE_POINT, - "rtl_status.safe_point_index must represent every configured safe-point slot"); RTL::RTL(Navigator *navigator) : @@ -218,105 +217,119 @@ void RTL::runExecutors() void RTL::setRtlTypeAndDestination() { - uint8_t safe_point_index = RTL_STATUS_NO_SAFE_POINT; - RtlType new_rtl_type{RtlType::RTL_DIRECT}; - const MissionRouteCache &mission_route_cache = _navigator->get_mission_route_cache(); + applyReturn(selectReturn()); +} - // init destination with Home (used also with Type 2 and 4 as backup) - DestinationType destination_type = DestinationType::DESTINATION_TYPE_HOME; +RTL::ReturnSelection RTL::selectReturn() +{ + const int32_t rtl_type = _param_rtl_type.get(); + + // Start at Home, the fallback of every type but 5, which keeps only its yaw. + ReturnSelection selection{}; + selection.destination = homeDestination(); + + switch (rtl_type) { + case RTL_TYPE_MISSION_FAST: + case RTL_TYPE_MISSION_FAST_OR_REVERSE: + selectMissionPath(rtl_type, selection); + break; + + case RTL_TYPE_SAFE_POINT_DIRECT: + selectSafePoint(selection); + break; + + case RTL_TYPE_HOME_OR_SAFE_POINT_DIRECT: + selectHomeIfReachable(selection); + break; + + default: + // 0, 1 and 3, and any other value + selectClosest(rtl_type, selection); + break; + } + + return selection; +} + +PositionYawSetpoint RTL::homeDestination() const +{ PositionYawSetpoint destination; destination.lat = _home_pos_sub.get().lat; destination.lon = _home_pos_sub.get().lon; destination.alt = _home_pos_sub.get().alt; destination.yaw = _home_pos_sub.get().yaw; + return destination; +} - loiter_point_s landing_loiter; - landing_loiter.lat = destination.lat; - landing_loiter.lon = destination.lon; - landing_loiter.height_m = NAN; +void RTL::selectMissionPath(int32_t rtl_type, ReturnSelection &selection) const +{ + // Without a valid mission, go direct to Home. + const bool follow_to_landing = hasMissionLandStart() + && ((rtl_type == RTL_TYPE_MISSION_FAST) || reverseIsFurther()); - // Not every type looks for approaches: report only those this evaluation finds. - _home_has_land_approach = false; - _one_rally_point_has_land_approach = false; + if (follow_to_landing) { + selection.executor = RtlType::RTL_MISSION_FAST; - if (_param_rtl_type.get() == RTL_TYPE_MISSION_FAST) { - if (hasMissionLandStart()) { - new_rtl_type = RtlType::RTL_MISSION_FAST; - - } else if (hasValidMission()) { - new_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE; - - } else { - // no valid mission, go direct to home - new_rtl_type = RtlType::RTL_DIRECT; - } - - } else if (_param_rtl_type.get() == RTL_TYPE_MISSION_FAST_OR_REVERSE) { - if (hasMissionLandStart() && reverseIsFurther()) { - new_rtl_type = RtlType::RTL_MISSION_FAST; - - } else if (hasValidMission()) { - new_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE; - - } else { - // no valid mission, go direct to home - new_rtl_type = RtlType::RTL_DIRECT; - } - - } else if (_param_rtl_type.get() == RTL_TYPE_HOME_OR_SAFE_POINT_DIRECT) { + } else if (hasValidMission()) { + selection.executor = RtlType::RTL_MISSION_FAST_REVERSE; + } +} +void RTL::selectHomeIfReachable(ReturnSelection &selection) +{ #if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL - _home_has_land_approach = mission_route::hasVtolLandApproachesNearLocation(mission_route_cache, destination, - _home_pos_sub.get().alt); + selection.home_has_land_approach = mission_route::hasVtolLandApproachesNearLocation( + _navigator->get_mission_route_cache(), selection.destination, _home_pos_sub.get().alt); #endif - // Set _rtl_direct with the home destination so calc_rtl_time_estimate() can check its reachability - const float rtl_alt_home = computeReturnAltitude(destination); - _rtl_direct.setRtlAlt(rtl_alt_home); - _rtl_direct.setRtlPosition(destination, landing_loiter); + const rtl_time_estimate_s time_to_home = estimateTimeToHome(); - const rtl_time_estimate_s time_to_home = _rtl_direct.calc_rtl_time_estimate(); + const float worst_battery_time_s = worstBatteryTimeRemaining(); - const float worst_battery_time_s = worstBatteryTimeRemaining(); + const bool home_within_reach = time_to_home.valid + && PX4_ISFINITE(worst_battery_time_s) + && (time_to_home.safe_time_estimate < worst_battery_time_s); - const bool home_within_reach = time_to_home.valid - && PX4_ISFINITE(worst_battery_time_s) - && (time_to_home.safe_time_estimate < worst_battery_time_s); + if (!home_within_reach) { + // If battery data is valid, home is out of range: pick the closest rally point unconditionally + // If battery data is unavailable (NaN), we cannot assess reachability: pick the closest home or rally point + const float min_dist = PX4_ISFINITE(worst_battery_time_s) + ? FLT_MAX + : get_distance_to_next_waypoint(_global_pos_sub.get().lat, + _global_pos_sub.get().lon, + _home_pos_sub.get().lat, + _home_pos_sub.get().lon); - if (!home_within_reach) { - // If battery data is valid, home is out of range: pick the closest rally point unconditionally - // If battery data is unavailable (NaN), we cannot assess reachability: pick the closest home or rally point - const float min_dist = PX4_ISFINITE(worst_battery_time_s) - ? FLT_MAX - : get_distance_to_next_waypoint(_global_pos_sub.get().lat, - _global_pos_sub.get().lon, - _home_pos_sub.get().lat, - _home_pos_sub.get().lon); + PositionYawSetpoint safe_point = findClosestSafePoint(min_dist, /*skip_near_home=*/false, + selection.safe_point_index, selection.rally_point_has_land_approach); - PositionYawSetpoint safe_point = findClosestSafePoint(min_dist, safe_point_index); - - if (safe_point_index != UINT8_MAX) { - destination = safe_point; - destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; - } - - // If no rally points are closer (or none are defined), fall back to home (already set as the destination) + if (selection.safe_point_index != RTL_STATUS_NO_SAFE_POINT) { + selection.destination = safe_point; + selection.destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; } - new_rtl_type = RtlType::RTL_DIRECT; - - } else { - // check the closest allowed destination. - findRtlDestination(destination_type, destination, safe_point_index); - - if (destination_type == DestinationType::DESTINATION_TYPE_MISSION_LAND) { - new_rtl_type = RtlType::RTL_DIRECT_MISSION_LAND; - - } else { - - new_rtl_type = RtlType::RTL_DIRECT; - } + // If no rally points are closer (or none are defined), fall back to home (already set as the destination) } +} + +rtl_time_estimate_s RTL::estimateTimeToHome() +{ + const PositionYawSetpoint home = homeDestination(); + loiter_point_s home_loiter; + home_loiter.lat = home.lat; + home_loiter.lon = home.lon; + home_loiter.height_m = NAN; + + _rtl_direct.setRtlAlt(computeReturnAltitude(home)); + _rtl_direct.setRtlPosition(home, home_loiter); + return _rtl_direct.calc_rtl_time_estimate(); +} + +void RTL::applyReturn(const ReturnSelection &selection) +{ + RtlType new_rtl_type = selection.executor; + const PositionYawSetpoint &destination = selection.destination; + const MissionRouteCache &mission_route_cache = _navigator->get_mission_route_cache(); const float rtl_alt = computeReturnAltitude(destination); @@ -346,6 +359,9 @@ void RTL::setRtlTypeAndDestination() _rtl_mission_type_handle->setRtlAlt(rtl_alt); } + // Only RtlDirect flies it + loiter_point_s landing_loiter{}; + if (new_rtl_type == RtlType::RTL_DIRECT) { #if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL landing_loiter = selectLandingApproach(destination); @@ -360,7 +376,7 @@ void RTL::setRtlTypeAndDestination() _rtl_direct.setRtlAlt(rtl_alt); _rtl_direct.setRtlPosition(destination, landing_loiter, - destination_type == DestinationType::DESTINATION_TYPE_CURRENT_POSITION); + selection.destination_type == DestinationType::DESTINATION_TYPE_CURRENT_POSITION); _rtl_type = new_rtl_type; @@ -398,14 +414,14 @@ void RTL::setRtlTypeAndDestination() rtl_status.safe_points_id = mission_route_cache.safePointsId(); rtl_status.is_evaluation_pending = !mission_route_cache.safePointsReady() || mission_route_cache.missionLandItemUpdatePending(); - rtl_status.has_vtol_approach = _home_has_land_approach || _one_rally_point_has_land_approach; + rtl_status.has_vtol_approach = selection.home_has_land_approach || selection.rally_point_has_land_approach; rtl_status.rtl_type = static_cast(_rtl_type); - rtl_status.safe_point_index = safe_point_index; + rtl_status.safe_point_index = selection.safe_point_index; rtl_status.timestamp = hrt_absolute_time(); _rtl_status_pub.publish(rtl_status); } -// Keep out of setRtlTypeAndDestination(): inlined, the battery_status_s copy would stay +// Keep out of selectHomeIfReachable(): inlined, the battery_status_s copy would stay // on its stack while it selects the destination. float RTL::worstBatteryTimeRemaining() { @@ -430,7 +446,8 @@ float RTL::worstBatteryTimeRemaining() return worst_battery_time_s; } -PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_point_index) +PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, bool skip_near_home, uint8_t &safe_point_index, + bool &has_land_approach) const { #if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol @@ -439,7 +456,7 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin const MissionRouteCache &mission_route_cache = _navigator->get_mission_route_cache(); PositionYawSetpoint closest_safe_point{static_cast(NAN), static_cast(NAN), NAN, NAN}; - _one_rally_point_has_land_approach = false; + has_land_approach = false; if (!mission_route_cache.safePointsReady()) { return closest_safe_point; @@ -475,8 +492,7 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin const bool far_from_home = get_distance_to_next_waypoint(_home_pos_sub.get().lat, _home_pos_sub.get().lon, candidate_setpoint.lat, candidate_setpoint.lon) > mission_route::kLandApproachAssociationDistanceM; - if (far_from_home || (_param_rtl_type.get() == RTL_TYPE_SAFE_POINT_DIRECT) - || (_param_rtl_type.get() == RTL_TYPE_HOME_OR_SAFE_POINT_DIRECT)) { + if (far_from_home || !skip_near_home) { const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, candidate_setpoint.lat, candidate_setpoint.lon)}; @@ -486,7 +502,7 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin _home_pos_sub.get().alt) }; - _one_rally_point_has_land_approach |= current_safe_point_has_approaches; + has_land_approach |= current_safe_point_has_approaches; const bool approach_requirement_satisfied = !vtol_in_fw_mode || (_param_rtl_appr_force.get() == 0) || current_safe_point_has_approaches; #else @@ -504,7 +520,7 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin return closest_safe_point; } -void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &destination, uint8_t &safe_point_index) +void RTL::selectClosest(int32_t rtl_type, ReturnSelection &selection) { const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); @@ -512,94 +528,109 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo const MissionRouteCache &mission_route_cache = _navigator->get_mission_route_cache(); float min_dist = FLT_MAX; - if (_param_rtl_type.get() != RTL_TYPE_SAFE_POINT_DIRECT) { #if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL - const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol - && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); - _home_has_land_approach = mission_route::hasVtolLandApproachesNearLocation(mission_route_cache, destination, - _home_pos_sub.get().alt); + const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + selection.home_has_land_approach = mission_route::hasVtolLandApproachesNearLocation(mission_route_cache, + selection.destination, + _home_pos_sub.get().alt); #endif - const bool prioritize_safe_points_over_home = ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode); + const bool prioritize_safe_points_over_home = ((rtl_type == RTL_TYPE_MISSION_LAND_OR_SAFE_POINT) && !vtol_in_rw_mode); #if defined(CONFIG_MODULES_VTOL_ATT_CONTROL) && CONFIG_MODULES_VTOL_ATT_CONTROL - const bool required_approach_missing_for_home = (vtol_in_fw_mode && (_param_rtl_appr_force.get() == 1) && !_home_has_land_approach); + const bool required_approach_missing_for_home = (vtol_in_fw_mode && (_param_rtl_appr_force.get() == 1) + && !selection.home_has_land_approach); #else - constexpr bool required_approach_missing_for_home = false; + constexpr bool required_approach_missing_for_home = false; #endif - // Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we force approach landing for vtol in fw and it is not defined for home. - const bool deprioritize_home = prioritize_safe_points_over_home || required_approach_missing_for_home; + // Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we force approach landing for vtol in fw and it is not defined for home. + const bool deprioritize_home = prioritize_safe_points_over_home || required_approach_missing_for_home; - if (!deprioritize_home) { - // distance to home position - min_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, destination.lat, destination.lon); - } + if (!deprioritize_home) { + // distance to home position + min_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, selection.destination.lat, + selection.destination.lon); + } - // Mission landing - if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == RTL_TYPE_DIRECT_WITH_MISSION_LAND) - || (fabsf(FLT_MAX - min_dist) < FLT_EPSILON)) && hasMissionLandStart()) { - mission_item_s land_mission_item; - int32_t land_index = -1; - const bool success = mission_route_cache.getMissionLandItem(land_index, land_mission_item); + // Mission landing + if (((rtl_type == RTL_TYPE_MISSION_LAND_OR_SAFE_POINT) || (rtl_type == RTL_TYPE_DIRECT_WITH_MISSION_LAND) + || (fabsf(FLT_MAX - min_dist) < FLT_EPSILON)) && hasMissionLandStart()) { + mission_item_s land_mission_item; + int32_t land_index = -1; + const bool success = mission_route_cache.getMissionLandItem(land_index, land_mission_item); - if (!success) { - const mission_s &mission = _mission_sub.get(); - const bool same_reported_source = _mission_land_failure_reported - && _mission_land_failure_mission_id == mission.mission_id - && _mission_land_failure_count == mission.count - && _mission_land_failure_index == mission.land_index - && _mission_land_failure_dataman_id == mission.mission_dataman_id; + if (!success) { + const mission_s &mission = _mission_sub.get(); + const bool same_reported_source = _mission_land_failure_reported + && _mission_land_failure_mission_id == mission.mission_id + && _mission_land_failure_count == mission.count + && _mission_land_failure_index == mission.land_index + && _mission_land_failure_dataman_id == mission.mission_dataman_id; - if (!same_reported_source) { - _mission_land_failure_reported = false; - } - - // A cache miss is expected while Navigator is asynchronously loading the item. - if ((!mission_route_cache.missionLandItemUpdatePending() - || mission_route_cache.missionLandItemAttemptFailed()) && !_mission_land_failure_reported) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission land item could not be read.\t"); - events::send(events::ID("rtl_failed_to_read_land_item"), events::Log::Error, - "Mission land item could not be read"); - _mission_land_failure_mission_id = mission.mission_id; - _mission_land_failure_count = mission.count; - _mission_land_failure_index = mission.land_index; - _mission_land_failure_dataman_id = mission.mission_dataman_id; - _mission_land_failure_reported = true; - } - - } else { + if (!same_reported_source) { _mission_land_failure_reported = false; - const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)}; + } - if ((dist + MIN_DIST_THRESHOLD) < min_dist) { - if (_param_rtl_type.get() != 0) { - min_dist = dist; + // A cache miss is expected while Navigator is asynchronously loading the item. + if ((!mission_route_cache.missionLandItemUpdatePending() + || mission_route_cache.missionLandItemAttemptFailed()) && !_mission_land_failure_reported) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission land item could not be read.\t"); + events::send(events::ID("rtl_failed_to_read_land_item"), events::Log::Error, + "Mission land item could not be read"); + _mission_land_failure_mission_id = mission.mission_id; + _mission_land_failure_count = mission.count; + _mission_land_failure_index = mission.land_index; + _mission_land_failure_dataman_id = mission.mission_dataman_id; + _mission_land_failure_reported = true; + } - } else { - // Mission landing is not allowed, but home has no approaches. Still use mission landing. - min_dist = FLT_MAX; - } + } else { + _mission_land_failure_reported = false; + const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)}; - setLandPosAsDestination(destination, land_mission_item); - destination_type = DestinationType::DESTINATION_TYPE_MISSION_LAND; + if ((dist + MIN_DIST_THRESHOLD) < min_dist) { + if (rtl_type != RTL_TYPE_CLOSEST_HOME_OR_SAFE_POINT) { + min_dist = dist; + + } else { + // Mission landing is not allowed, but home has no approaches. Still use mission landing. + min_dist = FLT_MAX; } + + setLandPosAsDestination(selection.destination, land_mission_item); + selection.destination_type = DestinationType::DESTINATION_TYPE_MISSION_LAND; + selection.executor = RtlType::RTL_DIRECT_MISSION_LAND; } } } // Safe/rally points - PositionYawSetpoint safe_point = findClosestSafePoint(min_dist, safe_point_index); + PositionYawSetpoint safe_point = findClosestSafePoint(min_dist, /*skip_near_home=*/true, + selection.safe_point_index, selection.rally_point_has_land_approach); - if (safe_point_index != RTL_STATUS_NO_SAFE_POINT) { - destination = safe_point; - destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; + if (selection.safe_point_index != RTL_STATUS_NO_SAFE_POINT) { + selection.destination = safe_point; + selection.destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; + selection.executor = RtlType::RTL_DIRECT; + } +} - } else if (_param_rtl_type.get() == RTL_TYPE_SAFE_POINT_DIRECT) { - // for RTL_TYPE=5: if no rally point is found fallback to current position - destination.alt = _global_pos_sub.get().alt; - destination.lat = _global_pos_sub.get().lat; - destination.lon = _global_pos_sub.get().lon; - destination_type = DestinationType::DESTINATION_TYPE_CURRENT_POSITION; +void RTL::selectSafePoint(ReturnSelection &selection) const +{ + const PositionYawSetpoint safe_point = findClosestSafePoint(FLT_MAX, /*skip_near_home=*/false, + selection.safe_point_index, selection.rally_point_has_land_approach); + + if (selection.safe_point_index != RTL_STATUS_NO_SAFE_POINT) { + selection.destination = safe_point; + selection.destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; + + } else { + // if no rally point is found fallback to current position, which keeps Home's yaw + selection.destination.alt = _global_pos_sub.get().alt; + selection.destination.lat = _global_pos_sub.get().lat; + selection.destination.lon = _global_pos_sub.get().lon; + selection.destination_type = DestinationType::DESTINATION_TYPE_CURRENT_POSITION; } } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index c776211b488..1c3a88ab058 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -116,8 +116,43 @@ private: */ bool reverseIsFurther() const; + /** rtl_status.safe_point_index if no rally point was chosen */ + static constexpr uint8_t RTL_STATUS_NO_SAFE_POINT{UINT8_MAX}; + static_assert(DM_KEY_SAFE_POINTS_MAX < RTL_STATUS_NO_SAFE_POINT, + "rtl_status.safe_point_index must represent every configured safe-point slot"); + + /** Where Return goes and which executor flies there. */ + struct ReturnSelection { + RtlType executor{RtlType::RTL_DIRECT}; + DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME}; + PositionYawSetpoint destination{}; + uint8_t safe_point_index{RTL_STATUS_NO_SAFE_POINT}; + bool home_has_land_approach{false}; ///< Home has a VTOL land approach, for the types that look + bool rally_point_has_land_approach{false}; ///< A rally point searched for this selection has one + }; + + /** Selects the destination of the current RTL_TYPE, then applies it. */ void setRtlTypeAndDestination(); + /** Starts at Home, then lets the policy of the current RTL_TYPE change the selection. */ + ReturnSelection selectReturn(); + /** RTL_TYPE 0, 1 and 3: the closest allowed of Home, the mission landing and the rally points. */ + void selectClosest(int32_t rtl_type, ReturnSelection &selection); + /** RTL_TYPE 2 and 4: follow the mission, or go to Home without a valid mission. */ + void selectMissionPath(int32_t rtl_type, ReturnSelection &selection) const; + /** RTL_TYPE 5: the closest rally point, or the current position. */ + void selectSafePoint(ReturnSelection &selection) const; + /** RTL_TYPE 6: Home if the battery reaches it, else the closest rally point. */ + void selectHomeIfReachable(ReturnSelection &selection); + + /** Creates and configures the executor, updates the avoidance planner and publishes rtl_status. */ + void applyReturn(const ReturnSelection &selection); + + PositionYawSetpoint homeDestination() const; + + /** Configures RtlDirect for Home to estimate it; applyReturn() then configures the selected destination. */ + rtl_time_estimate_s estimateTimeToHome(); + /** * @brief Publish the remaining time estimate to go to the Return landing point. * @@ -125,16 +160,13 @@ private: void publishRemainingTimeEstimate(); /** - * @brief Find return destination. + * @brief Find the closest rally point within min_dist * + * @param[in] skip_near_home ignore rally points within 10 m of Home + * @param[out] has_land_approach a rally point looked at has a VTOL land approach */ - void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &destination, uint8_t &safe_point_index); - - /** - * @brief Find return destination if only safe points are considered - * - */ - PositionYawSetpoint findClosestSafePoint(float min_dist, uint8_t &safe_point_index); + PositionYawSetpoint findClosestSafePoint(float min_dist, bool skip_near_home, uint8_t &safe_point_index, + bool &has_land_approach) const; /** * @brief Smallest remaining time [s] of the connected batteries, NAN if none reports one. @@ -188,9 +220,6 @@ private: uint8_t _mission_land_failure_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_0}; bool _mission_land_failure_reported{false}; - bool _home_has_land_approach{false}; ///< Flag if the home position has a land approach defined - bool _one_rally_point_has_land_approach{false}; ///< Flag if a rally point has a land approach defined - RtlDirect _rtl_direct; bool _enforce_rtl_alt{false};