fix(FlightTaskManualAltitude): Always invalidate stored distance when exiting terrain hold - #28231
Merged
mbjd merged 2 commits intoAug 31, 2026
Merged
mbjd merged 2 commits into
mbjd merged 2 commits into
Conversation
Copy link
Copy Markdown
Contributor
Sorry, something went wrong.
… when stopping terrain hold
When terrain hold disengages due to horizontal stick input,
_dist_to_ground_lock currently keeps its last value. If terrain hold
then re-engages while the altitude sp is NaN (vertical velocity control)
the vehicle would fly to the old height above ground.
Fix: Always invalidate the lock when stopping terrain hold.
Reproduce the failure / verify the fix:
- make px4_sitl gz_x500_lidar_down with default params
- particularly: MPC_ALT_MODE 2
- Take off in position mode, hover at low altitude to lock
- Give horizontal stick input to disengage terrain hold
- Give throttle up to ascend
- Stop giving horizontal stick input
- Release throttle at higher altitude
Result before: Vehicle descends down to initial altitude
Result after: Vehicle stays at altitude where throttle was released +
braking distance
Flight tasks are statically allocated, so _terrain_hold and _dist_to_ground_lock survive switching to another mode and back. On reactivation with sticks centered and a valid distance to ground, terrain following would resume on the old lock and command the height above ground from before the mode switch, however long ago and far away that was. Since no disengage or re-engage transition runs in between, invalidating the lock on those transitions cannot cover this path. Start every activation with terrain hold off and no lock; the regular engage logic then re-latches at the current height above ground when the conditions are met.
Copy link
Copy Markdown
Contributor
🔎 FLASH Analysispx4_fmu-v5x [Total VM Diff: 8 byte (0 %)] FILE SIZE VM SIZE
-------------- --------------
+0.0% +8 +0.0% +8 .text
+14% +12 +14% +12 FlightTaskManualAltitude::activate()
-0.6% -4 -0.6% -4 FlightTaskManualAltitude::_updateAltitudeLock()
+0.0% +57 [ = ] 0 .debug_abbrev
-0.0% -25 [ = ] 0 .debug_line
+250% +5 [ = ] 0 [Unmapped]
-0.0% -30 [ = ] 0 [section .debug_line]
-0.0% -19 [ = ] 0 .debug_loclists
-0.0% -17 [ = ] 0 .debug_rnglists
[DEL] -3 [ = ] 0 [Unmapped]
-0.0% -14 [ = ] 0 [section .debug_rnglists]
-0.1% -8 [ = ] 0 [Unmapped]
-0.0% -4 +0.0% +8 TOTAL
FILE SIZE VM SIZE
-------------- --------------
+0.0% +57 [ = ] 0 .debug_abbrev
-0.0% -33 [ = ] 0 .debug_line
-75.0% -3 [ = ] 0 [Unmapped]
-0.0% -30 [ = ] 0 [section .debug_line]
-0.0% -19 [ = ] 0 .debug_loclists
-0.0% -13 [ = ] 0 .debug_rnglists
+100% +1 [ = ] 0 [Unmapped]
-0.0% -14 [ = ] 0 [section .debug_rnglists]
-0.0% -8 [ = ] 0 TOTAL
Updated: 2026-08-12T15:16:24 |
Sorry, something went wrong.
dakejahl
approved these changes
Aug 13, 2026
dakejahl
left a comment
Copy link
Copy Markdown
Contributor
There was a problem hiding this comment.
There are a lot of issues with rangefinder fusion and state logic. If you want to jump down the rabbit hole let me know 😅
Sorry, something went wrong.
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
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.
Problem
When terrain hold disengages due to horizontal stick input, _dist_to_ground_lock currently keeps its last value. If terrain hold then re-engages while the altitude sp is NaN (vertical velocity control) the vehicle flies back to the old height above ground.
Solution
Always invalidate _dist_to_ground_lock when exiting terrain hold.
Related fix (claude found when reviewing the first): 10584a9 - reset terrain hold state in activate() to avoid flying to completely stale altitudes after switching to external mode and back. Can put in separate PR if preferred.
Testing
Reproduce the failure / verify the fix:
Result before: Vehicle descends down to initial altitude
Result after: Vehicle stays at altitude where throttle was released + braking distance
Changelog Entry
For release notes: