feat(navigator): Add infrastructure for smart mission route planning by JonasPerolini · Pull Request #27687 · PX4/PX4-Autopilot

JonasPerolini · 2026-06-17T21:00:02Z

Infrastructure part of this PR: #26973 (FYI @mrpollo). No changes in behavior. The infrastructure provides the interface and logic needed by features that:

Need quick, non-blocking access to the uploaded mission items.
Need to project the vehicle and safe-point positions onto the mission path.

The overall goal is to minimize how far the vehicle deviates from the planned route during contingencies. For example:

Smart mission join: after a GoTo or a manual reposition, project the vehicle onto every route segment and rejoin the mission at the best branch-in point.
Route-following Return: a Return mode that follows the mission route and only branches off once it is close to the selected safe point, instead of cutting straight across terrain.

The planner computes geometry and scoring only. It does not publish setpoints, change the mission index, or decide when a flight mode should use the result. This will be implemented in follow up PRs.

Motivation

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.
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.

Architecture

mission_route_planner.*           runs one planning pass, returns the result (start here)
 |-- mission_route_projection.*   projects the vehicle and safe points onto the route
 `-- mission_route_goal.*         scores safe points, picks the goal and route direction

mission_route_provider.*          interface for reading mission and safe-point data
mission_route_cache.*             caches the mission in RAM for non-blocking access
mission_route_types.*             shared structs, constants, and parsing helpers used by all

Route cache:

Planner/provider reads use a zero wait timeout. A cache miss therefore returns failure instead of blocking Navigator while dataman or the SD card catches up.

The cache tracks mission identity, dataman storage id, mission count, and readiness. If the mission changes, the full-route cache is reloaded. If validation finds that an item is missing, the cache schedules a retry using bounded exponential backoff.

Safe points have their own asynchronous state machine because the safe-point set is tracked through DM_KEY_SAFE_POINTS_STATE. The cache reloads safe points when the safe-point source identity changes and prevents stale safe-point data from being exposed while a reload is in progress.

CONFIG_RTL_MISSION_CACHE_SIZE controls the maximum number of mission items reserved for full-route planning helpers.

The default is:

100 for BOARD_TESTING builds.
0 for normal builds unless a board opts in. Setting the value to 0 disables the full mission-route cache and reserves no mission-route entries.

MissionRouteCache
|-- full mission route cache      [0 ... CONFIG_RTL_MISSION_CACHE_SIZE - 1]
|-- safe-point cache              [all uploaded safe-point dataman items]
|-- mission-land item cache       [published mission land index]
`-- safe-point stats reader       [DM_KEY_SAFE_POINTS_STATE async state machine]

Point Projection

This is the shared projection step used by both the Vehicle Projection and the Safe-Point Scoring below. The projected point is the vehicle in one case and a safe point (rally point) in the other.

The planner draws a perpendicular projection from the point onto every route segment and keeps up to three candidates.
A projection is a valid candidate only if its crosstrack distance is within a search margin of the closest candidate's crosstrack distance. The consuming feature supplies that margin.

The example below uses a large margin. The mission route is drawn in white and the rally-point (R) projections in green. Every rally point has three projections because the margin is large.

mission_route_planner_large_search_margin

Here is the same route with a smaller margin. Some projections are dropped because their crosstrack distance is greater than the closest projection's crosstrack distance plus the margin.

mission_route_planner_small_search_margin

With a margin of zero, each rally point keeps only its single closest projection.

Vehicle Projection

When a feature needs to rejoin the route, the first step is to project the current vehicle position onto the mission path and choose a "branch-in" point. The vehicle may have left the route (for example with a GoTo), so the planner picks the point that best preserves mission continuity.

The branch-in point is chosen in three phases:

Phase 1: Identifying valid candidates:

The vehicle is projected onto the route as described in the Point Projection paragraph above.

Phase 2: Selecting the best branch-in point:

The best candidate is chosen with a priority system:

