WeatherVane: only update with last row of rotation matrix

src/lib/WeatherVane/WeatherVane.cpp

@@ -43,20 +43,18 @@
 
 WeatherVane::WeatherVane() :
 	ModuleParams(nullptr)
-{
-	_R_sp_prev = matrix::Dcmf();
-}
+{ }
 
-void WeatherVane::update(const matrix::Quatf &q_sp_prev, float yaw)
+void WeatherVane::update(const matrix::Vector3f &dcm_z_sp_prev, float yaw)
 {
-	_R_sp_prev = matrix::Dcmf(q_sp_prev);
+	_dcm_z_sp_prev = dcm_z_sp_prev;
 	_yaw = yaw;
 }
 
 float WeatherVane::get_weathervane_yawrate()
 {
 	// direction of desired body z axis represented in earth frame
-	matrix::Vector3f body_z_sp(_R_sp_prev(0, 2), _R_sp_prev(1, 2), _R_sp_prev(2, 2));
+	matrix::Vector3f body_z_sp(_dcm_z_sp_prev);
 
 	// 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.
@@ -74,7 +72,6 @@ float WeatherVane::get_weathervane_yawrate()
 
 	} else if (roll_sp < -min_roll_rad) {
 		roll_exceeding_treshold = roll_sp + min_roll_rad;
-
 	}
 
 	return math::constrain(roll_exceeding_treshold * _param_wv_gain.get(), -math::radians(_param_wv_yrate_max.get()),

src/lib/WeatherVane/WeatherVane.hpp

@@ -60,15 +60,15 @@ public:
 
 	bool weathervane_enabled() { return _param_wv_en.get(); }
 
-	void update(const matrix::Quatf &q_sp_prev, float yaw);
+	void update(const matrix::Vector3f &dcm_z_sp_prev, float yaw);
 
 	float get_weathervane_yawrate();
 
 	void update_parameters() { ModuleParams::updateParams(); }
 
 private:
-	matrix::Dcmf _R_sp_prev;	// previous attitude setpoint rotation matrix
-	float _yaw = 0.0f;			// current yaw angle
+	matrix::Vector3f _dcm_z_sp_prev; ///< previous attitude setpoint body z axis
+	float _yaw = 0.0f; ///< current yaw angle
 
 	bool _is_active = true;
 

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -555,7 +555,7 @@ MulticopterPositionControl::Run()
 				}
 			}
 
-			_wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw);
+			_wv_controller->update(Quatf(_att_sp.q_d).dcm_z(), _states.yaw);
 		}
 
 		// an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes