fix(navigator): goToNextPositionItem skip loops when required#26993
fix(navigator): goToNextPositionItem skip loops when required#26993JonasPerolini wants to merge 8 commits intoPX4:mainfrom
Conversation
There was a problem hiding this comment.
Pull request overview
This PR refactors MissionBase mission-item traversal to make DO_JUMP handling explicit (via a new MissionTraversalType) and fixes an RTL fast-reverse edge case where an initial DO_JUMP loop could prevent selecting the correct first position waypoint. It also centralizes dataman cache reads via a loadMissionItemFromCache() helper and adds unit tests covering the new traversal semantics.
Changes:
- Replace the
execute_jumpboolean withMissionTraversalTypeand update mission/RTL call sites accordingly. - Add
loadMissionItemFromCache(), plusfindNext/PreviousPositionIndex()helpers, and refactor traversal helpers to use them. - Add navigator mission traversal unit tests and integrate them into the build.
Reviewed changes
Copilot reviewed 11 out of 11 changed files in this pull request and generated 3 comments.
Show a summary per file
| File | Description |
|---|---|
| src/modules/navigator/mission_base.h | Introduces MissionTraversalType, new traversal helper APIs, and loadMissionItemFromCache() interface. |
| src/modules/navigator/mission_base.cpp | Refactors traversal logic to respect traversal type and unifies dataman cache reads through a helper. |
| src/modules/navigator/mission.cpp | Updates mission progression/index-setting calls to use MissionTraversalType. |
| src/modules/navigator/rtl_mission_fast.cpp | Updates next-item traversal call to use MissionTraversalType. |
| src/modules/navigator/rtl_mission_fast_reverse.cpp | Uses IgnoreDoJump when bootstrapping reverse RTL from “no prior position item” state; keeps legacy traversal elsewhere. |
| src/modules/navigator/rtl_direct_mission_land.cpp | Updates item selection/progression calls to use MissionTraversalType. |
| src/modules/navigator/rtl_params.yaml | Updates RTL_TYPE descriptions to mention skipping DO_JUMP/non-position items in reverse-path modes. |
| docs/en/flight_modes/return.md | Updates Return-mode docs for reverse RTL behaviour (DO_JUMP handling) and adds the leading-loop edge case note. |
| src/modules/navigator/CMakeLists.txt | Adds the navigator test subdirectory to the build. |
| src/modules/navigator/test/CMakeLists.txt | Adds navigator functional gtest target. |
| src/modules/navigator/test/test_mission_base.cpp | Adds new unit tests for MissionBase traversal helpers and traversal semantics. |
| 2: Return to a planned mission landing, if available, using the mission path, | ||
| else return to home via the reverse mission path. Do not consider rally | ||
| points. | ||
| else return to home via the reverse mission path while skipping DO_JUMP | ||
| and other non-position mission items. Do not consider rally points. | ||
| 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: Return to the planned mission landing, or to home via the reverse mission | ||
| path, whichever is closer by counting waypoints. Do not consider rally points. | ||
| path while skipping DO_JUMP and other non-position mission items, whichever | ||
| is closer by counting waypoints. Do not consider rally points. |
There was a problem hiding this comment.
The updated RTL_TYPE descriptions for values 2 and 4 state that the reverse mission path runs "while skipping DO_JUMP". In the current implementation, reverse RTL still uses goToPreviousPositionItem(MissionTraversalType::FollowMissionControlFlow) (e.g. src/modules/navigator/rtl_mission_fast_reverse.cpp:108-111), which will execute active DO_JUMP items and can change traversal order (skip waypoints). Either adjust the description to match the actual behaviour (only skipping DO_JUMP in the specific start edge case) or change reverse traversal to use IgnoreDoJump consistently.
| - **Mission mode:** | ||
| - Mission flown "fast-backward" (in reverse) starting from the previous waypoint | ||
| - Jumps, delay and any other non-position commands ignored, loiter and other position waypoints converted to simple waypoints. | ||
| - DO_JUMP commands, delays and other non-position mission items are ignored, and loiter and other position waypoints are converted to simple waypoints. |
There was a problem hiding this comment.
This section states that in fast-backward (reverse mission) RTL, "DO_JUMP commands ... are ignored". However, reverse RTL currently advances using goToPreviousPositionItem(MissionTraversalType::FollowMissionControlFlow) (src/modules/navigator/rtl_mission_fast_reverse.cpp:108-111), which will execute active DO_JUMP items (potentially skipping waypoints). Please update the documentation to match the actual behaviour, or switch reverse traversal to IgnoreDoJump if the intent is to ignore DO_JUMP throughout reverse RTL.
| - DO_JUMP commands, delays and other non-position mission items are ignored, and loiter and other position waypoints are converted to simple waypoints. | |
| - Active `DO_JUMP` commands are followed as part of the mission control flow, so reverse RTL may skip waypoints. | |
| Delays and other non-position mission items are ignored, and loiter and other position waypoints are converted to simple waypoints. |
There was a problem hiding this comment.
In the code I've kept the legacy behaviour which does not skip the jumps:
bool RtlMissionFastReverse::setNextMissionItem()
{
return (goToPreviousPositionItem(true) == PX4_OK);
}
However the doc mentioned: DO_JUMP commands, delays and other non-position mission items are ignored
Should we update the doc or the logic @mrpollo?
There was a problem hiding this comment.
FWIW The docs reflect the original intent. The idea is that a mission path defines a safe path that you can follow forward or back to a safe location. However when you are RTL ing you want to return home in minimum time, so you want to avoid any programmed delays, or taking photos, or whatever.
if going forward through a mission you might have a DO_JUMP that takes you back to an earlier waypoint, you then progress forward until you get to the jump, and then repeat as many times as indicated. When reversing, you want to ignore that DO_JUMP and just follow the path back.
It is also possible while going forward through a mission that you might skip forward to a later item, skipping items in the middle (then going forward, and potentially jumping back and repeating bits etc). If you are reversing the mission I think you want to ignore these too - depending where you are it is possible you will actually traverse in reverse a waypoint you didn't encounter on your way out - but there is no way of knowing.
To summarise, I think it is safest to ingore all jumps in the mission, unless you can canonically work out a better minimum path that doesn't exit the planned mission path.
There was a problem hiding this comment.
Perhaps needs a little testing to verify some more complicated missions. But certainly this is better for the more common jumps, which go backwards in the mission.
There was a problem hiding this comment.
Hi @hamishwillee the way I understand loops, they are meant to target indexes lower than the current DO_JUMP point index (i.e. jump backwards) to ensure that we can repeat the loop by re-reaching the DO_JUMP. A DO_JUMP forward needs nested loops to perform the loop repetitions.
Considering this, the fastest way to get to the goal would be:
- In reverse direction: use the DO_JUMP because we skip other waypoints (shortcut)
- E.g. for a mission: [wp0, wp1, wp2, wp3, jump to 1, wp5], when flying reverse
- if the jump segment is followed: wp5 -> wp1 -> wp0
- if the jump segment is skipped: wp5 -> wp3 -> wp2 -> wp1 -> wp0
- E.g. for a mission: [wp0, wp1, wp2, wp3, jump to 1, wp5], when flying reverse
- In nominal direction: skip the DO_JUMP because we want to reach our goal
However, when flying in fixed-wing, the reverse DO_JUMP segment might not be feasible without large deviations e.g. sharp turn because the operator never intended to fly the DO_JUMP in reverse.
Overall, I would skip jumps for both rtl_mission_fast and rtl_mission_fast_reverse because the behavior is simpler and unified and because the DO_JUMP segment can have sharp angles when flown in reverse. However, assuming that jumps are meant to jump backwards, the shortest path is to skip jumps going fowards and use the jump segment when going reverse.
What do you think?
There was a problem hiding this comment.
Regarding the implementation, it is not trivial to skip jumps. It will require some changes both in rtl and in mission_base.
We need to update getNextPositionItems to use IgnoreDoJump in:
RtlMissionFast::on_activation()RtlMissionFast::setNextMissionItem()RtlMissionFast::setActiveMissionItems()
There are also other helper functions within mission_base that default to FollowMissionControlFlow:
checkClimbRequiredisLandingif (_inactivation_index > 0 && cameraWasTriggering()) {if (_align_heading_necessary && is_mission_item_reached_or_completed()) {
We probably need a general MissionTraversalType and both RTL modes would override it to IgnoreDoJump
We might also need to rework: reverseIsFurther
There was a problem hiding this comment.
Code updated here: b396503 @hamishwillee
We now have a new method virtual MissionTraversalType traversalType() const in mission_base which can be used to decide if we skip or continue loops. Note that the helper functions such as goToPreviousItem still take as input MissionTraversalType because there are exceptions for which we don't want to follow the general traversalType().
- E.g.
MissionBase::do_abort_landing()explicitely ignores DO_JUMPS in all modes.
To ensure that the helper function calls are consistent, each function has its overload method:
- with
MissionTraversalTypeused to explicitly set a traversalType e.g. do_abort_landing - without
MissionTraversalTypeas input used with the defaulttraversalType()
I did not change reverseIsFurther because it is out of scope for this PR. Once we have #26973 and the new mission route cache with non-blocking calls we can update reverseIsFurther such that the distance is used.
Added this to the docs:
::: info
For `RTL_TYPE=4`, PX4 currently chooses between continuing to a mission landing and reversing toward home by comparing raw mission item indices.
This is only an approximation of the flown path length, because the number if mission items is not indicative of the distance remaining and non-position items are also counted.
:::
There was a problem hiding this comment.
I'm not going to be able to look at this properly until next week - at this point its a technical review anyway. But
- because the number if mission item ... OF items.
- You can omit non-waypoint items possibly in the count, but the point is reasonable to add - the only way to do this properly is to calculate the actual path that will be travelled.
- I'd add some very complicated loops for testing. Perhaps because I'm rubbish at coding and tests are what save me.
There was a problem hiding this comment.
I'll update the typo: if --> of, good catch
What would be an example of some complicated loops? E.g. a loop pointing to a non-position item? Please note that there are several unit tests already for loops e.g.
// WHAT: [WP0, WP1, DO_JUMP->0, WP3] starting from idx 2 returns idx 0 then idx 1.
TEST_F(MissionBaseTraversalTest, GetNextPositionItemsFollowsActiveDoJump)
// WHAT: [WP0, WP1, DO_JUMP->0, WP3] from current_seq=3 should still land on idx 0.
TEST_F(MissionBaseTraversalTest, GoToPreviousPositionItemFollowsMissionControlFlow)
There was a problem hiding this comment.
Perhaps a check of a forward jump (to see if supported) and that this does indeed skip it. My general point was "for me more testing is better". If you're sure this works then do what you think is reasonable.
… bool execute_jump
JonasPerolini
force-pushed
the
pr-fix-rtl-loops
branch
from
b396503 to
43b45ef
Compare
🔎 FLASH Analysispx4_fmu-v5x [Total VM Diff: 280 byte (0.01 %)]px4_fmu-v6x [Total VM Diff: 272 byte (0.01 %)]Updated: 2026-04-09T09:35:49 |
|
No broken links found in changed files. |
|
Summary of the changes:
Tested the PR in SIM using RTL_TYPE 2 to enter
MissionModeLoops.mp4
ReturnNominal.mp4
ReturnReverse.mp4 |
As a first step for: #26973 as discussed with @mrpollo, this PR ensures that
goToNextPositionItem(bool execute_jump)uses theexecute_jumpinput.This fixes the init of
RTL_MISSION_FAST_REVERSEin a very specific edge case where the first position item is a loop. RTL now picks the correct starting waypoint if no previous waypoint exists.This PR also unifies the calls to
_dataman_cache.loadWaitwith a new helper function:loadMissionItemFromCachewhich checks the index inputs. Note that the_dataman_client.readSyncfromresetMissionJumpCounterwas updated to useloadMissionItemFromCacheas well.Tested with new unit tests:
make tests TESTFILTER=test_mission_baseSide note: a new enum class:
is added to clarify the function calls and simplify the code reading e.g.
goToItem(_mission.land_start_index, false)becomesgoToItem(_mission.land_start_index, MissionTraversalType::IgnoreDoJump))