feat(navigator): smart mission rejoin and new RTL mode to follow the mission to the closest safe point.

[JonasPerolini]

This PR is still a work in progress but I'm creating it to gather input if anyone is interested in the feature. The git diff is quite large so the plan would be to create separate smaller PRs to achieve the overall feature step by step. (Note that a lot of lines were added for unit tests and documentation).

---

Problem to solve:

Minimize deviations from the planned route during contingencies. Applies to both mission joining after a deviation and return mode.

1. With the GoTo feature, the vehicles' current position can be arbitrarily far from the last mission flown segment. When resuming a mission, flying to the last flown segment in a straight line can lead to large deviations from the mission path. In A to B missions e.g. for delivery, there is no need to fly back to the last flown segment, the goal is to get to B. Deviations can be caused for example by a detect and avoid maneouvre or can be requested by air traffic controllers when operating in shared airspaces.

2. RTL modes do not follow the mission path when navigating towards a safe point which can lead to dangerous deviations and constrains mission planning to ensure no geofence is breached during a return mode.

---

Solution: described in detail in docs/en/flight_modes/route_safe_point_return.md, ## How It Works

Smart mission join: Projects the vehicle position onto every segment of the route and chooses the "best" branch-in point.

1. find the branch-in point: WORK_ITEM_TYPE_JOIN_ROUTE (see decision process below)
2. Once branch-in reached, perform VTOL transitions if required. For FT: align with
   • Current target if far
   • Next target if within the acc rad of the current target

Finding the branch-in point:

• Phase 1 - Identifying valid candidates:
  • The system first identifies up to three potential projection points on the flight route by calculating perpendicular lines from the vehicle's current position to all route segments. A projection point is only considered a valid candidate if the crosstrack distance from the vehicle to the segment does not exceed the crosstrack distance to the closest available segment plus an allowed margin. This margin is determined by the vehicle's current flight mode:
    • Multicopter: MIS_MC_SEG_DIST by default, 30 m.
    • Fixed-wing: MIS_FW_SEG_DIST by default, 150 m.
• Phase 2 - Selecting the best branch-in point:
  • From the potential candidates, the system selects the best branch-in point using a priority-based system
    • Priority 1: if one of the valid projection points falls on the route segment the vehicle is currently expected to be flying (last targeted waypoint index), that point is immediately selected.
    • Priority 2: if no point satisfies Priority 1, the system calculates the combined distance score (A + B) for the remaining candidates and selects the point with the lowest score for the most efficient path:
      • A (crosstrack distance): the distance from the vehicle to the projection point on the route.
      • B (distance to last waypoint): the distance from the projection point to either the start or the end of the last flown segment (whichever is closer).
• Phase 3 - Determining the branch-in altitude:
  • Once the lateral branch-in point is established, the system calculates the target altitude for that point using the following logic:
    • Linear interpolation: by default, the altitude is calculated via linear interpolation between the altitudes of the segment's start and end waypoints (e.g., if rejoining on segment 2-3, the system interpolates between the altitudes of waypoint 2 and waypoint 3).
    • Special case (Land): if the branch-in point falls on a land segment, the system defaults to the altitude of the previous waypoint.
    • Special case (short segments): if the segment is too short for reliable interpolation (such as in the case of superimposed waypoints), the system uses the altitude of the segment's end waypoint.

Route Safe Point Return is a new RTL mode that uses the uploaded mission as the return path towards the closest safe point. First, the vehicle and every uploaded safe point are projected onto the mission route, and the closest safe point along the route is chosen. Once the planning phase is done, the vehicle follows the route in nominal or reverse direction, reaches the branch-off point defined as the projection of the selected safe-point on the route and only then branches off horizontally from the flight plan and proceeds to land at the safe point location:

1. Find the location of the vehicle on the route (using smart mission join)
2. Find the location of the safe points using the same projection algorithm as the vehicle
3. Select the closest safe point based on total distance to the safe point (along route + distance after branching off from the route)
   • If no safe points, fallback to takeoff or land, which ever is closest.
4. Navigate to the goal:
   4.1: join the route (smart mission join)
   4.2: follow the route till branch-off
   4.3: branch off towards safe point
   4.4: land at safe point.

---

PoC video:

Poc.mp4

• First, observe how given the deviation on mission start, despite being closest to segment 10-11, segment 2-3 is chosen because it is a valid candidate (within MIS_MC_SEG_DIST of the closest xtrack segement) and it is closer to the last flown segment
• Second, observe the new RTL_TYPE 6 behavior
  • The vehicle follows the mission path, branches-off to go land at the safe point.
  • The PIC triggers a switch to Mission, the vehicle starts going back to the branch-in
  • The PIC triggers a return once again, since we're close to the branch-off - safe point segment, the vehicle goes straight to the safe point

---

Main challenge: memory managment. The algorithms to find the optimal mission return and safe point require to project the vehicle and the safe points onto every route segment. The current solution is to have a new module: mission_route_cache which keeps the mission in a large cache defined by CONFIG_RTL_MISSION_CACHE_SIZE. The new features would only apply to missions smaller or equal to this cache size. The idea is that we can't have blocking SD card reads for long missions.

---

Legacy behavior:

• mission: the changes are only enabled if the param MIS_ROUTE_JOIN is set to true.
• RTL: new mode, does not affect the other modes.
• Selecting land approaches:
  • Current code caps collected approaches at land_approaches_s::num_approaches_max (8) in mission_route_planner.cpp main did not bounds-check and would keep incrementing
  • Current code validates the rally point before returning index-based approaches in mission_route_planner.cpp and main RTL::readVtolLandApproaches(...) in rtl.cpp matched by lat/lon and did not validate the rally frame.
  • Current chooseBestLandingApproach(...) uses the rally land location stored in land_location_lat_lon as the bearing origin in rtl.cpp, main used home position as the origin
• Fix goToNextPositionItem(bool execute_jump) such that it properly skips loop jumps when required
• mission_base: Change dataman _dataman_cache.loadWait to use loadMissionItemFromCache

[anilkir]

#26968 also introduces a new RTL_TYPE=6. FYI in case deconflicting is needed.

[Copilot]

Pull request overview

This PR introduces route-based planning to (1) rejoin missions “smartly” after deviations and (2) add a new RTL mode that follows the uploaded mission route to the closest usable safe point before branching off to land, backed by a new mission/safe-point caching layer to avoid blocking dataman reads.

Changes:

• Add MissionRoutePlanner projection/selection logic plus MissionRouteCache to preload mission + safe points for route-based planning.
• Add RTL_TYPE=6 (“Route Safe Point Return”) and integrate route-join logic into Mission activation/resume, including VTOL transition handling.
• Add extensive navigator/unit + integration tests and update parameters, uORB message docs, and user documentation.

Reviewed changes

Copilot reviewed 49 out of 49 changed files in this pull request and generated 6 comments.

Show a summary per file