Priority 1: a candidate is selected immediately if it lies on the segment the vehicle is currently expected to be flying (the last targeted waypoint index, or the last flown DO_JUMP segment if the caller supplied it).
Priority 2: if no candidate matches Priority 1, the planner scores each candidate by A + B and keeps the lowest:
- A (crosstrack distance): from the vehicle to the projection on the route.
- B (distance to last flown segment): from the projection to whichever end of the last-flown segment is closer.

Example 1:

The vehicle is flying a mission, where the outbound and inbound legs are close to one another at the start.
While on the outbound leg, the vehicles deviates east to a GoTo location (e.g. due to incoming traffic).
That GoTo location is closer to the inbound leg, but both legs are close enough that the projection step returns two valid candidates: one on the outbound leg and one on the inbound leg.
The outbound segment is the one the vehicle is currently expected to fly, so it wins immediately on Priority 1.
Even without that rule it would still win on Priority 2, because its distance to the last-flown segment is much smaller.
The vehicle rejoins the mission and continues toward B.

Example 2:

The vehicle is flying north and deviates to a GoTo location that has two valid branch-in candidates.
Neither projection lies on the current segment, so each candidate is scored by A + B: crosstrack distance plus the along-route distance to the last-flown segment. Take the candidate projected onto the east segment. Its score is the crosstrack distance (166.95 m) plus the distance along the route back to the last-flown segment (the red lines). The closer end of the last-flown segment is used, and the along-route distance is the sum of the straight lines between waypoints. The other candidate, projected onto the south segment, wins because its total distance is smaller.

mission_route_planner_segment_selection_2

Phase 3: Determining the branch-in altitude:

Linear interpolation: by default the altitude is interpolated between the start and end waypoint altitudes of the segment (for example, rejoining on segment 2-3 interpolates between waypoint 2 and waypoint 3).
Special case (land): if the branch-in point falls on a land segment, the previous waypoint altitude is used.
Special case (short segments): if the segment is too short to interpolate reliably (such as stacked waypoints), the segment end waypoint altitude is used.

Safe-Point Scoring

For a route-following return, the vehicle follows the mission route until it reaches a "branch-off" point and then flies straight to a safe point. The planner first finds the candidate branch-off points and then selects the safe point with the lowest total return cost.

This runs in two phases:

Phase 1: Identifying valid candidates:

The vehicle and all safe points are projected in a single scan of the route, using the same Point Projection described above

Phase 2: Selecting the best projection point:

Each candidate branch-off is scored by total path cost, and the lowest wins. The cost is built from:

Along-route distance: along the route geometry from the vehicle projection to the safe-point projection (branch-off point), using straight lines between waypoints.
Branch-off leg: the straight-line distance from the safe-point projection to the safe point — the off-route leg flown after leaving the route.
U-turn penalty: for fixed-wing and VTOL-in-FW, an extra distance penalty is added when the path would require an immediate U-turn, so forward-flowing paths are preferred. The caller supplies the penalty value (set it to 0 to disable).

Safe points are filtered while the planner builds its batch:

Safe points are read from the dataman store through the provider.
Invalid coordinates, unsupported frames, or filtered safe points are skipped.
Every remaining safe point gets up to three projections.

Route-Skip Shortcuts

After the best safe point has been chosen, a route-to-goal caller can skip the route join/follow entirely:

Direct-to-safe-point: if the vehicle is already within the direct acceptance radius of the selected safe point, go straight to it.
Close-to-branch-leg: if the vehicle is already close to the selected branch-off leg, continue straight toward the goal.

These shortcuts are applied only after selection, so they never change which safe point wins the cost comparison.

Example: In the image below the vehicle is already on the branch-off leg, after a GoTo or a canceled Return.
If a new Return is requested, the vehicle flies straight to the rally point (R) instead of flying back to the branch-in point only to branch off again.

mission_route_planning_close_to_branch_off

PoC video, behaviors not yet available in this PR:

