diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index 73a939c716..9c6e058dee 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -365,6 +365,12 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 	_thr_sp.copyTo(local_position_setpoint.thrust);
 }
 
+void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint)
+{
+	attitude_setpoint = ControlMath::thrustToAttitude(_thr_sp, _yaw_sp);
+	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
+}
+
 void PositionControl::updateParams()
 {
 	ModuleParams::updateParams();
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
index 17d92445da..8af6bbd4a9 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018 - 2019 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
@@ -39,6 +39,7 @@
 
 #include <matrix/matrix/math.hpp>
 
+#include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_constraints.h>
@@ -128,13 +129,6 @@ public:
 	 */
 	void resetIntegralZ() { _thr_int(2) = 0.0f; }
 
-	/**
-	 * 	Get the
-	 * 	@see _yawspeed_sp
-	 * 	@return The yawspeed set-point member.
-	 */
-	const float &getYawspeedSetpoint() { return _yawspeed_sp; }
-
 	/**
 	 * 	Get the
 	 * 	@see _vel_sp
@@ -164,6 +158,14 @@ public:
 	 */
 	void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint);
 
+	/**
+	 * Get the controllers output attitude setpoint
+	 * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control.
+	 * It needs to be executed by the attitude controller to achieve velocity and position tracking.
+	 * @param attitude_setpoint reference to struct to fill up
+	 */
+	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint);
+
 protected:
 
 	void updateParams() override;
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index a8ea009207..be6e55fa54 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -68,7 +68,6 @@
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 
 #include <PositionControl.hpp>
-#include <ControlMath.hpp>
 #include "Takeoff.hpp"
 
 #include <float.h>
@@ -689,12 +688,10 @@ MulticopterPositionControl::Run()
 				limit_thrust_during_landing(local_pos_sp);
 			}
 
-			// Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint.
 			vehicle_attitude_setpoint_s attitude_setpoint{};
-			attitude_setpoint = ControlMath::thrustToAttitude(matrix::Vector3f(local_pos_sp.thrust), local_pos_sp.yaw);
-			attitude_setpoint.yaw_sp_move_rate = _control.getYawspeedSetpoint();
-			attitude_setpoint.fw_control_yaw = false;
-			attitude_setpoint.apply_flaps = false;
+			attitude_setpoint.timestamp = hrt_absolute_time();
+			_control.getAttitudeSetpoint(attitude_setpoint);
+
 
 			// publish attitude setpoint
 			// Note: this requires review. The reason for not sending