From cf1332b140155b95e7d155bfed81c79c562a2080 Mon Sep 17 00:00:00 2001 From: Anil Kircaliali Date: Wed, 1 Apr 2026 23:45:48 -0700 Subject: [PATCH 01/10] Add new RTL_TYPE for prioritizing home over rally points with a reachability condition --- src/modules/navigator/rtl.cpp | 48 +++++++++++++++++++++++++-- src/modules/navigator/rtl.h | 2 ++ src/modules/navigator/rtl_params.yaml | 4 ++- 3 files changed, 50 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index d2dc738f6ba5..84f2a25d5dd6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -165,6 +165,7 @@ void RTL::on_inactive() _mission_sub.update(); _home_pos_sub.update(); _wind_sub.update(); + _battery_status_sub.update(); updateDatamanCache(); @@ -225,7 +226,7 @@ void RTL::on_activation() _mission_sub.update(); _home_pos_sub.update(); _wind_sub.update(); - + _battery_status_sub.update(); setRtlTypeAndDestination(); switch (_rtl_type) { @@ -265,7 +266,6 @@ void RTL::on_active() _mission_sub.update(); _home_pos_sub.update(); _wind_sub.update(); - updateDatamanCache(); switch (_rtl_type) { @@ -368,6 +368,48 @@ void RTL::setRtlTypeAndDestination() new_rtl_type = RtlType::RTL_DIRECT; } + } else if (_param_rtl_type.get() == 6) { + // estimate time to fly home using the same infrastructure as the battery failsafe +<<<<<<< HEAD + const float rtl_alt_home = computeReturnAltitude(destination, DestinationType::DESTINATION_TYPE_HOME, + (float)_param_rtl_cone_ang.get()); +======= + const float rtl_alt_home = computeReturnAltitude(destination); +>>>>>>> a1dbebc558 (fixup! Add new RTL_TYPE for prioritizing home over rally points with a reachability condition) + _rtl_direct.setRtlAlt(rtl_alt_home); + _rtl_direct.setRtlPosition(destination, landing_loiter); + + const rtl_time_estimate_s time_to_home = _rtl_direct.calc_rtl_time_estimate(); + const float battery_remaining_s = _battery_status_sub.get().time_remaining_s; + + const bool home_within_reach = time_to_home.valid + && PX4_ISFINITE(battery_remaining_s) + && (time_to_home.safe_time_estimate < battery_remaining_s); + + if (!home_within_reach) { + // home is out of battery range, pick closest rally point + PositionYawSetpoint safe_point = findClosestSafePoint(FLT_MAX, safe_point_index); + + if (safe_point_index != UINT8_MAX) { + destination = safe_point; + destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; + } + // If no rally points are defined, fall back to home (already set as the destination) + } + + new_rtl_type = RtlType::RTL_DIRECT; + + land_approaches_s rtl_land_approaches{readVtolLandApproaches(destination)}; + landing_loiter.lat = destination.lat; + landing_loiter.lon = destination.lon; + landing_loiter.height_m = NAN; + + 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); + } + } else { // check the closest allowed destination. findRtlDestination(destination_type, destination, safe_point_index); @@ -450,7 +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, mission_safe_point.lat, mission_safe_point.lon) > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES; - if (far_from_home || (_param_rtl_type.get() == 5)) { + if (far_from_home || (_param_rtl_type.get() == 5) || (_param_rtl_type.get() == 6)) { 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; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index a7c64ef60fee..0fbd323d6d90 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -60,6 +60,7 @@ #include #include #include +#include class Navigator; @@ -248,6 +249,7 @@ class RTL : public NavigatorMode, public ModuleParams uORB::SubscriptionData _mission_sub{ORB_ID(mission)}; uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; + uORB::SubscriptionData _battery_status_sub{ORB_ID(battery_status)}; uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; uORB::Publication _rtl_status_pub{ORB_ID(rtl_status)}; diff --git a/src/modules/navigator/rtl_params.yaml b/src/modules/navigator/rtl_params.yaml index 30c09f65bc49..e6d1519dcafc 100644 --- a/src/modules/navigator/rtl_params.yaml +++ b/src/modules/navigator/rtl_params.yaml @@ -78,7 +78,9 @@ parameters: Skip DO_JUMP and other non-position mission items while following either mission path. Do not consider rally points. 5: Return directly to safe landing point (do not consider mission landing - and Home) + and Home). + 6: Return to home if time estimate to home is less than battery remaining estimate, + else return to the closest rally point. default: 0 RTL_CONE_ANG: description: From fbc59595257230fb9f3259eb5d8336725175fb69 Mon Sep 17 00:00:00 2001 From: Anil Kircaliali Date: Fri, 3 Apr 2026 10:20:20 -0700 Subject: [PATCH 02/10] If battery time estimate is unavailable, return to the closest safe (home or rally) point. --- src/modules/navigator/rtl.cpp | 17 ++++++++++++++--- src/modules/navigator/rtl_params.yaml | 3 ++- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 84f2a25d5dd6..b460957fd7e4 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -386,15 +386,26 @@ void RTL::setRtlTypeAndDestination() && PX4_ISFINITE(battery_remaining_s) && (time_to_home.safe_time_estimate < battery_remaining_s); + PX4_INFO("RTL: time to home: %.1fs, battery remaining: %.1fs, home within reach: %s", + static_cast(time_to_home.safe_time_estimate), static_cast(battery_remaining_s), home_within_reach ? "true" : "false"); + if (!home_within_reach) { - // home is out of battery range, pick closest rally point - PositionYawSetpoint safe_point = findClosestSafePoint(FLT_MAX, safe_point_index); + // 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(battery_remaining_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, safe_point_index); if (safe_point_index != UINT8_MAX) { destination = safe_point; destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; } - // If no rally points are defined, fall back to home (already set as the destination) + // If no rally points are closer (or none are defined), fall back to home (already set as the destination) } new_rtl_type = RtlType::RTL_DIRECT; diff --git a/src/modules/navigator/rtl_params.yaml b/src/modules/navigator/rtl_params.yaml index e6d1519dcafc..ff35104c7095 100644 --- a/src/modules/navigator/rtl_params.yaml +++ b/src/modules/navigator/rtl_params.yaml @@ -80,7 +80,8 @@ parameters: 5: Return directly to safe landing point (do not consider mission landing and Home). 6: Return to home if time estimate to home is less than battery remaining estimate, - else return to the closest rally point. + else return to the closest rally point. If battery remaining estimate is not available, + return to the closest safe point (home or rally point). default: 0 RTL_CONE_ANG: description: From e30070a773ad242011d6b4681a22faa3559083e4 Mon Sep 17 00:00:00 2001 From: Anil Kircaliali Date: Fri, 3 Apr 2026 13:58:50 -0700 Subject: [PATCH 03/10] Remove debug log for testing --- src/modules/navigator/rtl.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b460957fd7e4..e2eceb7146f1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -386,9 +386,6 @@ void RTL::setRtlTypeAndDestination() && PX4_ISFINITE(battery_remaining_s) && (time_to_home.safe_time_estimate < battery_remaining_s); - PX4_INFO("RTL: time to home: %.1fs, battery remaining: %.1fs, home within reach: %s", - static_cast(time_to_home.safe_time_estimate), static_cast(battery_remaining_s), home_within_reach ? "true" : "false"); - 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 From 744a396526d8106c2187fc9c582be620bcb1e71d Mon Sep 17 00:00:00 2001 From: Anil Kircaliali Date: Mon, 6 Apr 2026 12:49:25 -0700 Subject: [PATCH 04/10] add SITL tests for RTL_TYPE 6 NaN battery fallback --- test/mavsdk_tests/CMakeLists.txt | 1 + test/mavsdk_tests/test_multicopter_rtl.cpp | 95 ++++++++++++++++++++++ 2 files changed, 96 insertions(+) create mode 100644 test/mavsdk_tests/test_multicopter_rtl.cpp diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 73da84c07ecb..729d776489ef 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -27,6 +27,7 @@ if(MAVSDK_FOUND) test_multicopter_failure_injection.cpp test_multicopter_failsafe.cpp test_multicopter_mission.cpp + test_multicopter_rtl.cpp test_multicopter_offboard.cpp test_multicopter_manual.cpp test_vtol_mission.cpp diff --git a/test/mavsdk_tests/test_multicopter_rtl.cpp b/test/mavsdk_tests/test_multicopter_rtl.cpp new file mode 100644 index 000000000000..1d13d3579975 --- /dev/null +++ b/test/mavsdk_tests/test_multicopter_rtl.cpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "autopilot_tester_rtl.h" + + +// TODO: Add test cases for these scenarios: +// - "RTL type 6 home within reach with no rally points", "[multicopter]" +// - "RTL type 6 home within reach with rally points and home is the closest", "[multicopter]" +// - "RTL type 6 home within reach with rally points and home is not the closest", "[multicopter]" +// - "RTL type 6 home out of reach with rally points and at least one rally point within reach", "[multicopter]" + +TEST_CASE("RTL_TYPE=6 time_remaining_s NaN with no rally points", "[multicopter]") +{ + AutopilotTesterRtl tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.store_home(); + tester.set_rtl_type(6); + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.execute_rtl(); + tester.wait_until_disarmed(std::chrono::seconds(90)); + tester.check_home_within(5.0f); +} + +TEST_CASE("RTL_TYPE=6 time_remaining_s NaN with rally points and home is closest", "[multicopter]") +{ + AutopilotTesterRtl tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.store_home(); + tester.set_rtl_type(6); + // Rally is farther than home; vehicle is above home after takeoff so home wins on distance + tester.add_local_rally_point({100., -100.}); + tester.upload_rally_points(); + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.execute_rtl(); + tester.wait_until_disarmed(std::chrono::seconds(90)); + tester.check_home_within(5.0f); +} + +TEST_CASE("RTL_TYPE=6 time_remaining_s NaN with rally points and home is not closest", "[multicopter]") +{ + AutopilotTesterRtl tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.store_home(); + tester.set_rtl_type(6); + // Rally point is between the vehicle and home, so rally point wins on distance + tester.add_local_rally_point({300., 0.}); + tester.upload_rally_points(); + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + // Distance to home is 400m, distance to rally is 100m + tester.offboard_goto({400.f, 0.f, -20.f}, 5.f, std::chrono::seconds(60)); + tester.execute_rtl(); + tester.wait_until_disarmed(std::chrono::seconds(150)); + tester.check_rally_point_within(5.0f); + tester.check_home_not_within(20.0f); +} From 7a1dab74302cfeb090fe9734404d8126252a8ad4 Mon Sep 17 00:00:00 2001 From: Anil Kircaliali Date: Mon, 6 Apr 2026 12:53:00 -0700 Subject: [PATCH 05/10] fix formatting per CI --- src/modules/navigator/rtl.cpp | 26 +++++++++++--------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index e2eceb7146f1..5282d7a76761 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -370,12 +370,7 @@ void RTL::setRtlTypeAndDestination() } else if (_param_rtl_type.get() == 6) { // estimate time to fly home using the same infrastructure as the battery failsafe -<<<<<<< HEAD - const float rtl_alt_home = computeReturnAltitude(destination, DestinationType::DESTINATION_TYPE_HOME, - (float)_param_rtl_cone_ang.get()); -======= const float rtl_alt_home = computeReturnAltitude(destination); ->>>>>>> a1dbebc558 (fixup! Add new RTL_TYPE for prioritizing home over rally points with a reachability condition) _rtl_direct.setRtlAlt(rtl_alt_home); _rtl_direct.setRtlPosition(destination, landing_loiter); @@ -383,18 +378,18 @@ void RTL::setRtlTypeAndDestination() const float battery_remaining_s = _battery_status_sub.get().time_remaining_s; const bool home_within_reach = time_to_home.valid - && PX4_ISFINITE(battery_remaining_s) - && (time_to_home.safe_time_estimate < battery_remaining_s); + && PX4_ISFINITE(battery_remaining_s) + && (time_to_home.safe_time_estimate < battery_remaining_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(battery_remaining_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); + ? 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, safe_point_index); @@ -402,6 +397,7 @@ void RTL::setRtlTypeAndDestination() 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) } @@ -413,9 +409,9 @@ void RTL::setRtlTypeAndDestination() landing_loiter.height_m = NAN; 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); + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) + && rtl_land_approaches.isAnyApproachValid()) { + landing_loiter = chooseBestLandingApproach(rtl_land_approaches); } } else { From c9b2cc89f4dd7d718a214ac5d1a8073d01697291 Mon Sep 17 00:00:00 2001 From: Anil Kircaliali Date: Wed, 8 Apr 2026 15:05:33 -0700 Subject: [PATCH 06/10] Add NAV_CMD_RALLY_POINT check again to make sure VTOL implementation remains the same --- src/modules/navigator/rtl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 5282d7a76761..50739aaaa4ae 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -496,7 +496,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, mission_safe_point.lat, mission_safe_point.lon) > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES; - if (far_from_home || (_param_rtl_type.get() == 5) || (_param_rtl_type.get() == 6)) { + if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && (far_from_home || (_param_rtl_type.get() == 5) || (_param_rtl_type.get() == 6))) { 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; From 4d36039e61df3c1dc4e6f08ccdd87278f4cf3fb6 Mon Sep 17 00:00:00 2001 From: Anil Kircaliali Date: Tue, 21 Apr 2026 12:55:19 -0700 Subject: [PATCH 07/10] Add documentation for RTL_TYPEs --- docs/en/flight_modes/return.md | 53 ++++++++++++++++++++++++++++++++-- docs/en/releases/main.md | 3 ++ 2 files changed, 53 insertions(+), 3 deletions(-) diff --git a/docs/en/flight_modes/return.md b/docs/en/flight_modes/return.md index 3af824f93866..1dd2ce96d845 100644 --- a/docs/en/flight_modes/return.md +++ b/docs/en/flight_modes/return.md @@ -44,7 +44,7 @@ The following sections explain how to configure the [return type](#return_types) ## Return Types (RTL_TYPE) -PX4 provides four alternative approaches for finding an unobstructed path to a safe destination and/or landing, which are set using the [RTL_TYPE](#RTL_TYPE) parameter. +PX4 provides several alternative approaches for finding an unobstructed path to a safe destination and/or landing, which are set using the [RTL_TYPE](#RTL_TYPE) parameter. At high level these are: @@ -56,6 +56,12 @@ At high level these are: If no _mission_ defined, return direct to home (rally points are ignored). - [Closest safe destination return](#closest-safe-destination-return-type-rtl-type-3) (`RTL_TYPE=3`): Ascend to a safe altitude and return via direct path to closest destination: home, start of mission landing pattern, or rally point. If the destination is a mission landing pattern, follow the pattern to land. +- [Closest mission landing or reverse mission return](#closest-mission-landing-or-reverse-mission-return-type-rtl-type-4) (`RTL_TYPE=4`): Like `RTL_TYPE=2`, but chooses between mission landing and reverse mission path based on which requires fewer waypoints to traverse. + Rally points are not considered. +- [Rally point return](#rally-point-return-type-rtl-type-5) (`RTL_TYPE=5`): Return directly to the closest rally point, ignoring home and mission landing. + If no rally point is defined, land at the current position. +- [Battery-aware home priority return](#battery-aware-home-priority-return-type-rtl-type-6) (`RTL_TYPE=6`): Return to home if the estimated flight time is within the remaining battery time, otherwise return to the closest rally point. + Falls back to the closest safe point (home or rally) if battery time remaining is unavailable. More detailed explanations for each of the types are provided in the following sections. @@ -145,7 +151,7 @@ This is only an approximation of the flown path length, because the number if mi ### Closest Safe Destination Return Type (RTL_TYPE=3) -In this return type the vehicle: +In this return type, the vehicle: - Ascends to a safe [minimum return altitude](#minimum-return-altitude) (above any expected obstacles). - Flies a direct path to the closest destination of: home location, mission landing pattern or rally point. @@ -154,6 +160,47 @@ In this return type the vehicle: By default an MC or VTOL in MC mode will land, and a fixed-wing vehicle circles at the descent altitude. A VTOL in FW mode aligns its heading to the destination point, transitions to MC mode, and then lands. +### Closest Mission Landing or Reverse Mission Return Type (RTL_TYPE=4) + +This return type is similar to [Mission Path Return (RTL_TYPE=2)](#mission-path-return-type-rtl-type-2) but selects between the forward mission landing and the reverse mission path based on +which requires fewer waypoints to traverse from the current position, rather than always preferring the forward mission landing. + +In this return type, the vehicle: + +- If a mission landing is defined and the landing start is closer by waypoint count than the mission start +(i.e. fewer waypoints remain to reach the landing than have been passed since the start): flies fast-forward +through the remaining mission waypoints to land. +- Otherwise, if a valid mission is defined: fast-reverses the mission path back to home. +- If no mission is defined: flies directly to home. + +Rally points are not considered. +Landing behaviour at the destination follows the same rules as RTL_TYPE=2. + +### Rally Point Return Type (RTL_TYPE=5) + +In this return type, the vehicle ignores home and mission landing patterns entirely and returns only to rally points: + +- Ascends to a safe minimum return altitude (above any expected obstacles). +- Flies via direct path to the closest rally point. +- Lands or waits at the rally point destination. + +::: info +If no rally points are defined the vehicle will land at its current position rather than returning to home. +This type is intended for use cases where only pre-approved rally points are acceptable landing destinations. +::: + +### Battery-Aware Home Priority Return Type (RTL_TYPE=6) + +In this return type, the vehicle uses the estimated flight time to home compared against the remaining battery time to decide the return destination: + +- Computes the estimated time to fly home, including a safety factor and margin (RTL_TIME_FACTOR and RTL_TIME_MARGIN). +- If the estimated flight time to home is less than the remaining battery time (`time_remaining_s`): returns to home via a direct path. +- If home is not within battery reach: returns to the closest rally point via a direct path. +- If no rally point is closer than home, or no rally points are defined, falls back to home. +- If the battery time remaining estimate is unavailable (e.g. no current sensor, battery capacity not +configured): falls back to returning to the closest safe point — whichever of home or the defined rally +points is nearest. + ## Minimum Return Altitude For most [return types](#return_types) a vehicle will ascend to a _minimum safe altitude_ before returning (unless already above that altitude), in order to avoid any obstacles between it and the destination. @@ -214,7 +261,7 @@ The RTL parameters are listed in [Parameter Reference > Return Mode](../advanced | Parameter | Description | | ----------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| [RTL_TYPE](../advanced_config/parameter_reference.md#RTL_TYPE) | Return mechanism (path and destination).
`0`: Return to a rally point or home (whichever is closest) via direct path.
`1`: Return to a rally point or the mission landing pattern start point (whichever is closest), via direct path. If neither mission landing or rally points are defined return home via a direct path. If the destination is a mission landing pattern, follow the pattern to land.
`2`: Use the mission path to landing while skipping DO_JUMP and other non-position mission items if a landing pattern is defined, otherwise fast-reverse to home with the same traversal rules. Ignore rally points. Fly direct to home if no mission plan is defined.
`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. | +| [RTL_TYPE](../advanced_config/parameter_reference.md#RTL_TYPE) | Return mechanism (path and destination).
`0`: Return to a rally point or home (whichever is closest) via direct path.
`1`: Return to a rally point or the mission landing pattern start point (whichever is closest), via direct path. If neither mission landing or rally points are defined return home via a direct path. If the destination is a mission landing pattern, follow the pattern to land.
`2`: Use the mission path to landing while skipping DO_JUMP and other non-position mission items if a landing pattern is defined, otherwise fast-reverse to home with the same traversal rules. Ignore rally points. Fly direct to home if no mission plan is defined.
`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.
`4`: Like type 2, but selects between mission landing (fast-forward) and reverse mission path based on which requires fewer waypoints from the current position. Rally points not considered. Fly direct to home if no mission is defined.
`5`: Return via direct path to closest rally point. If no rally points are defined, land at current position.
`6`: Return to home if estimated flight time to home is less than remaining battery time, otherwise return to closest rally point. Falls back to closest safe point (home or rally) if battery time remaining is unavailable. | | [RTL_RETURN_ALT](../advanced_config/parameter_reference.md#RTL_RETURN_ALT) | Return altitude in meters (default: 60m) when [RTL_CONE_ANG](../advanced_config/parameter_reference.md#RTL_CONE_ANG) is 0. If already above this value the vehicle will return at its current altitude. | | [RTL_DESCEND_ALT](../advanced_config/parameter_reference.md#RTL_DESCEND_ALT) | Minimum return altitude and altitude at which the vehicle will slow or stop its initial descent from a higher return altitude (default: 30m) | | [RTL_LAND_DELAY](../advanced_config/parameter_reference.md#RTL_LAND_DELAY) | Time to wait at `RTL_DESCEND_ALT` before landing (default: 0.5s) -by default this period is short so that the vehicle will simply slow and then land immediately. If set to -1 the system will loiter at `RTL_DESCEND_ALT` rather than landing. The delay is provided to allow you to configure time for landing gear to be deployed (triggered automatically). | diff --git a/docs/en/releases/main.md b/docs/en/releases/main.md index 1c0bcd31cf23..88b72db11023 100644 --- a/docs/en/releases/main.md +++ b/docs/en/releases/main.md @@ -48,6 +48,9 @@ Please continue reading for [upgrade instructions](#upgrade-guide). - Added new flight mode(s): [Altitude Cruise (MC)](../flight_modes_mc/altitude_cruise.md), Altitude Cruise (FW). For fixed-wing the mode behaves the same as Altitude mode but you can disable the manual control loss failsafe. ([PX4-Autopilot#25435: Add new flight mode: Altitude Cruise](https://github.com/PX4/PX4-Autopilot/pull/25435)). +- Added `RTL_TYPE=6` for battery-aware home priority return ([PX4-Autopilot#26968](https://github.com/PX4/PX4-Autopilot/pull/26968)). + Returns to home if the estimated flight time to home is within the remaining battery time; otherwise returns to the closest rally point. + Falls back to the closest safe point (home or rally) if battery time remaining is unavailable. ### Estimation From 364dcf573aadd07226af90f5231e9d43693a11a4 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Wed, 22 Apr 2026 17:11:47 +1000 Subject: [PATCH 08/10] docs(update): subedit --- docs/en/flight_modes/return.md | 76 +++++++++++++++++----------------- 1 file changed, 37 insertions(+), 39 deletions(-) diff --git a/docs/en/flight_modes/return.md b/docs/en/flight_modes/return.md index 1dd2ce96d845..003ea7e54570 100644 --- a/docs/en/flight_modes/return.md +++ b/docs/en/flight_modes/return.md @@ -4,11 +4,11 @@ The _Return_ flight mode is used to _fly a vehicle to safety_ on an unobstructed path to a safe destination, where it should land. -The following topics should be read first if you're using these vehicle types: +Each vehicle has a **default** return mode behavior which is described in the linked topics (read those first if you plan on using the defaults): -- [Multicopter](../flight_modes_mc/return.md) -- [Fixed-wing (Plane)](../flight_modes_fw/return.md) -- [VTOL](../flight_modes_vtol/return.md) +- [Multicopter](../flight_modes_mc/return.md) — Home/rally point return +- [Fixed-wing (Plane)](../flight_modes_fw/return.md) — Mission landing/Rally point return +- [VTOL](../flight_modes_vtol/return.md) — Mission landing/Rally point return ::: info @@ -40,11 +40,9 @@ This topic covers all the possible return types that any vehicle _might_ be conf The following sections explain how to configure the [return type](#return_types), [minimum return altitude](#minimum-return-altitude) and [landing/arrival behaviour](#loiter-landing-at-destination). - +## Return Types (RTL_TYPE) {#return_types} -## Return Types (RTL_TYPE) - -PX4 provides several alternative approaches for finding an unobstructed path to a safe destination and/or landing, which are set using the [RTL_TYPE](#RTL_TYPE) parameter. +PX4 provides a number of alternative approaches for finding an unobstructed path to a safe destination and/or landing, which are set using the [RTL_TYPE](#RTL_TYPE) parameter. At high level these are: @@ -61,13 +59,11 @@ At high level these are: - [Rally point return](#rally-point-return-type-rtl-type-5) (`RTL_TYPE=5`): Return directly to the closest rally point, ignoring home and mission landing. If no rally point is defined, land at the current position. - [Battery-aware home priority return](#battery-aware-home-priority-return-type-rtl-type-6) (`RTL_TYPE=6`): Return to home if the estimated flight time is within the remaining battery time, otherwise return to the closest rally point. - Falls back to the closest safe point (home or rally) if battery time remaining is unavailable. + Falls back to the closest safe point (home or rally) if the remaining battery time is unknown/unavailable. More detailed explanations for each of the types are provided in the following sections. - - -### Home/Rally Point Return Type (RTL_TYPE=0) +### Home/Rally Point Return Type (RTL_TYPE=0) {#home_return} This is the default return type for a [multicopter](../flight_modes_mc/return.md) (see topic for more information). @@ -160,21 +156,19 @@ In this return type, the vehicle: By default an MC or VTOL in MC mode will land, and a fixed-wing vehicle circles at the descent altitude. A VTOL in FW mode aligns its heading to the destination point, transitions to MC mode, and then lands. -### Closest Mission Landing or Reverse Mission Return Type (RTL_TYPE=4) +### Closest of Mission Landing or Reverse Mission Return Type (RTL_TYPE=4) -This return type is similar to [Mission Path Return (RTL_TYPE=2)](#mission-path-return-type-rtl-type-2) but selects between the forward mission landing and the reverse mission path based on -which requires fewer waypoints to traverse from the current position, rather than always preferring the forward mission landing. +This return type is similar to [Mission Path Return (RTL_TYPE=2)](#mission-path-return-type-rtl-type-2), but selects between the forward mission landing and the reverse mission path based on +which requires fewer waypoints to traverse from the current position (instead of always preferring the forward mission landing). In this return type, the vehicle: -- If a mission landing is defined and the landing start is closer by waypoint count than the mission start -(i.e. fewer waypoints remain to reach the landing than have been passed since the start): flies fast-forward -through the remaining mission waypoints to land. -- Otherwise, if a valid mission is defined: fast-reverses the mission path back to home. -- If no mission is defined: flies directly to home. - -Rally points are not considered. -Landing behaviour at the destination follows the same rules as RTL_TYPE=2. +- If valid mission landing is defined: + - Flies fast-forward through the remaining mission waypoints to land if the start of the landing sequence is closer than the start of the mission (by waypoint count). + - Otherwise, fast-reverses the mission path back to home. +- If no mission is defined the vehicle flies directly to home. +- Rally points are not considered. +- Landing behaviour at the destination follows the same rules as RTL_TYPE=2. ### Rally Point Return Type (RTL_TYPE=5) @@ -185,21 +179,23 @@ In this return type, the vehicle ignores home and mission landing patterns entir - Lands or waits at the rally point destination. ::: info -If no rally points are defined the vehicle will land at its current position rather than returning to home. +If no rally points are defined the vehicle will land at its current position instead of returning to home. This type is intended for use cases where only pre-approved rally points are acceptable landing destinations. ::: ### Battery-Aware Home Priority Return Type (RTL_TYPE=6) -In this return type, the vehicle uses the estimated flight time to home compared against the remaining battery time to decide the return destination: +In this return type, the vehicle uses the estimated flight time to home compared against the remaining battery time to decide the return destination. + +The vehicle: -- Computes the estimated time to fly home, including a safety factor and margin (RTL_TIME_FACTOR and RTL_TIME_MARGIN). +- Computes the estimated time to fly home, including a safety factor and margin ([RTL_TIME_FACTOR](#RTL_TIME_FACTOR) and [RTL_TIME_MARGIN](#RTL_TIME_MARGIN)). - If the estimated flight time to home is less than the remaining battery time (`time_remaining_s`): returns to home via a direct path. - If home is not within battery reach: returns to the closest rally point via a direct path. - If no rally point is closer than home, or no rally points are defined, falls back to home. - If the battery time remaining estimate is unavailable (e.g. no current sensor, battery capacity not -configured): falls back to returning to the closest safe point — whichever of home or the defined rally -points is nearest. + configured): falls back to returning to the closest safe point — whichever of home or the defined rally + points is nearest. ## Minimum Return Altitude @@ -259,15 +255,17 @@ For this reason fixed-wing vehicles are configured to use [Mission landing/reall The RTL parameters are listed in [Parameter Reference > Return Mode](../advanced_config/parameter_reference.md#return-mode) (and summarised below). -| Parameter | Description | -| ----------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| Parameter | Description | +| ----------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | | [RTL_TYPE](../advanced_config/parameter_reference.md#RTL_TYPE) | Return mechanism (path and destination).
`0`: Return to a rally point or home (whichever is closest) via direct path.
`1`: Return to a rally point or the mission landing pattern start point (whichever is closest), via direct path. If neither mission landing or rally points are defined return home via a direct path. If the destination is a mission landing pattern, follow the pattern to land.
`2`: Use the mission path to landing while skipping DO_JUMP and other non-position mission items if a landing pattern is defined, otherwise fast-reverse to home with the same traversal rules. Ignore rally points. Fly direct to home if no mission plan is defined.
`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.
`4`: Like type 2, but selects between mission landing (fast-forward) and reverse mission path based on which requires fewer waypoints from the current position. Rally points not considered. Fly direct to home if no mission is defined.
`5`: Return via direct path to closest rally point. If no rally points are defined, land at current position.
`6`: Return to home if estimated flight time to home is less than remaining battery time, otherwise return to closest rally point. Falls back to closest safe point (home or rally) if battery time remaining is unavailable. | -| [RTL_RETURN_ALT](../advanced_config/parameter_reference.md#RTL_RETURN_ALT) | Return altitude in meters (default: 60m) when [RTL_CONE_ANG](../advanced_config/parameter_reference.md#RTL_CONE_ANG) is 0. If already above this value the vehicle will return at its current altitude. | -| [RTL_DESCEND_ALT](../advanced_config/parameter_reference.md#RTL_DESCEND_ALT) | Minimum return altitude and altitude at which the vehicle will slow or stop its initial descent from a higher return altitude (default: 30m) | -| [RTL_LAND_DELAY](../advanced_config/parameter_reference.md#RTL_LAND_DELAY) | Time to wait at `RTL_DESCEND_ALT` before landing (default: 0.5s) -by default this period is short so that the vehicle will simply slow and then land immediately. If set to -1 the system will loiter at `RTL_DESCEND_ALT` rather than landing. The delay is provided to allow you to configure time for landing gear to be deployed (triggered automatically). | -| [RTL_MIN_DIST](../advanced_config/parameter_reference.md#RTL_MIN_DIST) | Minimum horizontal distance from home position to trigger ascent to the return altitude specified by the "cone". If the vehicle is horizontally closer than this distance to home, it will return at its current altitude or `RTL_DESCEND_ALT` (whichever is higher) instead of first ascending to RTL_RETURN_ALT. | -| [RTL_CONE_ANG](../advanced_config/parameter_reference.md#RTL_CONE_ANG) | Half-angle of the cone that defines the vehicle RTL return altitude. Values (in degrees): 0, 25, 45, 65, 80, 90. Note that 0 is "no cone" (always return at `RTL_RETURN_ALT` or higher), while 90 indicates that the vehicle must return at the current altitude or `RTL_DESCEND_ALT` (whichever is higher). | -| [COM_RC_OVERRIDE](../advanced_config/parameter_reference.md#COM_RC_OVERRIDE) | Controls whether stick movement on a multicopter (or VTOL in MC mode) causes a mode change to [Position mode](../flight_modes_mc/position.md) (except when vehicle is handling a critical battery failsafe). This can be separately enabled for auto modes and for offboard mode, and is enabled in auto modes by default. | -| [COM_RC_STICK_OV](../advanced_config/parameter_reference.md#COM_RC_STICK_OV) | The amount of stick movement that causes a transition to [Position mode](../flight_modes_mc/position.md) (if [COM_RC_OVERRIDE](#COM_RC_OVERRIDE) is enabled). | -| [RTL_LOITER_RAD](../advanced_config/parameter_reference.md#RTL_LOITER_RAD) | [Fixed-wing Only] The radius of the loiter circle (at [RTL_LAND_DELAY](#RTL_LAND_DELAY)). | -| [MIS_TKO_LAND_REQ](../advanced_config/parameter_reference.md#MIS_TKO_LAND_REQ) | Specify whether a mission landing or takeoff pattern is _required_. Generally fixed-wing vehicles set this to require a landing pattern but VTOL do not. | +| [RTL_RETURN_ALT](../advanced_config/parameter_reference.md#RTL_RETURN_ALT) | Return altitude in meters (default: 60m) when [RTL_CONE_ANG](../advanced_config/parameter_reference.md#RTL_CONE_ANG) is 0. If already above this value the vehicle will return at its current altitude. | +| [RTL_DESCEND_ALT](../advanced_config/parameter_reference.md#RTL_DESCEND_ALT) | Minimum return altitude and altitude at which the vehicle will slow or stop its initial descent from a higher return altitude (default: 30m) | +| [RTL_LAND_DELAY](../advanced_config/parameter_reference.md#RTL_LAND_DELAY) | Time to wait at `RTL_DESCEND_ALT` before landing (default: 0.5s) -by default this period is short so that the vehicle will simply slow and then land immediately. If set to -1 the system will loiter at `RTL_DESCEND_ALT` rather than landing. The delay is provided to allow you to configure time for landing gear to be deployed (triggered automatically). | +| [RTL_MIN_DIST](../advanced_config/parameter_reference.md#RTL_MIN_DIST) | Minimum horizontal distance from home position to trigger ascent to the return altitude specified by the "cone". If the vehicle is horizontally closer than this distance to home, it will return at its current altitude or `RTL_DESCEND_ALT` (whichever is higher) instead of first ascending to RTL_RETURN_ALT. | +| [RTL_CONE_ANG](../advanced_config/parameter_reference.md#RTL_CONE_ANG) | Half-angle of the cone that defines the vehicle RTL return altitude. Values (in degrees): 0, 25, 45, 65, 80, 90. Note that 0 is "no cone" (always return at `RTL_RETURN_ALT` or higher), while 90 indicates that the vehicle must return at the current altitude or `RTL_DESCEND_ALT` (whichever is higher). | +| [COM_RC_OVERRIDE](../advanced_config/parameter_reference.md#COM_RC_OVERRIDE) | Controls whether stick movement on a multicopter (or VTOL in MC mode) causes a mode change to [Position mode](../flight_modes_mc/position.md) (except when vehicle is handling a critical battery failsafe). This can be separately enabled for auto modes and for offboard mode, and is enabled in auto modes by default. | +| [COM_RC_STICK_OV](../advanced_config/parameter_reference.md#COM_RC_STICK_OV) | The amount of stick movement that causes a transition to [Position mode](../flight_modes_mc/position.md) (if [COM_RC_OVERRIDE](#COM_RC_OVERRIDE) is enabled). | +| [RTL_LOITER_RAD](../advanced_config/parameter_reference.md#RTL_LOITER_RAD) | [Fixed-wing Only] The radius of the loiter circle (at [RTL_LAND_DELAY](#RTL_LAND_DELAY)). | +| [MIS_TKO_LAND_REQ](../advanced_config/parameter_reference.md#MIS_TKO_LAND_REQ) | Specify whether a mission landing or takeoff pattern is _required_. Generally fixed-wing vehicles set this to require a landing pattern but VTOL do not. | +| [RTL_TIME_FACTOR](../advanced_config/parameter_reference.md#RTL_TIME_FACTOR) | RTL time estimate safety factor. | +| [RTL_TIME_MARGIN ](../advanced_config/parameter_reference.RTL_TIME_MARGIN) | RTL time estimate safety margin. | From a9d3e35be3812ca42ea56d3dc4ce6bf434fe7bb3 Mon Sep 17 00:00:00 2001 From: Anil Kircaliali Date: Sun, 26 Apr 2026 19:24:24 -0700 Subject: [PATCH 09/10] Add descriptive comment --- src/modules/navigator/rtl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 50739aaaa4ae..aee5d0e6d98f 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -369,7 +369,7 @@ void RTL::setRtlTypeAndDestination() } } else if (_param_rtl_type.get() == 6) { - // estimate time to fly home using the same infrastructure as the battery failsafe + // 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); From 43f417d49d126c729b82c478f9626913f2673944 Mon Sep 17 00:00:00 2001 From: Anil Kircaliali Date: Sun, 26 Apr 2026 19:27:12 -0700 Subject: [PATCH 10/10] Update docs/en/flight_modes/return.md Co-authored-by: Silvan Fuhrer --- docs/en/flight_modes/return.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/en/flight_modes/return.md b/docs/en/flight_modes/return.md index 003ea7e54570..05a893564469 100644 --- a/docs/en/flight_modes/return.md +++ b/docs/en/flight_modes/return.md @@ -179,7 +179,7 @@ In this return type, the vehicle ignores home and mission landing patterns entir - Lands or waits at the rally point destination. ::: info -If no rally points are defined the vehicle will land at its current position instead of returning to home. +If no rally points are defined the vehicle will land at its current position instead of returning to home. There is a pre-flight check that prevents taking off without any rally point if this RTL_TYPE is selected. This type is intended for use cases where only pre-approved rally points are acceptable landing destinations. :::