fix(flight_mode_manager): fix terrain following position setpoint

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.

Author: dakejahl

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -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;
 }

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -69,5 +69,5 @@ class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 				       )
 
 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 */
 };