File	Description
src/modules/vtol_att_control/vtol_att_control_main.h	Adds helper to gate MC→FW transitions by nav state (now allowing AUTO_RTL).
src/modules/vtol_att_control/vtol_att_control_main.cpp	Uses new nav-state gating for DO_VTOL_TRANSITION acceptance/rejection.
src/modules/navigator/test/test_tools/AddAndVisualizeUnitTests.md	Documents a Streamlit tool for creating/visualizing navigator test datasets.
src/modules/navigator/test/test_RTL_projection.cpp	New unit tests for vehicle/route projection behavior and edge cases.
src/modules/navigator/test/test_RTL_helpers.h	New shared test helpers (providers, factories, runtime setup, tolerances).
src/modules/navigator/test/test_RTL_data.h	New reusable mission/safe-point datasets for planner testing.
src/modules/navigator/test/test_navigator_route_rejoin_and_rtl.cpp	New end-to-end navigator tests for mission rejoin and RTL type 6 behavior.
src/modules/navigator/test/test_mission_route_planner_candidates.cpp	New unit tests for candidate buffer insertion/pruning and helper logic.
src/modules/navigator/test/test_mission_route_cache.cpp	New tests validating mission/safe-point cache loading and retry behavior.
src/modules/navigator/test/CMakeLists.txt	Adds navigator functional gtests and links them to appropriate libraries.
src/modules/navigator/rtl.h	Adds RTL type 6 executor plumbing, new params, and route-safe-point plan state.
src/modules/navigator/rtl_params.yaml	Documents RTL_TYPE=6 and adds RTL_RP_SEG_DIST / RTL_FW_UTURN_PEN params.
src/modules/navigator/rtl_mission_safe_point_follow.h	Declares new RTL executor that follows mission route then branches to goal.
src/modules/navigator/rtl_mission_fast.cpp	Updates traversal API usage for next position item selection.
src/modules/navigator/rtl_mission_fast_reverse.cpp	Updates traversal API usage (including geometry-only traversal option).
src/modules/navigator/rtl_direct_mission_land.cpp	Updates traversal API usage for next position item selection.
src/modules/navigator/rtl_base.h	Adds route-safe-point configuration handoff and loop-anchor accessor.
src/modules/navigator/navigator.h	Adds MissionRouteCache as a Navigator-owned service for Mission/RTL.
src/modules/navigator/navigator_main.cpp	Removes legacy safe-points update trigger (now handled via route cache).
src/modules/navigator/mission.h	Adds mission-route-cache syncing, active update hook, and rejoin parameters.
src/modules/navigator/mission.cpp	Implements route-cache syncing and smart route-join planning on activation.
src/modules/navigator/mission_route_planner.h	Adds new planner API/types for projection, selection, and full plan output.
src/modules/navigator/mission_route_cache.h	Declares new dataman-backed cache provider for planner inputs.
src/modules/navigator/mission_route_cache.cpp	Implements mission/safe-point cache loading, validation, and retry state machines.
src/modules/navigator/mission_params.yaml	Adds MIS_ROUTE_JOIN + projection-margin parameters for mission/RTL planning.
src/modules/navigator/mission_block.h	Inlines item_contains_position() helper (used by new planner default logic).
src/modules/navigator/mission_block.cpp	Removes out-of-line item_contains_position() implementation.
src/modules/navigator/Kconfig	Adds CONFIG_RTL_MISSION_CACHE_SIZE Kconfig option for mission cache sizing.
src/modules/navigator/CMakeLists.txt	Adds planner library, test subdir, and new sources/executor to navigator module.
src/lib/dataman_client/DatamanClient.hpp	Adds API to clear stale async responses + update cached items in-place.
src/lib/dataman_client/DatamanClient.cpp	Implements stale-response clearing, zero-size cache behavior, and cache patching.
msg/RtlStatus.msg	Extends RTL status type enum and clarifies field semantics for new mode.
docs/en/msg_docs/RtlStatus.md	Updates generated docs to match RtlStatus message changes.
docs/en/flying/plan_safety_points.md	Updates guidance to include RTL_TYPE=6 route-based safe point return.
docs/en/flight_modes/return.md	Documents RTL_TYPE=4/5/6, including route safe point return behavior.
docs/en/flight_modes_mc/mission.md	Documents mission resume behavior with smart branch-in + params.
docs/en/flight_modes_fw/mission.md	Documents mission resume behavior with smart branch-in + params.
boards/px4/sitl/default.px4board	Enables CONFIG_RTL_MISSION_CACHE_SIZE for SITL default build.

[Copilot]

AUTO_RTL is now allowed for MC→FW transitions, which also implicitly allows external VEHICLE_CMD_DO_VTOL_TRANSITION commands during RTL (since the check doesn’t gate on vehicle_command.from_external). If the intent is only to allow RTL executors, consider keeping AUTO_RTL blocked for external commands (e.g. reject when from_external is true) while still permitting internal requests.

[JonasPerolini]

I would not block external FT commands in AUTO_RTL mode because in the new RTL_TYPE=6, the vehicle follows the route and performs the planned transitions.

[mrpollo]

Thanks for the writeup and the tests, Jonas. I went through this end-to-end. A few things to resolve before this can land.

Please split this PR. You already mention you intend to. The umbrella is doing five distinct things and will be hard to bisect if any of them regress. Suggestion:

1. goToNextPositionItem jump-flag bugfix + findNextPositionIndex consolidation. Standalone, mergeable now, deserves its own release-notes line because it changes rtl_mission_fast_reverse behavior (it now actually skips DO_JUMP loops where the flag was previously ignored).
2. MissionRouteCache + MissionRoutePlanner library + tests. Pure infra, no behavior change.
3. Smart rejoin (MIS_ROUTE_JOIN) on top of (2).
4. Route Safe Point Return RTL on top of (2).
5. The VTOL transition guard change. Standalone, with its own justification (see inline comment).

RTL_TYPE=6 collides with #26968. @anilkir already flagged this. #26968 is small, scoped, and has review traction. Suggestion: #26968 takes slot 6, this PR moves to a different value. Please coordinate before either lands so we don't churn the param enum twice.

The "no blocking SD card reads" claim in the body is misleading. MissionRouteCache::loadMissionItem calls loadWait(..., MAX_DATAMAN_LOAD_WAIT) with a 500ms timeout. This matches what MissionBase does on main today, so it's not a regression, but the body sells it as eliminated and a worst-case cache miss can stall the navigator loop. Please update the body, or document the steady-state vs. worst-case behavior honestly.

