flight mode manager: fix terrain hold

src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp

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

src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp

@@ -87,4 +87,6 @@ private:
 
 	ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions
 	ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction
+
+	bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */
 };