diff --git a/src/lib/WeatherVane/WeatherVane.cpp b/src/lib/WeatherVane/WeatherVane.cpp
index 9e9856ddd201c73c1cb1862b1e2919199454bb2a..a3be265f435ff54232f46d3637a281222360fb71 100644
--- a/src/lib/WeatherVane/WeatherVane.cpp
+++ b/src/lib/WeatherVane/WeatherVane.cpp
@@ -47,7 +47,7 @@ WeatherVane::WeatherVane() :
 	_R_sp_prev = matrix::Dcmf();
 }
 
-void WeatherVane::update(matrix::Quatf q_sp_prev, float yaw)
+void WeatherVane::update(const matrix::Quatf &q_sp_prev, float yaw)
 {
 	_R_sp_prev = matrix::Dcmf(q_sp_prev);
 	_yaw = yaw;
diff --git a/src/lib/WeatherVane/WeatherVane.hpp b/src/lib/WeatherVane/WeatherVane.hpp
index 7a43557f7ac44f157bc30b942f705b0b12176c67..dfa49e6850346b5a966b885abccfc9c3940f25e8 100644
--- a/src/lib/WeatherVane/WeatherVane.hpp
+++ b/src/lib/WeatherVane/WeatherVane.hpp
@@ -62,7 +62,7 @@ public:
 
 	bool auto_enabled() { return _wv_auto_enabled.get(); }
 
-	void update(matrix::Quatf q_sp_prev, float yaw);
+	void update(const matrix::Quatf &q_sp_prev, float yaw);
 
 	float get_weathervane_yawrate();
 
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 aa72c731e4ba5b005115642057f1bf7335859291..b1c0b5a4b8c22c7be0e2e6aa1930aaeb2cd49e26 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -379,7 +379,9 @@ MulticopterPositionControl::parameters_update(bool force)
 		// set trigger time for arm hysteresis
 		_arm_hysteresis.set_hysteresis_time_from(false, (int)(MPC_IDLE_TKO.get() * 1000000.0f));
 
-		_wv_controller->update_parameters();
+		if (_wv_controller != nullptr) {
+			_wv_controller->update_parameters();
+		}
 	}
 
 	return OK;
@@ -603,15 +605,19 @@ MulticopterPositionControl::task_main()
 
 		// 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 (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled
-		    && _wv_controller->manual_enabled()) {
-			_wv_controller->activate();
+		if (_wv_controller != nullptr) {
+			if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled
+			    && _wv_controller->manual_enabled()) {
+				_wv_controller->activate();
 
-		} else if (_control_mode.flag_control_auto_enabled && _wv_controller->auto_enabled()) {
-			_wv_controller->activate();
+			} else if (_control_mode.flag_control_auto_enabled && _wv_controller->auto_enabled()) {
+				_wv_controller->activate();
 
-		} else {
-			_wv_controller->deactivate();
+			} else {
+				_wv_controller->deactivate();
+			}
+
+			_wv_controller->update(matrix::Quatf(_att_sp.q_d), _local_pos.yaw);
 		}
 
 		if (_control_mode.flag_armed) {
@@ -750,8 +756,6 @@ MulticopterPositionControl::task_main()
 			_att_sp.disable_mc_yaw_control = false;
 			_att_sp.apply_flaps = false;
 
-			_wv_controller->update(matrix::Quatf(_att_sp.q_d), _local_pos.yaw);
-
 			if (!constraints.landing_gear) {
 				if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) {
 					_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP;