FW nav: fix WP JUMP target skipped by stale turn-smoothing flag#11648
Merged
Conversation
When a WP mission JUMP activated its target waypoint, isWaypointReached() could read a wpTurnSmoothingActive flag left over from the previous waypoint. The JUMP defers target setup by one navigation loop, and in that gap the FW position controller re-arms the flag from the previous waypoint's geometry. With nav_fw_wp_turn_smoothing = CUT this marks the jump target as reached on its first loop (and cascades the following WP via the bearing-miss check), so the mission jumps to target+1 instead of the target. Reset wpTurnSmoothingActive when a new active waypoint is set so a freshly activated waypoint cannot inherit the previous waypoint's smoothing-reached state. Turn smoothing is otherwise unchanged: the FW controller recomputes the flag every position update from the new waypoint geometry. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
Contributor
Qodo reviews are paused for this user.Troubleshooting steps vary by plan Learn more → On a Teams plan? Using GitHub Enterprise Server, GitLab Self-Managed, or Bitbucket Data Center? |
|
Test firmware build ready — commit Download firmware for PR #11648 238 targets built. Find your board's
|
Collaborator
Author
|
Intensively tested in SITL. One line variable reset. No break found. Will merge. |
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
1 participant
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.
What
Fixes a long-standing bug where a WP mission JUMP lands on target+1 instead of
the intended target waypoint. Intermittent in general, but ~100% reproducible
with
nav_fw_wp_turn_smoothing = CUT, especially on specific mission layouts.Root cause
isWaypointReached()reads the sharedwpTurnSmoothingActiveflag, which is ownedand recomputed by the FW position controller. A JUMP defers setup of its target
waypoint by one navigation loop; in that gap the controller re-arms the flag from
the previous waypoint's geometry. On the next loop the freshly activated target
inherits that stale flag and, in CUT mode, is marked "reached" on its very first
loop (and can cascade the following WP via the bearing-miss check). The active
waypoint counter advances past the target.
This matches blackbox evidence: the correct target WP is active for exactly one
50 Hz nav tick (~20 ms) before being skipped.
Fix
Reset
wpTurnSmoothingActivewhen a new active waypoint is set, so a freshlyactivated waypoint cannot inherit the previous waypoint's smoothing-reached state.
One line in
calculateAndSetActiveWaypointToLocalPosition(), next to the existingnextTurnAnglereset.Turn smoothing itself is unchanged — the controller recomputes the flag every
position update from the new waypoint's geometry, so smoothed turns still work.
Testing
nav_fw_wp_turn_smoothing = CUT.Notes
settings.yamlchange → no CLI docs regeneration needed.