AP_Mission: fix get_next_nav_cmd - #33863
Open
robertlong13 wants to merge 8 commits into
Open
Conversation
Contributor
|
This is really great to see and, "a jump loop containing no nav command (a forever DO_JUMP over do commands) never terminates the search and hangs the main loop" sounds like a serious bug we need to fix. Thanks! |
robertlong13
force-pushed
the
pr/fix-get-next-nav-cmd
branch
from
July 28, 2026 13:29
c7e1063 to
5aea137
Compare
robertlong13
force-pushed
the
pr/fix-get-next-nav-cmd
branch
from
July 29, 2026 07:05
5aea137 to
7868aad
Compare
Fly a mission that loops forever over one leg, with unreachable waypoints placed 300m beyond the jump, and check the vehicle keeps to the leg. Position is checked rather than the reported current waypoint, which advances correctly even when the lookahead used to shape the path does not. Four shapes are covered: a DO_JUMP to a do command, a DO_JUMP straight to a nav command, a DO_JUMP_TAG whose tag is followed by a nav command, and a DO_JUMP_TAG whose tag is followed by a do command.
get_next_cmd, get_jump_times_run and increment_jump_times_run resolve do-jumps against the live _jump_tracking counters, so any caller scanning the mission ahead of the vehicle either disturbs the counters the running mission depends on, or has to save and restore them around the scan. Give the three of them an optional jump cursor to use in place of _jump_tracking. A caller that supplies one gets jumps resolved and counted exactly as execution would, against its own copy. Passing nullptr keeps the live counters, so nothing changes for existing callers; no caller supplies a cursor yet.
distance_to_landing and distance_to_mission_leg copied _jump_tracking into a local array, let the scan mutate the live counters, then copied them back on the way out. Every exit had to route through a goto so the restore could not be missed. Seed a jump cursor from the live counters and scan on that instead. The counters are never touched, so the backup, the restore and the two goto labels go away and each exit can simply return. No change in behavior.
send_gcs_msg was the other half of the workaround the jump cursor has just replaced: a scan running against the live counters had to ask for silence, or it would announce jumps the vehicle was not taking. Every caller that wanted silence wanted it because it was scanning ahead, which the cursor now states outright, so the flag carries no information the cursor does not. Drop it and report a jump exactly when the live counters are the ones being advanced. A look-ahead can no longer announce a jump by forgetting an argument. No change in behavior.
get_next_nav_cmd stepped its own index after each command, so a jump followed during the scan was discarded: the search resumed in the linear sequence past the jump rather than at the command the jump resolved to, and reported a nav command the vehicle will never fly. On Copter that answer becomes the next destination handed to the S-curve, so the vehicle flies toward it. A DO_JUMP landing directly on a nav command hid the fault, as the scan returned on its first pass; DO_JUMP_TAG always tripped it, the tag marker never being a nav command. Resume from the index the scan resolved to, which is what advance_current_nav_cmd already does, and walk a private jump cursor so the look-ahead follows finite jumps as execution would without spending the running mission's counters. Fixes issues 31021 and 21534.
Six scans walk the mission through get_next_cmd, and each one decided for itself where to look next. Three resumed from the index the scan resolved to; get_next_nav_cmd stepped its own index, which was the previous commit's bug, and distance_to_landing's inner scan still does, disagreeing with the outer loop of the very same function. The rule is easy to state and was written out six times, wrongly twice. Hand the rule to get_next_cmd: scan_index becomes the caller's position in the mission and is left pointing at the command to resume from, so a scan continued through this function follows jumps the way the running mission does. Every caller drops its own copy, and is_takeoff_next stops re-finding the nav command it just examined. This is a consistency change rather than a fix -- the previous commit already closed the reported bug -- but it also corrects distance_to_landing, which nothing in the test suite covers, and it means the seventh scan cannot reintroduce this.
Setting the current command searches forward for a nav command to load, following jumps on the way. A forever DO_JUMP over do commands never runs out of repeats and never reaches a nav command, so the search has nothing to terminate on. Upload such a mission and set the current command. Without a bound on the search the vehicle stops answering: the run_cmd below gets no ack and sim time stops advancing, because the main loop never comes back.
set_current_cmd walks the mission looking for a nav command to load, and the walk follows jumps. Nothing bounded it, so a jump loop holding no nav command spun forever inside the main loop: a forever DO_JUMP never exhausts its repeats, and no nav command is ever reached to end the search. get_next_cmd's own guard does not help, as it counts the jumps followed within a single call and this walk follows one per call. Bound the search the same way advance_current_nav_cmd bounds its own, and report failure when the bound is hit. Checking the counter inside the loop rather than after it keeps the test honest, as a post-decrement guard wraps back to 255 on the iteration that ends the loop.
robertlong13
force-pushed
the
pr/fix-get-next-nav-cmd
branch
from
July 29, 2026 10:07
96cc1d7 to
29865ab
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Classification & Testing
Description
This supersedes PRs #32029 and #31038
On Copter, the next nav command is fetched before it is executed:
get_next_nav_cmdsupplies the target the S-curve calculations are shaped through, andadvance_current_nav_cmdlater loads what actually runs. Nothing guaranteed the two resolved the mission the same way, and around jumps they did not (as reported in #21534), causing Copter to claim it was flying to one waypoint (after it advances it) while flying to a different one (the one it fetched before). On Plane, we don't pre-fetch the next waypoint, so this bug has less dramatic impact.They disagreed because
get_next_nav_cmdnever actually followed a jump. The scan handedget_next_cmda linearly incremented index each pass, so a jump's target was only glanced at (accepted if it happened to be a nav command directly, discarded otherwise) until the index walked past the jump and the scan continued in the linear sequence beyond it, reporting a nav command the vehicle may not fly next (and in fact, may never fly if that was an infinite jump).A jump tag is never a nav command, so
DO_JUMP_TAGmissions tripped this every time, but tags are just the common case of jumping to a non-nav command; aDO_JUMPwhich targets any non-nav command fails the same way.The fix builds on a small mechanism: the jump-resolving look-ups (
get_next_cmd,get_jump_times_run,increment_jump_times_run) gain an optional jump cursor, a private copy of the do-jump counters, so a caller can scan ahead following jumps exactly as execution would, without spending the running mission's counters. Fetch and execute now walk the same resolution path, differing only in whose counters they spend, so they cannot disagree. That cursor replaces the two workarounds that existed for the same problem: the backup/restore-through-goto dance in the distance scans, and thesend_gcs_msgflag (a jump is now reported to the GCS exactly when the live counters are the ones being advanced).get_next_cmdalso takes over advancing the caller's scan position, so the resume-from-the-resolved-index rule is written once instead of six times: previously two of those six were wrong, includingdistance_to_landing's inner scan (couldn't find anyone reporting any bugs from that, but that could have in theory messed with the logic for finding the closest landing on Plane).While bounding these searches,
set_current_cmdturned out to have an unbounded one: a jump loop containing no nav command (a foreverDO_JUMPover do commands) never terminates the search and hangs the main loop. It's now bounded the same wayadvance_current_nav_cmdbounds its own, failing the command instead. New autotests cover the four jump shapes (DO_JUMPandDO_JUMP_TAG, each landing on a nav and on a do command) by checking the vehicle's actual position rather than the reported waypoint, plus the set-current hang on Rover.