Weathervane: pass quaterionon as constant reference

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

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;

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();
 

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;