fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(flight_mode_manager): fix terrain following position setpoint

Browse Source 
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.
This commit is contained in:
Jacob Dahl 2026-04-04 13:52:06 -08:00
parent 550b7148a5
commit c0be2b5d7d
2 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
View File

@ -107,14 +107,23 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
if (!_terrain_hold) {
if (_terrain_hold_previous) {
// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
const bool terrain_position = (_terrain_hold || _param_mpc_alt_mode.get() == 1)
&& PX4_ISFINITE(_dist_to_bottom)
&& PX4_ISFINITE(_position_setpoint(2));
if (terrain_position) {
// Terrain hold or terrain following: parent class set position from terrain.
// Keep smoothing block synchronized to prevent divergence on transitions.
_smoothing.setCurrentPosition(_position_setpoint(2));
} else {
if (_terrain_position_previous) {
// Transitioning out of terrain mode: reset smoothing to current position
_smoothing.setCurrentPosition(_position(2));
}
_position_setpoint(2) = _smoothing.getCurrentPosition();
}
_terrain_hold_previous = _terrain_hold;
_terrain_position_previous = terrain_position;
}

2
src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
View File

@ -69,5 +69,5 @@ protected:
)
private:
bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */
bool _terrain_position_previous{false}; /**< true when parent class was managing position setpoint from terrain data in the previous iteration */
};