Hidden VTOL safety regression. The new frontTransitionAllowedInNavState returns true for every NAVIGATION_STATE_AUTO_RTL, not just the new route-following type. That widens a guard for users on RTL_TYPE 0/3/5 who never opted into this feature. Must be gated on the actual RTL executor (or the param value), or pulled into its own PR with its own review and release-notes entry. Inline below.

Cache is SITL-only. CONFIG_RTL_MISSION_CACHE_SIZE=300 is enabled only in boards/px4/sitl/default.px4board. No FMU board picks it up, so the feature is currently unshippable on real hardware. Before this lands we need a per-board enablement plan and an FMUv5/v6 heap budget. 300 items at ~76 B is ~22.8 KB, plus dynamic new[] from the navigator main loop on every mission upload (see inline on DatamanClient.cpp).

Other things flagged inline: geofence-not-enforced warning missing from the docs, clearPendingResponse only drains a single stale message instead of looping, DatamanCache::resize allocates from the navigator hot path, RTL_FW_UTURN_PEN=4000m magic default with no rationale, MAX_SAFE_POINT_BATCH=64 silently truncates without a mavlink warning, "does not not exceed" typo x3 across mission_params.yaml and rtl_params.yaml, the Streamlit dev tool sitting under src/modules/navigator/test/test_tools/, and a couple of px4_usleep polling loops in the new tests that will flake on slower CI runners.

Tests: placement is right, framework is right, they wire into the existing tests matrix correctly. Two notes: (a) the 1998-line mission_tools.py Streamlit visualizer doesn't belong under src/modules/..., that's Tools/. (b) please don't add streamlit/folium/pydeck to Tools/setup/requirements.txt; keep them as opt-in for whoever uses the visualizer.

[mrpollo]

BLOCKER. This unconditionally allows MC to FW transitions for every RTL_TYPE, not just the new route-following one. The previous behavior denied transitions in AUTO_RTL outright. Users on RTL_TYPE 0/3/5, who never opted into this PR's feature, now get a relaxed safety guard with no opt-in.

Two options: (a) gate this on the active RTL executor (e.g. expose _rtl_type from navigator and only allow when it equals route-safe-point-follow), or (b) pull this into its own PR with its own justification, review, and release-notes call-out. As-is, this is a hidden behavioral change that doesn't belong in a smart-rejoin PR.

[JonasPerolini]

Thanks for the feedback @mrpollo

My first intention was to use _rtl_type in the gate but the main issue is that the new RTL_TYPE 6 can fallback to other RTL modes (due to errors or the mission being too large). This would lead to different RTL behaviours depending on whether the mode was selected or if we defaulted to the mode.

My preferred approach is to handle the new RTL_TYPE 6 in two steps:

1. PR 1: create the RTL_TYPE 6 without transitions
2. PR 2: include transitions in the RTL_TYPE 6 + add the changes in vtol_att_control_main.h:
   • allow transitions for all RTL modes, and in the specific RTL mode module, skip transitions when encountered. With that implementation, the only remaining behaviour change is that external FT would now be accepted in all modes.

[mrpollo]

BLOCKER. This contradicts the "no blocking SD card reads for long missions" claim in the PR body. On a cache miss this stalls the navigator loop for up to 500ms (see MAX_DATAMAN_LOAD_WAIT in mission_route_cache.h:22), the same as the existing path in MissionBase::loadMissionItemFromCache. You haven't eliminated blocking, you've reduced typical-case latency by pre-warming.

Either:

1. Drop the timeout to 0 here and treat misses as a planning failure that falls back to direct RTL (the fallback is already wired in rtl.cpp:380-387, this would just exercise it more often), or
2. Update the PR body to honestly describe steady-state vs. worst-case behavior.

Preference is (1). A 500ms navigator stall during an RTL is the kind of thing that bites once a year and is awful to debug. Same applies to loadSafePointItem and getMissionLandItem below.

[JonasPerolini]

Set to 0ms here: debcae7

[mrpollo]

BLOCKER for shipping. Currently this option is only enabled in boards/px4/sitl/default.px4board. No FMU board defines it, so RTL_TYPE=6 effectively can't run on real hardware. Two things:

1. Add a per-board enablement plan. FMUv5 has tight RAM. 300 x ~76B is ~22.8 KB heap, plus the dynamic new[] on mission upload (see DatamanClient.cpp comment). Suggest starting with default=100 for FMUv5 and letting v6x/v6c go higher.
2. Pre-allocate at construction sized to CONFIG_RTL_MISSION_CACHE_SIZE, instead of calling _dataman_cache_mission.resize(mission.count) on every mission upload from the navigator main loop. Allocations from the hot path are a DoS surface (spam mission uploads, repeated new[]/delete[]).

[JonasPerolini]

Pre-allocated here: debcae7

Added to new boards here: 79aace8

RTL fallback if config not set here 1340cec and default Kconfig to 0 (disabled) here 1340cec

[mrpollo]

This drains only the most recent stale response. If more than one is queued (which can happen after abortCurrentOperation or a previously timed-out async op), the rest survive and will be picked up by the next request as if they were its response, which is exactly the bug this is presumably trying to prevent. Loop until updated == false.

Also worth a one-line comment explaining why this is called from every readAsync/writeAsync/clearAsync. It's a behavior change for every existing consumer of DatamanClient (geofence, safe_points, mission_base) and reviewers will want to understand why.

[JonasPerolini]

Cleared all pending responses here: 60d3dbc

[mrpollo]

Heap allocation from the navigator main loop on every mission upload. For 300 items at ~76 B each that's ~22.8 KB allocated/freed per upload. On NuttX with a small heap and active flight, this is a fragmentation hazard, and a malicious GCS spamming mission uploads is a DoS surface. Pre-allocate to CONFIG_RTL_MISSION_CACHE_SIZE once when MissionRouteCache is constructed and never resize after that.

[JonasPerolini]

Pre-allocated here debcae7, no more resizes

[mrpollo]

Typo: "does not not exceed" should be "does not exceed". Same typo on line 147 (MIS_FW_SEG_DIST) and in rtl_params.yaml:131 (RTL_RP_SEG_DIST).

[mrpollo]

BLOCKER for merge hygiene. 1998-line Streamlit/folium/pydeck visualizer in the wrong place. PX4 dev tooling lives under Tools/, not under src/modules/.../test/test_tools/. Please move to Tools/navigator_mission_visualizer/ (or similar). And please don't add streamlit/folium/pydeck to Tools/setup/requirements.txt; keep them as opt-in deps documented in the tool's own README. We don't want every PX4 builder pulling Streamlit's transitive deps.

Same applies to AddAndVisualizeUnitTests.md in this directory.

[mrpollo]

Wall-clock px4_usleep polling in unit tests will flake on shared CI runners under load. Either expose a deterministic flush/notify hook on MissionRouteCache that the test can call, or use a bounded retry that fails loudly with a clear message on timeout. Same pattern at line 203 and in test_navigator_route_rejoin_and_rtl.cpp:307.

[JonasPerolini]

removed usleep from unit tests here 76c830d

[mrpollo]

A few things on the doc:

1. Geofence is not enforced when following the route. Confirmed by reading rtl_mission_safe_point_follow.cpp, there's no geofence check before issuing route waypoints. Doc admits "geofence pruning not yet implemented" but that line is buried. This needs a prominent warning block at the top of the page: "RTL_TYPE=6 follows the uploaded mission route. PX4 does not currently verify that the route stays inside an active geofence. Plan accordingly."
2. The "Developer Deep Dive" section (state machine, file table, internal class diagram, virtual waypoints) is half the doc and doesn't belong in flight_modes/. Move to source comments or a dev/ page. Users will bounce.
3. No diagrams. The whole feature is geometric (project vehicle, route, branch-off, land). Two SVGs would do more for comprehension than 200 lines of prose. The PoC video is good but a video isn't a doc.
4. The 64-rally-point limit and the cache-too-large fallback should be promoted into the user-facing "Limitations" section, not buried in the developer notes.
5. The RTL_TYPE table in return.md is now a wall of text. A short decision matrix ("Have rally points? Mission already cleared for terrain? Geofence hugs the mission?") would help operators pick.

[mrpollo]

This is a real bugfix. On main, the bool execute_jump flag was silently ignored because the function went through getNextPositionItems. The new typed PositionTraversalType enum routes through findNextPositionIndex which actually honors it. The behavioral consequence is that rtl_mission_fast_reverse.cpp:90 (now passing IgnoreDoJump) will start skipping DO_JUMP loops in reverse RTL where it previously executed them.

This is the right fix, but it changes flight behavior for any user with DO_JUMP items in a fast-reverse RTL. Please pull it into its own bugfix PR (PR (1) in the split plan above) so it gets a proper release-notes entry and isn't buried in a 16k-line feature PR.

[JonasPerolini]

opened #26993

[mrpollo]

Closing in favor of splitting this up