diff --git a/src/lib/WeatherVane/WeatherVane.cpp b/src/lib/WeatherVane/WeatherVane.cpp
index e6bdc66774884f2b4956eb0119c65de56a23d013..f5b2343d6820aa88ccaa530930e082b3334b1d8a 100644
--- a/src/lib/WeatherVane/WeatherVane.cpp
+++ b/src/lib/WeatherVane/WeatherVane.cpp
@@ -44,21 +44,19 @@
 WeatherVane::WeatherVane() :
 	ModuleParams(nullptr)
 {
-	_q_sp_prev = matrix::Quatf();
+	_R_sp_prev = matrix::Dcmf();
 }
 
 void WeatherVane::update(matrix::Quatf q_sp_prev, float yaw)
 {
-	_q_sp_prev = q_sp_prev;
+	_R_sp_prev = matrix::Dcmf(q_sp_prev);
 	_yaw = yaw;
 }
 
 float WeatherVane::get_weathervane_yawrate()
 {
-	matrix::Dcmf R_sp(_q_sp_prev);
-
 	// direction of desired body z axis represented in earth frame
-	matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+	matrix::Vector3f body_z_sp(_R_sp_prev(0, 2), _R_sp_prev(1, 2), _R_sp_prev(2, 2));
 
 	// rotate desired body z axis into new frame which is rotated in z by the current
 	// heading of the vehicle. we refer to this as the heading frame.
diff --git a/src/lib/WeatherVane/WeatherVane.hpp b/src/lib/WeatherVane/WeatherVane.hpp
index d732f6474921a0afa7417b1cf291881366659548..7a43557f7ac44f157bc30b942f705b0b12176c67 100644
--- a/src/lib/WeatherVane/WeatherVane.hpp
+++ b/src/lib/WeatherVane/WeatherVane.hpp
@@ -69,7 +69,7 @@ public:
 	void update_parameters() { ModuleParams::updateParams(); }
 
 private:
-	matrix::Quatf _q_sp_prev;	// previous attitude setpoint quaternion
+	matrix::Dcmf _R_sp_prev;	// previous attitude setpoint rotation matrix
 	float _yaw = 0.0f;			// current yaw angle
 
 	bool _is_active = true;
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 17a33d66ea8b723c23763e9ab969895eff2f6955..92559939236c27d41542cc84b6a5d246f4033f65 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -741,7 +741,7 @@ MulticopterPositionControl::task_main()
 			_att_sp.disable_mc_yaw_control = false;
 			_att_sp.apply_flaps = false;
 
-			_wv_controller.update(matrix::Quatf(&_att_sp.q_d[0]), _local_pos.yaw);
+			_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) {