fix(flight_mode_manager): fix terrain following position setpoint by dakejahl · Pull Request #26975 · PX4/PX4-Autopilot

dakejahl · 2026-04-04T21:54:16Z

Summary

Fixes [Bug] Terrain Following in Position mode doesn't work as expected #24511
In MPC_ALT_MODE=1 (terrain following), the smoothing block in _setOutputState() was unconditionally overwriting the terrain-adjusted position setpoint computed by the parent class, effectively making terrain following non-functional when using acceleration-based position mode (FlightTaskManualAltitudeSmoothVel)
Lift ownership of "is the Z setpoint coming from terrain this iteration?" into the base class FlightTaskManualAltitude via a _z_setpoint_from_terrain flag. Children that manage their own smoothing read the flag instead of re-deriving the terrain gate
This also fixes a related case the original open-coded condition missed: the parent invokes _terrainFollowing() from the min-altitude safety branch (when braking below the distance sensor's minimum valid range — vehicle_local_position.hagl_min, populated by EKF2 — regardless of MPC_ALT_MODE). That setpoint is now also preserved through the smoothing block

github-actions · 2026-04-04T22:00:52Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 96 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +96  +0.0%     +96    .text
  +105%     +84  +105%     +84    FlightTaskManualAltitudeSmoothVel::_setOutputState()
  +0.0%      +8  +0.0%      +8    [section .text]
  +3.7%      +2  +3.7%      +2    FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
  +4.8%      +2  +4.8%      +2    FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0%    +260  [ = ]       0    .debug_info
+0.0%    +189  [ = ]       0    .debug_line
  +600%      +6  [ = ]       0    [Unmapped]
  +0.0%    +183  [ = ]       0    [section .debug_line]
+0.0%    +113  [ = ]       0    .debug_loclists
+0.0%     +14  [ = ]       0    .debug_rnglists
  +100%      +1  [ = ]       0    [Unmapped]
  +0.0%     +13  [ = ]       0    [section .debug_rnglists]
+0.0%     +21  [ = ]       0    .debug_str
+0.0%     +16  [ = ]       0    .symtab
   +50%     +16  [ = ]       0    FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ()
  +100%     +32  [ = ]       0    FlightTaskManualAltitudeSmoothVel::_setOutputState()
 -50.0%     -16  [ = ]       0    FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
 -50.0%     -16  [ = ]       0    FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
   +33%     +16  [ = ]       0    FlightTaskManualAltitudeSmoothVel::activate()
 -25.0%     -16  [ = ]       0    FlightTaskOrbit::activate()
 -40.0%     -32  [ = ]       0    __nxsem_wait_veneer
   +67%     +32  [ = ]       0    __sq_remafter_veneer
-0.8%     -96  [ = ]       0    [Unmapped]
+0.0%    +668  +0.0%     +96    TOTAL

px4_fmu-v6x [Total VM Diff: 96 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +96  +0.0%     +96    .text
  +105%     +84  +105%     +84    FlightTaskManualAltitudeSmoothVel::_setOutputState()
  +0.0%      +8  +0.0%      +8    [section .text]
  +3.7%      +2  +3.7%      +2    FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
  +4.8%      +2  +4.8%      +2    FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0%    +260  [ = ]       0    .debug_info
+0.0%    +189  [ = ]       0    .debug_line
  [NEW]      +6  [ = ]       0    [Unmapped]
  +0.0%    +183  [ = ]       0    [section .debug_line]
+0.0%    +113  [ = ]       0    .debug_loclists
+0.0%     +10  [ = ]       0    .debug_rnglists
  [DEL]      -3  [ = ]       0    [Unmapped]
  +0.0%     +13  [ = ]       0    [section .debug_rnglists]
+0.0%     +21  [ = ]       0    .debug_str
+0.0%     +16  [ = ]       0    .symtab
   +50%     +16  [ = ]       0    FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ()
  +100%     +32  [ = ]       0    FlightTaskManualAltitudeSmoothVel::_setOutputState()
 -50.0%     -16  [ = ]       0    FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
 -50.0%     -16  [ = ]       0    FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
   +33%     +16  [ = ]       0    FlightTaskManualAltitudeSmoothVel::activate()
 -25.0%     -16  [ = ]       0    FlightTaskOrbit::activate()
-1.4%     -96  [ = ]       0    [Unmapped]
+0.0%    +664  +0.0%     +96    TOTAL

Updated: 2026-04-04T22:18:17

In MPC_ALT_MODE=1 terrain following, the smoothing block was overwriting the parent class's terrain-adjusted position setpoint. Unify terrain hold and terrain following into a single terrain_position flag that syncs the smoothing block to the parent's position, preventing divergence and simplifying transition handling.

dakejahl · 2026-04-08T18:54:50Z

I tested this in simulation and it works as expected

…lass FlightTaskManualAltitudeSmoothVel re-derived the parent's terrain gate in _setOutputState() to decide whether to keep the parent's position setpoint or overwrite it with the smoothing block. Two problems with that: 1. The condition was duplicated verbatim from the parent's gate in _updateAltitudeLock(), so any future change there would silently break the child. 2. The parent also invokes _terrainFollowing() from the min-altitude safety branch (MPC_ALT_MODE != 1 && !_terrain_hold but below hagl_min while braking). The child's open-coded condition did not cover that case, so the smoothing block still overwrote a valid terrain-adjusted setpoint there. Move ownership of the decision into the parent: _terrainFollowing() sets _z_setpoint_from_terrain iff it produced a finite setpoint, and _updateAltitudeLock() resets the flag at the top of each iteration. The child now reads the flag instead of re-deriving the condition, which also fixes the min-altitude safety branch above. _z_setpoint_from_terrain is named to stay semantically distinct from _terrain_hold (an MPC_ALT_MODE=2 sub-state that gates the terrain following logic).

github-actions · 2026-04-14T20:49:20Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 72 byte (0 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +72  +0.0%     +72    .text
     +40%     +32   +40%     +32    FlightTaskManualAltitudeSmoothVel::_setOutputState()
     +24%     +28   +24%     +28    FlightTaskManualAltitude::_terrainFollowing()
    +1.3%      +8  +1.3%      +8    FlightTaskManualAltitude::_updateAltitudeLock()
    +1.6%      +4  +1.6%      +4    FlightTaskManualAltitude::FlightTaskManualAltitude()
    +2.2%      +4  +2.2%      +4    GainCompression3d::~GainCompression3d()
    -1.3%      -4  -1.3%      -4    FwAutotuneAttitudeControl::~FwAutotuneAttitudeControl()
  +0.0%     +56  [ = ]       0    .debug_abbrev
  +0.0%      +9  [ = ]       0    .debug_info
  +0.0%    +135  [ = ]       0    .debug_line
   -33.3%      -2  [ = ]       0    [Unmapped]
    +0.0%    +137  [ = ]       0    [section .debug_line]
  -0.0%     -46  [ = ]       0    .debug_loclists
  +0.0%      +2  [ = ]       0    .debug_rnglists
     +50%      +1  [ = ]       0    [Unmapped]
    +0.0%      +1  [ = ]       0    [section .debug_rnglists]
  +0.0%     +32  [ = ]       0    .debug_str
  -0.6%     -72  [ = ]       0    [Unmapped]
  +0.0%    +188  +0.0%     +72    TOTAL

px4_fmu-v6x [Total VM Diff: 64 byte (0 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +64  +0.0%     +64    .text
     +40%     +32   +40%     +32    FlightTaskManualAltitudeSmoothVel::_setOutputState()
     +24%     +28   +24%     +28    FlightTaskManualAltitude::_terrainFollowing()
    +1.3%      +8  +1.3%      +8    FlightTaskManualAltitude::_updateAltitudeLock()
    +1.6%      +4  +1.6%      +4    FlightTaskManualAltitude::FlightTaskManualAltitude()
    -1.3%      -4  -1.3%      -4    FwAutotuneAttitudeControl::~FwAutotuneAttitudeControl()
    -2.2%      -4  -2.2%      -4    GainCompression3d::~GainCompression3d()
  +0.0%     +56  [ = ]       0    .debug_abbrev
  +0.0%      +9  [ = ]       0    .debug_info
  +0.0%    +135  [ = ]       0    .debug_line
   -66.7%      -2  [ = ]       0    [Unmapped]
    +0.0%    +137  [ = ]       0    [section .debug_line]
  -0.0%     -46  [ = ]       0    .debug_loclists
  +0.0%      +2  [ = ]       0    .debug_rnglists
     +50%      +1  [ = ]       0    [Unmapped]
    +0.0%      +1  [ = ]       0    [section .debug_rnglists]
  +0.0%     +32  [ = ]       0    .debug_str
  -1.6%     -64  [ = ]       0    [Unmapped]
  +0.0%    +188  +0.0%     +64    TOTAL

Updated: 2026-04-14T20:49:04

Copilot

Pull request overview

This PR fixes terrain-following altitude behavior in MPC_ALT_MODE=1 when using acceleration-based position mode (FlightTaskManualAltitudeSmoothVel) by preventing the smoothing output stage from overwriting the terrain-adjusted Z position setpoint computed by the parent task.

Changes:

Introduce a base-class flag (_z_setpoint_from_terrain) in FlightTaskManualAltitude to indicate when _terrainFollowing() produced the Z position setpoint for the current iteration.
Update FlightTaskManualAltitudeSmoothVel to preserve terrain-driven _position_setpoint(2) and keep the smoothing state synchronized across terrain/non-terrain transitions.

Reviewed changes

Copilot reviewed 4 out of 4 changed files in this pull request and generated no comments.

File	Description
src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp	Replace previous terrain-hold tracking with a flag that mirrors parent terrain-driven Z state across iterations.
src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp	Avoid overwriting terrain-driven Z position setpoint; synchronize smoothing state and handle transition off terrain-driven Z.
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp	Add `_z_setpoint_from_terrain` to expose “terrain drove Z this iteration” to derived tasks.
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp	Reset/set `_z_setpoint_from_terrain` per-iteration and mark it after `_terrainFollowing()` based on whether it produced a finite Z setpoint.

julianoes

This is better than my fix.

julianoes · 2026-05-04T02:36:10Z

Tested on my end, fixes the problem I saw.

dakejahl mentioned this pull request Apr 4, 2026

[Bug] Terrain Following in Position mode doesn't work as expected #24511

Closed

dakejahl force-pushed the dakejahl/terrain-follow-bugfix branch from fcfa4aa to c0be2b5 Compare April 4, 2026 22:11

dakejahl requested review from MaEtUgR, bresch and dirksavage88 April 8, 2026 18:53

dakejahl requested a review from Copilot April 14, 2026 20:48

Copilot started reviewing on behalf of dakejahl April 14, 2026 20:49 View session

Copilot AI reviewed Apr 14, 2026

View reviewed changes

dakejahl mentioned this pull request Apr 16, 2026

Rangefinder step-changes cause altitude jumps in Terrain Hold with optical flow #27110

Open

julianoes mentioned this pull request May 4, 2026

fix(ManualAltitudeSmoothVel): respect terrain lock when smoothing #27270

Closed

julianoes approved these changes May 4, 2026

View reviewed changes

julianoes merged commit 6f6ead5 into main May 4, 2026
88 checks passed

julianoes deleted the dakejahl/terrain-follow-bugfix branch May 4, 2026 02:36

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(flight_mode_manager): fix terrain following position setpoint#26975

fix(flight_mode_manager): fix terrain following position setpoint#26975
julianoes merged 2 commits intomainfrom
dakejahl/terrain-follow-bugfix

dakejahl commented Apr 4, 2026 •

edited

Loading

Uh oh!

github-actions Bot commented Apr 4, 2026 •

edited

Loading

Uh oh!

dakejahl commented Apr 8, 2026

Uh oh!

github-actions Bot commented Apr 14, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

julianoes left a comment

Uh oh!

julianoes commented May 4, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Conversation

dakejahl commented Apr 4, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Uh oh!

github-actions Bot commented Apr 4, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

🔎 FLASH Analysis

Uh oh!

dakejahl commented Apr 8, 2026

Uh oh!

github-actions Bot commented Apr 14, 2026

🔎 FLASH Analysis

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

julianoes left a comment

Choose a reason for hiding this comment

Uh oh!

julianoes commented May 4, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

dakejahl commented Apr 4, 2026 •

edited

Loading

github-actions Bot commented Apr 4, 2026 •

edited

Loading