fix(ManualAltitudeSmoothVel): respect terrain lock when smoothing

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -80,6 +80,15 @@ class FlightTaskManualAltitude : public FlightTask
 	bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
 	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
 
+	/**
+	 * Distance to ground during terrain following.
+	 * If user does not demand velocity change in D-direction and the vehicle
+	 * is in terrain-following mode, then height to ground will be locked to
+	 * _dist_to_ground_lock. Protected so the smoother subclass can detect
+	 * an active terrain lock and avoid overwriting the position setpoint.
+	 */
+	float _dist_to_ground_lock = NAN;
+
 	float _velocity_constraint_up{INFINITY};
 	float _velocity_constraint_down{INFINITY};
 
@@ -129,12 +138,4 @@ class FlightTaskManualAltitude : public FlightTask
 
 	float _min_distance_to_ground{(float)(-INFINITY)}; /**< min distance to ground constraint */
 	float _max_distance_to_ground{(float)INFINITY};  /**< max distance to ground constraint */
-
-	/**
-	 * Distance to ground during terrain following.
-	 * If user does not demand velocity change in D-direction and the vehcile
-	 * is in terrain-following mode, then height to ground will be locked to
-	 * _dist_to_ground_lock.
-	 */
-	float _dist_to_ground_lock = NAN;
 };

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

@@ -107,14 +107,22 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
 	_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
 
-	if (!_terrain_hold) {
+	// Terrain following (MPC_ALT_MODE 1) and terrain hold (MPC_ALT_MODE 2)
+	// both compute _position_setpoint(2) directly from the rangefinder lock
+	// in _terrainFollowing(). In those cases we must not overwrite it with
+	// the velocity-integrated smoother output, otherwise the climb/descend
+	// commanded to maintain AGL gets stomped every cycle. Mode 1 never sets
+	// _terrain_hold, so checking _dist_to_ground_lock catches both cases.
+	const bool terrain_lock_active = _terrain_hold || PX4_ISFINITE(_dist_to_ground_lock);
+
+	if (!terrain_lock_active) {
 		if (_terrain_hold_previous) {
-			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
+			// Reset smoother to current position when leaving terrain lock
 			_smoothing.setCurrentPosition(_position(2));
 		}
 
 		_position_setpoint(2) = _smoothing.getCurrentPosition();
 	}
 
-	_terrain_hold_previous = _terrain_hold;
+	_terrain_hold_previous = terrain_lock_active;
 }