diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
index 7bc9c6120f..495954430d 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
@@ -44,9 +44,8 @@
 using namespace matrix;
 namespace ControlMath
 {
-vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp)
+void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
 {
-	vehicle_attitude_setpoint_s att_sp = {};
 	att_sp.yaw_body = yaw_sp;
 
 	// desired body_z axis = -normalize(thrust_vector)
@@ -103,8 +102,6 @@ vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float
 	att_sp.roll_body = euler(0);
 	att_sp.pitch_body = euler(1);
 	att_sp.thrust_body[2] = -thr_sp.length();
-
-	return att_sp;
 }
 
 Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
index 47390004b1..abc0e98234 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
@@ -51,7 +51,8 @@ namespace ControlMath
  * @param yaw_sp the desired yaw
  * @return vehicle_attitude_setpoints_s structure
  */
-vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp);
+void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp,
+		      vehicle_attitude_setpoint_s &attitude_setpoint);
 
 /**
  * Outputs the sum of two vectors but respecting the limits and priority.
diff --git a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
index 0594d15bb5..daf009159b 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
@@ -46,7 +46,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	Vector3f thr{0.0f, 0.0f, -1.0f};
 	float yaw = 0.0f;
 	vehicle_attitude_setpoint_s att{};
-	att = thrustToAttitude(thr, yaw);
+	thrustToAttitude(thr, yaw, att);
 	EXPECT_EQ(att.roll_body, 0);
 	EXPECT_EQ(att.pitch_body, 0);
 	EXPECT_EQ(att.yaw_body, 0);
@@ -55,7 +55,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	/* expected: same as before but with 90 yaw
 	 * reason: only yaw changed */
 	yaw = M_PI_2_F;
-	att = thrustToAttitude(thr, yaw);
+	thrustToAttitude(thr, yaw, att);
 	EXPECT_EQ(att.roll_body, 0);
 	EXPECT_EQ(att.pitch_body, 0);
 	EXPECT_EQ(att.yaw_body, M_PI_2_F);
@@ -65,7 +65,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	 * reason: thrust points straight down and order Euler
 	 * order is: 1. roll, 2. pitch, 3. yaw */
 	thr = Vector3f(0.0f, 0.0f, 1.0f);
-	att = thrustToAttitude(thr, yaw);
+	thrustToAttitude(thr, yaw, att);
 	EXPECT_NEAR(att.roll_body, -M_PI_F, 1e-4);
 	EXPECT_EQ(att.pitch_body, 0);
 	EXPECT_EQ(att.yaw_body, M_PI_2_F);
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index c75c0f703f..3db656e877 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -367,7 +367,7 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 
 void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
 {
-	attitude_setpoint = ControlMath::thrustToAttitude(_thr_sp, _yaw_sp);
+	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
 }