WeatherVane: calculate rotation matrix directly when updating

Signed-off-by: Roman <bapstroman@gmail.com>

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.

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;

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) {