diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index 11776402d6..54506565ad 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -48,7 +48,10 @@
 class FlightTasks
 {
 public:
-	FlightTasks() {};
+	FlightTasks(control::SuperBlock *parent) :
+		Manual(parent, "MAN"),
+		Orbit(parent, "ORB")
+	{};
 	~FlightTasks() {};
 
 	/**
diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp
index 81e579ea77..4c4ea97023 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.hpp
@@ -41,10 +41,11 @@
 
 #pragma once
 
-class FlightTask
+class FlightTask : public control::SuperBlock
 {
 public:
-	FlightTask()
+	FlightTask(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name)
 	{
 		_vehicle_position = nullptr;
 		_manual_control_setpoint = nullptr;
@@ -78,6 +79,7 @@ public:
 		_time = hrt_elapsed_time(&_starting_time_stamp) / 1e6f;
 		_deltatime  = math::min(hrt_elapsed_time(&_last_time_stamp) / 1e6f, (float)_timeout);
 		_last_time_stamp = hrt_absolute_time();
+		updateSubscriptions();
 		_evaluate_sticks();
 		_evaluate_position();
 		_evaluate_velocity();
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
index 37834ef7ec..c0f9976681 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
@@ -47,13 +47,15 @@
 class FlightTaskManual : public FlightTask
 {
 public:
-	FlightTaskManual() :
+	FlightTaskManual(SuperBlock *parent, const char *name) :
+		FlightTask(parent, name),
 		_xy_vel_man_expo(nullptr, "MPC_XY_MAN_EXPO"),
 		_z_vel_man_expo(nullptr, "MPC_Z_MAN_EXPO"),
 		_hold_dz(nullptr, "MPC_HOLD_DZ"),
 		_velocity_hor_manual(nullptr, "MPC_VEL_MANUAL"),
 		_z_vel_max_up(nullptr, "MPC_Z_VEL_MAX_UP"),
-		_z_vel_max_down(nullptr, "MPC_Z_VEL_MAX_DN")
+		_z_vel_max_down(nullptr, "MPC_Z_VEL_MAX_DN"),
+		_sub_control_state(ORB_ID(control_state), 0, 0, &getSubscriptions())
 	{};
 	virtual ~FlightTaskManual() {};
 
@@ -98,12 +100,8 @@ public:
 			man_vel_sp(1) /= man_vel_hor_length;
 		}
 
-		/* prepare yaw to rotate into NED frame */
-		float yaw_input_fame = 0;
-		//float yaw_input_fame = _yaw;
-
-		/* setpoint in NED frame */
-		man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, yaw_input_fame)) * man_vel_sp;
+		/* rotate setpoint to be in NED frame */
+		man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, get_input_frame_yaw())) * man_vel_sp;
 
 		/* scale smaller than unit length vector to maximal manual speed (m/s) */
 		matrix::Vector3f vel_scale(_velocity_hor_manual.get(),
@@ -116,6 +114,14 @@ public:
 		return 0;
 	};
 
+protected:
+	float get_input_frame_yaw()
+	{
+		matrix::Quatf q(&_sub_control_state.get().q[0]);
+		matrix::Eulerf euler_angles(q);
+		return euler_angles(2);
+	};
+
 private:
 	control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */
 	control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */
@@ -124,4 +130,6 @@ private:
 	control::BlockParamFloat _z_vel_max_up; /**< maximal vertical velocity when flying upwards with the stick */
 	control::BlockParamFloat _z_vel_max_down; /**< maximal vertical velocity when flying downwards with the stick */
 
+	uORB::Subscription<control_state_s> _sub_control_state;
+
 };
diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
index 3315265304..b3e17ca4aa 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
@@ -46,7 +46,9 @@
 class FlightTaskOrbit : public FlightTask
 {
 public:
-	FlightTaskOrbit() {};
+	FlightTaskOrbit(SuperBlock *parent, const char *name) :
+		FlightTask(parent, name)
+	{};
 	virtual ~FlightTaskOrbit() {};
 
 	/**