FlightTaskManualPosition: use existing stick library rotation

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

@@ -273,14 +273,6 @@ void FlightTaskManualAltitude::_respectGroundSlowdown()
 	}
 }
 
-void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
-{
-	const float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
-	Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
-	v(0) = v_r(0);
-	v(1) = v_r(1);
-}
-
 void FlightTaskManualAltitude::_updateHeadingSetpoints()
 {
 	if (_isYawInput() || !_is_yaw_good_for_control) {

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

@@ -63,11 +63,6 @@ protected:
 	bool _checkTakeoff() override;
 	void _updateConstraintsFromEstimator();
 
-	/**
-	 * rotates vector into local frame
-	 */
-	void _rotateIntoHeadingFrame(matrix::Vector2f &vec);
-
 	/**
 	 *  Check and sets for position lock.
 	 *  If sticks are at center position, the vehicle

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -91,8 +91,8 @@ void FlightTaskManualPosition::_scaleSticks()
 	// scale velocity to its maximum limits
 	Vector2f vel_sp_xy = stick_xy * velocity_scale;
 
-	/* Rotate setpoint into local frame. */
-	_rotateIntoHeadingFrame(vel_sp_xy);
+	// Rotate setpoint into local frame
+	Sticks::rotateIntoHeadingFrameXY(vel_sp_xy, _yaw, _yaw_setpoint);
 
 	// collision prevention
 	if (_collision_prevention.is_active()) {