diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp
index f0de43d21a..f5836722f6 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.hpp
@@ -102,32 +102,17 @@ protected:
 	/**
 	 * Put the position vector produced by the task into the setpoint message
 	 */
-	void _set_position_setpoint(const matrix::Vector3f &position_setpoint)
-	{
-		_vehicle_local_position_setpoint.x = position_setpoint(0);
-		_vehicle_local_position_setpoint.y = position_setpoint(1);
-		_vehicle_local_position_setpoint.z = position_setpoint(2);
-	}
+	void _set_position_setpoint(const matrix::Vector3f &position_setpoint) { position_setpoint.copyToRaw(&_vehicle_local_position_setpoint.x); }
 
 	/**
 	 * Put the velocity vector produced by the task into the setpoint message
 	 */
-	void _set_velocity_setpoint(const matrix::Vector3f &velocity_setpoint)
-	{
-		_vehicle_local_position_setpoint.vx = velocity_setpoint(0);
-		_vehicle_local_position_setpoint.vy = velocity_setpoint(1);
-		_vehicle_local_position_setpoint.vz = velocity_setpoint(2);
-	}
+	void _set_velocity_setpoint(const matrix::Vector3f &velocity_setpoint) { velocity_setpoint.copyToRaw(&_vehicle_local_position_setpoint.vx); }
 
 	/**
 	 * Put the acceleration vector produced by the task into the setpoint message
 	 */
-	void _set_acceleration_setpoint(const matrix::Vector3f &acceleration_setpoint)
-	{
-		_vehicle_local_position_setpoint.acc_x = acceleration_setpoint(0);
-		_vehicle_local_position_setpoint.acc_y = acceleration_setpoint(1);
-		_vehicle_local_position_setpoint.acc_z = acceleration_setpoint(2);
-	}
+	void _set_acceleration_setpoint(const matrix::Vector3f &acceleration_setpoint) { acceleration_setpoint.copyToRaw(&_vehicle_local_position_setpoint.acc_x); }
 
 	/**
 	 * Put the yaw angle produced by the task into the setpoint message