diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControlMath.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControlMath.hpp
new file mode 100644
index 0000000000..fe0f760ed4
--- /dev/null
+++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControlMath.hpp
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file AttitudeControlMath.hpp
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+namespace AttitudeControlMath
+{
+/**
+ * Rotate a tilt quaternion (without yaw rotation) such that when rotated by a yaw setpoint
+ * the resulting tilt is the same as if it was rotated by the current yaw of the vehicle
+ * @param q_sp_tilt pure tilt quaternion (yaw = 0) that needs to be corrected
+ * @param q_att current attitude of the vehicle
+ * @param q_sp_yaw pure yaw quaternion of the desired yaw setpoint
+ */
+void inline correctTiltSetpointForYawError(matrix::Quatf &q_sp_tilt, const matrix::Quatf &q_att,
+		const matrix::Quatf &q_sp_yaw)
+{
+	const matrix::Vector3f z_unit(0.f, 0.f, 1.f);
+
+	// Extract yaw from the current attitude
+	const matrix::Vector3f att_z = q_att.dcm_z();
+	const matrix::Quatf q_tilt(z_unit, att_z);
+	const matrix::Quatf q_yaw = q_tilt.inversed() * q_att; // This is not euler yaw
+
+	// Find the quaternion that creates a tilt aligned with the body frame
+	// when rotated by the yaw setpoint
+	// To do so, solve q_yaw * q_tilt_ne = q_sp_yaw * q_sp_rp_compensated
+	const matrix::Quatf q_sp_rp_compensated = q_sp_yaw.inversed() * q_yaw * q_sp_tilt;
+
+	// Extract the corrected tilt
+	const matrix::Vector3f att_sp_z = q_sp_rp_compensated.dcm_z();
+	q_sp_tilt = matrix::Quatf(z_unit, att_sp_z);
+}
+}
diff --git a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt
index 4ff754e8e9..4865b42cac 100644
--- a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt
+++ b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt
@@ -34,6 +34,7 @@
 px4_add_library(AttitudeControl
 	AttitudeControl.cpp
 	AttitudeControl.hpp
+	AttitudeControlMath.hpp
 )
 target_compile_options(AttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(AttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 40013ae520..5925237219 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -49,6 +49,8 @@
 #include <mathlib/math/Limits.hpp>
 #include <mathlib/math/Functions.hpp>
 
+#include "AttitudeControl/AttitudeControlMath.hpp"
+
 using namespace matrix;
 
 MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
@@ -162,21 +164,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
 		// an attitude setpoint from the yaw setpoint will lead to unexpected attitude behaviour from
 		// the user's view as the tilt will not be aligned with the heading of the vehicle.
 
-		const Vector3f z_unit(0.f, 0.f, 1.f);
-
-		// Extract yaw from the current attitude
-		Vector3f att_z = q.dcm_z();
-		Quatf q_delta = Quatf(z_unit, att_z);
-		Quatf q_yaw = q_delta.inversed() * q; // This is not euler yaw
-
-		// Find the quaternion that creates a tilt aligned with the body frame
-		// when rotated by the yaw setpoint
-		// To do so, solve q_yaw * q_sp_rp = q_sp_yaw * q_sp_rp_compensated
-		Quatf q_sp_rp_compensated = q_sp_yaw.inversed() * q_yaw * q_sp_rp;
-
-		// Extract the corrected tilt as we still want to include the yaw setpoint
-		Vector3f att_sp_z = q_sp_rp_compensated.dcm_z();
-		q_sp_rp = Quatf(z_unit, att_sp_z);
+		AttitudeControlMath::correctTiltSetpointForYawError(q_sp_rp, q, q_sp_yaw);
 	}
 
 	// Align the desired tilt with the yaw setpoint