Note that the features from the PoC are not implemented in this PR but the infrastructure from this PR allows to enable them in a future PR.

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 the threshold of the closest xtrack segement) and it is closer to the last flown segment
Second, observe the new RTL_TYPE X 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

hamishwillee · 2026-06-18T00:29:45Z

@JonasPerolini This looks pretty cool. Still getting my head around it. YOu might want to (additionally) look at https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_RETURN_PATH_START which I don't think PX4 supports. This is used on ArduPilot to provide what is effectively a rally point on the return path that is known safe to move towards if you're already on the outbound path of a mission.

It might not be useful, but it strikes me that it might provide a useful hint for prioritising some kinds of returns. In particular this is intended for safer return when flying a corridor - where you can't just assume any path to the mission is viable.

hamishwillee · 2026-06-18T00:45:28Z

+The mission route is drawn in white and the rally-point (`R`) projections in green.
+Every rally point has three projections because the margin is large.
+
+![Point projection with a large search margin](../../assets/mission_route_planner/mission_route_planner_large_search_margin.png)


Why is one of the R drawn differently?

It would be good to make one of these a vehicle.

The red R joins a waypoint, not a segment perpendicularly. That isn't mentioned.

Added some details in a ::: details box

updated the image to have a plane instead of the red "R" (it was red because selected). They probably need to be resized down once again, could you please do it @hamishwillee?

hamishwillee · 2026-06-18T00:46:17Z

+The mission route is drawn in white and the rally-point (`R`) projections in green.
+Every rally point has three projections because the margin is large.
+
+![Point projection with a large search margin](../../assets/mission_route_planner/mission_route_planner_large_search_margin.png)


Can/ should we draw the margins on all these images?

I don't think there is much added value. We would need to also add all the xtrack distances as well

github-actions

(1/2)

github-actions

(2/2)

github-actions

(1/2)

github-actions

(2/2)

github-actions

(1/2)

github-actions

(2/2)

