flight mode manager: fix terrain hold

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

@@ -106,5 +106,15 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 	_jerk_setpoint(2) = _smoothing.getCurrentJerk();
 	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
 	_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
-	_position_setpoint(2) = _smoothing.getCurrentPosition();
+
+	if (!_terrain_hold) {
+		if (_terrain_hold_previous) {
+			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
+			_smoothing.setCurrentPosition(_position(2));
+		}
+
+		_position_setpoint(2) = _smoothing.getCurrentPosition();
+	}
+
+	_terrain_hold_previous = _terrain_hold;
 }

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

@@ -67,4 +67,7 @@ protected:
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
 				       )
+
+private:
+	bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */
 };