refactor(navigator): split Return destination selection per RTL_TYPE (#29682)

Assisted-by: Claude:claude-opus-5-5
This commit is contained in:
Jonas Perolini authored and GitHub committed 2026-11-18 06:21:06 +01:00
1 parent f76d30eb66
commit 3a2477c00a
2 files changed
+227 -167

No files matched your search

+187 -156
View File
@@ -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<uint8_t>(_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<double>(NAN), static_cast<double>(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;
}
}
+40 -11
View File
@@ -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};