github-actions · 2026-06-18T09:17:46Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 4320 byte (0.21 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.2% +3.78Ki  +0.2% +3.78Ki    .text
    +0.8% +1.31Ki  +0.8% +1.31Ki    [section .text]
   -90.8%    +962 -90.8%    +962    [53 Others]
    [NEW]    +488  [NEW]    +488    MissionRouteCache::updateSafePointCache()
    [NEW]    +360  [NEW]    +360    MissionRouteCache::updateMissionCache()
    [NEW]    +236  [NEW]    +236    mission_route::Provider::scanVtolLandApproachBlock()
    [NEW]    +196  [NEW]    +196    mission_route::Provider::findAssociatedSafePointIndex()
    [NEW]    +172  [NEW]    +172    MissionRouteCache::updateMissionLandItemCache()
    [NEW]    +164  [NEW]    +164    mission_route::Position::valid()
    +8.1%    +156  +8.1%    +156    Navigator::Navigator()
    [NEW]    +136  [NEW]    +136    mission_route::Provider::anySafePointHasVtolLandApproach()
    [NEW]    +136  [NEW]    +136    mission_route::Provider::getMissionTakeoffItem()
    [NEW]    +134  [NEW]    +134    mission_route::Provider::getVtolLandApproachesNearLocation()
    [NEW]    +128  [NEW]    +128    MissionRouteCache::getMissionLandItem()
    [NEW]    +128  [NEW]    +128    mission_route::extractSafePointPosition()
    [NEW]    +108  [NEW]    +108    MissionRoutePlanner::MissionRoutePlanner()
    [NEW]    +100  [NEW]    +100    MissionRouteCache::missionCacheFullyLoaded()
    [DEL]    -140  [DEL]    -140    RTL::getVtolLandApproachesNearLocation()
    [DEL]    -192  [DEL]    -192    RTL::scanVtolLandApproachBlock()
    [DEL]    -224  [DEL]    -224    RTL::findAssociatedSafePointIndex()
    [DEL]    -248  [DEL]    -248    RTL::extractValidSafePointPosition()
   -94.4%    -268 -94.4%    -268    RTL::updateDatamanCache()
  [ = ]       0  +0.5%    +448    .bss
    [ = ]       0  [NEW]    +400    projectionReferenceBatch()::storage
    [ = ]       0  [NEW]     +44    [section .bss]
    [ = ]       0  [NEW]     +12    projectionReferenceBatch()::batch
    [ = ]       0 -50.0%      -4    g_dma_perf
    [ = ]       0 -50.0%      -4    mavlink_log_pub
  +0.4% +8.14Ki  [ = ]       0    .debug_abbrev
  +0.4%    +608  [ = ]       0    .debug_aranges
  +0.4% +1.85Ki  [ = ]       0    .debug_frame
  +0.3% +94.5Ki  [ = ]       0    .debug_info
  +0.3% +13.6Ki  [ = ]       0    .debug_line
     +50%      +1  [ = ]       0    [Unmapped]
    +0.3% +13.6Ki  [ = ]       0    [section .debug_line]
  +0.2% +7.47Ki  [ = ]       0    .debug_loclists
  +0.2% +1.26Ki  [ = ]       0    .debug_rnglists
    [NEW]      +1  [ = ]       0    [Unmapped]
    +0.2% +1.26Ki  [ = ]       0    [section .debug_rnglists]
  +0.4% +13.7Ki  [ = ]       0    .debug_str
  +0.8%      +2  [ = ]       0    .shstrtab
  +0.3% +1.90Ki  [ = ]       0    .strtab
    [NEW]     +44  [ = ]       0    DatamanClient::abortCurrentOperation()
    [NEW]     +24  [ = ]       0    MissionRouteCache
    [NEW]     +54  [ = ]       0    MissionRouteCache::RetryBackoff::scheduleRetry()
    [NEW]     +64  [ = ]       0    MissionRouteCache::getMissionLandItem()
    [NEW]     +60  [ = ]       0    MissionRouteCache::loadMissionItem()
    [NEW]     +62  [ = ]       0    MissionRouteCache::loadSafePointItem()
    [NEW]     +62  [ = ]       0    MissionRouteCache::missionCacheFullyLoaded()
    [NEW]     +40  [ = ]       0    MissionRouteCache::missionCount()
    [NEW]     +58  [ = ]       0    MissionRouteCache::missionMatchesCache()
    [NEW]     +48  [ = ]       0    MissionRouteCache::publishSafePointCache()
    [NEW]     +60  [ = ]       0    MissionRouteCache::queueMissionCacheLoads()
    [NEW]     +47  [ = ]       0    MissionRouteCache::queueMissionLandItem()
    [NEW]     +51  [ = ]       0    MissionRouteCache::resetSafePointCacheState()
    [NEW]     +53  [ = ]       0    MissionRouteCache::safePointCacheFullyLoaded()
    [NEW]     +58  [ = ]       0    MissionRouteCache::safePointCacheMatchesReadStats()
    [NEW]     +42  [ = ]       0    MissionRouteCache::safePointCount()
    [NEW]     +43  [ = ]       0    MissionRouteCache::update()
    [NEW]     +56  [ = ]       0    MissionRouteCache::updateMissionCache()
    [NEW]     +64  [ = ]       0    MissionRouteCache::updateMissionLandItemCache()
    [NEW]     +58  [ = ]       0    MissionRouteCache::updateSafePointCache()
   -96.6%    +894  [ = ]       0    [29 Others]
  +0.2% +1.30Ki  [ = ]       0    .symtab
   -50.0%     -16  [ = ]       0    DatamanCache::changeUpdateIndex()
     +50%     +16  [ = ]       0    DatamanCache::invalidate()
    [NEW]     +16  [ = ]       0    DatamanClient::abortCurrentOperation()
     +50%     +16  [ = ]       0    DatamanClient::lastOperationCompleted()
   -50.0%     -16  [ = ]       0    MissionBase::on_inactivation()
     +50%     +16  [ = ]       0    MissionBase::updateDatamanCache()
    [NEW]     +32  [ = ]       0    MissionRouteCache
    [NEW]     +48  [ = ]       0    MissionRouteCache::RetryBackoff::scheduleRetry()
    [NEW]     +32  [ = ]       0    MissionRouteCache::getMissionLandItem()
    [NEW]     +32  [ = ]       0    MissionRouteCache::loadMissionItem()
    [NEW]     +32  [ = ]       0    MissionRouteCache::loadSafePointItem()
    [NEW]     +16  [ = ]       0    MissionRouteCache::missionCacheFullyLoaded()
    [NEW]     +32  [ = ]       0    MissionRouteCache::missionCount()
    [NEW]     +48  [ = ]       0    MissionRouteCache::missionMatchesCache()
    [NEW]     +48  [ = ]       0    MissionRouteCache::publishSafePointCache()
    [NEW]     +48  [ = ]       0    MissionRouteCache::queueMissionCacheLoads()
    [NEW]     +48  [ = ]       0    MissionRouteCache::queueMissionLandItem()
    [NEW]     +16  [ = ]       0    MissionRouteCache::resetSafePointCacheState()
    [NEW]     +32  [ = ]       0    MissionRouteCache::safePointCacheFullyLoaded()
    [NEW]     +32  [ = ]       0    MissionRouteCache::safePointCacheMatchesReadStats()
   -94.6%    +800  [ = ]       0    [46 Others]
  +2.7%    +224  [ = ]       0    [Unmapped]
  +0.3%  +148Ki  +0.2% +4.22Ki    TOTAL

px4_fmu-v6x [Total VM Diff: 4320 byte (0.21 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.2% +3.78Ki  +0.2% +3.78Ki    .text
    +0.8% +1.31Ki  +0.8% +1.31Ki    [section .text]
   -90.8%    +958 -90.8%    +958    [54 Others]
    [NEW]    +488  [NEW]    +488    MissionRouteCache::updateSafePointCache()
    [NEW]    +360  [NEW]    +360    MissionRouteCache::updateMissionCache()
    [NEW]    +236  [NEW]    +236    mission_route::Provider::scanVtolLandApproachBlock()
    [NEW]    +196  [NEW]    +196    mission_route::Provider::findAssociatedSafePointIndex()
    [NEW]    +172  [NEW]    +172    MissionRouteCache::updateMissionLandItemCache()
    [NEW]    +164  [NEW]    +164    mission_route::Position::valid()
    +8.1%    +156  +8.1%    +156    Navigator::Navigator()
    [NEW]    +136  [NEW]    +136    mission_route::Provider::anySafePointHasVtolLandApproach()
    [NEW]    +136  [NEW]    +136    mission_route::Provider::getMissionTakeoffItem()
    [NEW]    +134  [NEW]    +134    mission_route::Provider::getVtolLandApproachesNearLocation()
    [NEW]    +128  [NEW]    +128    MissionRouteCache::getMissionLandItem()
    [NEW]    +128  [NEW]    +128    mission_route::extractSafePointPosition()
    [NEW]    +108  [NEW]    +108    MissionRoutePlanner::MissionRoutePlanner()
    [NEW]    +100  [NEW]    +100    MissionRouteCache::missionCacheFullyLoaded()
    [DEL]    -140  [DEL]    -140    RTL::getVtolLandApproachesNearLocation()
    [DEL]    -192  [DEL]    -192    RTL::scanVtolLandApproachBlock()
    [DEL]    -224  [DEL]    -224    RTL::findAssociatedSafePointIndex()
    [DEL]    -248  [DEL]    -248    RTL::extractValidSafePointPosition()
   -94.4%    -268 -94.4%    -268    RTL::updateDatamanCache()
  [ = ]       0  +0.4%    +448    .bss
    [ = ]       0  [NEW]    +400    projectionReferenceBatch()::storage
    [ = ]       0   +80%     +32    [section .bss]
    [ = ]       0  [NEW]     +12    projectionReferenceBatch()::batch
    [ = ]       0  +3.1%      +4    g_gpio_callbacks
  +0.4% +8.14Ki  [ = ]       0    .debug_abbrev
  +0.4%    +608  [ = ]       0    .debug_aranges
  +0.4% +1.85Ki  [ = ]       0    .debug_frame
  +0.3% +94.5Ki  [ = ]       0    .debug_info
  +0.3% +13.6Ki  [ = ]       0    .debug_line
     +17%      +1  [ = ]       0    [Unmapped]
    +0.3% +13.6Ki  [ = ]       0    [section .debug_line]
  +0.2% +7.47Ki  [ = ]       0    .debug_loclists
  +0.2% +1.26Ki  [ = ]       0    .debug_rnglists
    [DEL]      -3  [ = ]       0    [Unmapped]
    +0.2% +1.26Ki  [ = ]       0    [section .debug_rnglists]
  +0.4% +13.7Ki  [ = ]       0    .debug_str
  +0.9%      +2  [ = ]       0    .shstrtab
  +0.3% +1.90Ki  [ = ]       0    .strtab
    [NEW]     +44  [ = ]       0    DatamanClient::abortCurrentOperation()
    [NEW]     +24  [ = ]       0    MissionRouteCache
    [NEW]     +54  [ = ]       0    MissionRouteCache::RetryBackoff::scheduleRetry()
    [NEW]     +64  [ = ]       0    MissionRouteCache::getMissionLandItem()
    [NEW]     +60  [ = ]       0    MissionRouteCache::loadMissionItem()
    [NEW]     +62  [ = ]       0    MissionRouteCache::loadSafePointItem()
    [NEW]     +62  [ = ]       0    MissionRouteCache::missionCacheFullyLoaded()
    [NEW]     +40  [ = ]       0    MissionRouteCache::missionCount()
    [NEW]     +58  [ = ]       0    MissionRouteCache::missionMatchesCache()
    [NEW]     +48  [ = ]       0    MissionRouteCache::publishSafePointCache()
    [NEW]     +60  [ = ]       0    MissionRouteCache::queueMissionCacheLoads()
    [NEW]     +47  [ = ]       0    MissionRouteCache::queueMissionLandItem()
    [NEW]     +51  [ = ]       0    MissionRouteCache::resetSafePointCacheState()
    [NEW]     +53  [ = ]       0    MissionRouteCache::safePointCacheFullyLoaded()
    [NEW]     +58  [ = ]       0    MissionRouteCache::safePointCacheMatchesReadStats()
    [NEW]     +42  [ = ]       0    MissionRouteCache::safePointCount()
    [NEW]     +43  [ = ]       0    MissionRouteCache::update()
    [NEW]     +56  [ = ]       0    MissionRouteCache::updateMissionCache()
    [NEW]     +64  [ = ]       0    MissionRouteCache::updateMissionLandItemCache()
    [NEW]     +58  [ = ]       0    MissionRouteCache::updateSafePointCache()
   -96.5%    +894  [ = ]       0    [27 Others]
  +0.2% +1.30Ki  [ = ]       0    .symtab
   -50.0%     -16  [ = ]       0    DatamanCache::changeUpdateIndex()
     +50%     +16  [ = ]       0    DatamanCache::invalidate()
    [NEW]     +16  [ = ]       0    DatamanClient::abortCurrentOperation()
     +50%     +16  [ = ]       0    DatamanClient::lastOperationCompleted()
   -50.0%     -16  [ = ]       0    MissionBase::on_inactivation()
     +50%     +16  [ = ]       0    MissionBase::updateDatamanCache()
    [NEW]     +32  [ = ]       0    MissionRouteCache
    [NEW]     +48  [ = ]       0    MissionRouteCache::RetryBackoff::scheduleRetry()
    [NEW]     +32  [ = ]       0    MissionRouteCache::getMissionLandItem()
    [NEW]     +32  [ = ]       0    MissionRouteCache::loadMissionItem()
    [NEW]     +32  [ = ]       0    MissionRouteCache::loadSafePointItem()
    [NEW]     +16  [ = ]       0    MissionRouteCache::missionCacheFullyLoaded()
    [NEW]     +32  [ = ]       0    MissionRouteCache::missionCount()
    [NEW]     +48  [ = ]       0    MissionRouteCache::missionMatchesCache()
    [NEW]     +48  [ = ]       0    MissionRouteCache::publishSafePointCache()
    [NEW]     +48  [ = ]       0    MissionRouteCache::queueMissionCacheLoads()
    [NEW]     +48  [ = ]       0    MissionRouteCache::queueMissionLandItem()
    [NEW]     +16  [ = ]       0    MissionRouteCache::resetSafePointCacheState()
    [NEW]     +32  [ = ]       0    MissionRouteCache::safePointCacheFullyLoaded()
    [NEW]     +32  [ = ]       0    MissionRouteCache::safePointCacheMatchesReadStats()
   -94.6%    +800  [ = ]       0    [44 Others]
  +4.2%    +224  [ = ]       0    [Unmapped]
  +0.3%  +148Ki  +0.2% +4.22Ki    TOTAL

Updated: 2026-07-10T06:39:42

github-actions · 2026-06-18T09:46:09Z

No broken links found in changed files.

JonasPerolini · 2026-06-18T10:20:11Z

+/**
+ * TODO: FIX: for now this does not work because some variables are init to NAN.
+ * Therefore, the large struct lives in .data instead of .bss
+ *
+ * The ~24 KB ProjectionReferenceBatch is reused by collectVehicleProjection and
+ * selectSafePoint. There is no thread concerns, we just need to find a way to define it in the .bss
+ */
+static ProjectionReferenceBatch s_safe_point_batch;


To be fixed before merging

The size of the ProjectionReferenceBatch buffer is now defined by CONFIG_RTL_SAFE_POINT_BATCH_SIZE. Since this struct is large, we can't have it in .data (FLASH), we need it in .bss (AXI_SRAM),

Because this struct uses non-zero default initializers (e.g., NAN, -1, NAV_CMD_INVALID), the compiler places the entire 24 KB initialized struct into the .data (FLASH) which has no room available for such struct.

Solution to move this to .bss without breaking standard initialization:

Allocate uninitialized, correctly aligned raw byte storage in .bss.

Construct the ProjectionReferenceBatch inside this storage buffer during the MissionRoutePlanner construction.

Considered option: Change the default init to zero for all the fields. Rejected because it would require too many .reset()

Tests on fmu-v6c:

CONFIG_RTL_SAFE_POINT_BATCH_SIZE = 1: AXI_SRAM = 12.51%

CONFIG_RTL_SAFE_POINT_BATCH_SIZE = 64: AXI_SRAM = 17.02%

CONFIG_RTL_SAFE_POINT_BATCH_SIZE = 128: AXI_SRAM = 21.61%

CONFIG_RTL_SAFE_POINT_BATCH_SIZE = 254: AXI_SRAM = 30.66%

JonasPerolini · 2026-06-18T11:09:28Z

Thank you for the review @hamishwillee, I've updated the docs.

I prefer to tackle MAV_CMD_NAV_RETURN_PATH_START in a follow up PR to keep this one as focused as possible (less things to test). It is compatible, as we project points onto every route segment, we would need to clear the cached candidates once we see the first MAV_CMD_NAV_RETURN_PATH_START (we can't skip all projections until MAV_CMD_NAV_RETURN_PATH_START because the route might not contain one). We would need to check how to handle routes with several MAV_CMD_NAV_RETURN_PATH_START.

mrpollo

Great feature, however this is adding a significanta mount of FLASH (0.2%) and it will touch all hardware targets, we need to wait on merging until v1.18 is branched out, we should be in better shape for this once the changes to PX4 for v2.0 hit main, then we can talk about adding this feature.

I'm marking this PR as needs changes, just so we can block merge for now.

… route-following RTL

…ling