diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp
index 969af4f33b..2647ca9ff8 100644
--- a/src/modules/flight_mode_manager/FlightModeManager.cpp
+++ b/src/modules/flight_mode_manager/FlightModeManager.cpp
@@ -34,6 +34,7 @@
 #include "FlightModeManager.hpp"
 
 #include <lib/mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
 
 using namespace time_literals;
 
@@ -106,24 +107,25 @@ void FlightModeManager::Run()
 		_takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed, false,
 					    10.f, !_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled, time_stamp_now);
 
-		// // activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
-		// // that turns the nose of the vehicle into the wind
-		// if (_wv_controller != nullptr) {
+		// activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
+		// that turns the nose of the vehicle into the wind
+		if (_wv_controller != nullptr) {
 
-		// 	// in manual mode we just want to use weathervane if position is controlled as well TODO
-		// 	// in mission, enabling wv is done in flight task
-		// 	if (_control_mode.flag_control_manual_enabled) {
-		// 		if (_control_mode.flag_control_position_enabled && _wv_controller->weathervane_enabled()) {
-		// 			_wv_controller->activate();
+			// in manual mode we just want to use weathervane if position is controlled as well
+			// in mission, enabling wv is done in flight task
+			if (_vehicle_control_mode_sub.get().flag_control_manual_enabled) {
+				if (_vehicle_control_mode_sub.get().flag_control_position_enabled && _wv_controller->weathervane_enabled()) {
+					_wv_controller->activate();
 
-		// 		} else {
-		// 			_wv_controller->deactivate();
-		// 		}
-		// 	}
+				} else {
+					_wv_controller->deactivate();
+				}
+			}
 
-		// 	_wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z(); // TODO
-		// 	_wv_controller->update(_wv_dcm_z_sp_prev, _states.yaw);
-		// }
+			vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
+			_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint);
+			_wv_controller->update(matrix::Quatf(vehicle_attitude_setpoint.q_d).dcm_z(), vehicle_local_position.heading);
+		}
 
 		start_flight_task();
 
diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp
index 0352196d16..7810d69595 100644
--- a/src/modules/flight_mode_manager/FlightModeManager.hpp
+++ b/src/modules/flight_mode_manager/FlightModeManager.hpp
@@ -42,6 +42,7 @@
 
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -92,6 +93,7 @@ private:
 
 	uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::SubscriptionData<vehicle_control_mode_s> _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::SubscriptionData<vehicle_land_detected_s> _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};