diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
index 47c9faa8ee..1955b44155 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
@@ -41,6 +41,19 @@
 
 using namespace matrix;
 
+bool FlightTaskManualStabilized::initializeSubscriptions(SubscriptionArray &subscription_array)
+{
+	if (!FlightTaskManual::initializeSubscriptions(subscription_array)) {
+		return false;
+	}
+
+	if (!subscription_array.get(ORB_ID(vehicle_attitude), _sub_attitude)) {
+		return false;
+	}
+
+	return true;
+}
+
 bool FlightTaskManualStabilized::activate()
 {
 	bool ret = FlightTaskManual::activate();
@@ -70,6 +83,13 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
 		// hold the current heading when no more rotation commanded
 		if (!PX4_ISFINITE(_yaw_setpoint)) {
 			_yaw_setpoint = _yaw;
+		} else {
+			// check reset counter and update yaw setpoint if necessary
+			if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) {
+				_yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi();
+				_heading_reset_counter = _sub_attitude->get().quat_reset_counter;
+			}
+
 		}
 	}
 }
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
index b56f5a1413..47b71af5f1 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
@@ -41,6 +41,7 @@
 #pragma once
 
 #include "FlightTaskManual.hpp"
+#include <uORB/topics/vehicle_attitude.h>
 
 class FlightTaskManualStabilized : public FlightTaskManual
 {
@@ -48,10 +49,10 @@ public:
 	FlightTaskManualStabilized() = default;
 
 	virtual ~FlightTaskManualStabilized() = default;
-
 	bool activate() override;
-
 	bool update() override;
+	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
+
 
 protected:
 	virtual void _updateSetpoints(); /**< updates all setpoints*/
@@ -59,6 +60,10 @@ protected:
 	void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /**< rotates vector into local frame */
 
 private:
+	uint8_t _heading_reset_counter = 0; /**< estimator heading reset */
+
+	uORB::Subscription<vehicle_attitude_s> *_sub_attitude{nullptr};
+
 
 	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
 	void _updateThrustSetpoints(); /**< sets thrust setpoint */