FlightTask: Weather vane cleanup

Remove the entire external yaw handler, dynamic memory allocation,
pointer passing logic. Directly instanciate the weather vane instance
in the flight tasks that support it.

src/lib/weather_vane/WeatherVane.cpp

@@ -41,24 +41,45 @@
 #include <mathlib/mathlib.h>
 
 
-WeatherVane::WeatherVane() :
-	ModuleParams(nullptr)
+WeatherVane::WeatherVane(ModuleParams *parent) :
+	ModuleParams(parent)
 { }
 
-void WeatherVane::update(const matrix::Vector3f &dcm_z_sp_prev, float yaw)
+void WeatherVane::update()
 {
-	_dcm_z_sp_prev = dcm_z_sp_prev;
-	_yaw = yaw;
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+		_is_vtol = vehicle_status.is_vtol;
+	}
+
+	vehicle_control_mode_s vehicle_control_mode;
+
+	if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) {
+		_flag_control_manual_enabled = vehicle_control_mode.flag_control_manual_enabled;
+		_flag_control_position_enabled = vehicle_control_mode.flag_control_position_enabled;
+	}
+
+	// Weathervane is only enabled for VTOLs if it's enabled by parameter
+	// in manual we use weathervane just if position is controlled as well
+	// in mission we use weathervane except for when navigator disables it
+	_is_active = _is_vtol && _param_wv_en.get()
+		     && ((_flag_control_manual_enabled && _flag_control_position_enabled)
+			 || (!_flag_control_manual_enabled && !_navigator_force_disabled));
 }
 
-float WeatherVane::get_weathervane_yawrate()
+float WeatherVane::getWeathervaneYawrate()
 {
 	// direction of desired body z axis represented in earth frame
-	matrix::Vector3f body_z_sp(_dcm_z_sp_prev);
+	vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
+	_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint);
+	matrix::Vector3f body_z_sp(matrix::Quatf(vehicle_attitude_setpoint.q_d).dcm_z()); // attitude setpoint body z axis
 
 	// 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.
-	matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -_yaw);
+	vehicle_local_position_s vehicle_local_position{};
+	_vehicle_local_position_sub.copy(&vehicle_local_position);
+	matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -vehicle_local_position.heading);
 	body_z_sp = R_yaw * body_z_sp;
 	body_z_sp.normalize();
 

src/lib/weather_vane/WeatherVane.hpp

@@ -44,33 +44,40 @@
 
 #include <px4_platform_common/module_params.h>
 #include <matrix/matrix/math.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
 
 class WeatherVane : public ModuleParams
 {
 public:
-	WeatherVane();
+	WeatherVane(ModuleParams *parent);
 
 	~WeatherVane() = default;
 
-	void activate() {_is_active = true;}
+	void setNavigatorForceDisabled(bool navigator_force_disabled) { _navigator_force_disabled = navigator_force_disabled; };
 
-	void deactivate() {_is_active = false;}
+	bool isActive() {return _is_active;}
 
-	bool is_active() {return _is_active;}
+	void update();
 
-	bool weathervane_enabled() { return _param_wv_en.get(); }
-
-	void update(const matrix::Vector3f &dcm_z_sp_prev, float yaw);
-
-	float get_weathervane_yawrate();
-
-	void update_parameters() { ModuleParams::updateParams(); }
+	float getWeathervaneYawrate();
 
 private:
-	matrix::Vector3f _dcm_z_sp_prev; ///< previous attitude setpoint body z axis
-	float _yaw = 0.0f; ///< current yaw angle
+	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	bool _is_active = true;
+	bool _is_active{false};
+
+	// local copies of status such that we don't need to copy uORB messages all the time
+	bool _is_vtol{false};
+	bool _flag_control_manual_enabled{false};
+	bool _flag_control_position_enabled{false};
+	bool _navigator_force_disabled{false};
 
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::WV_EN>) _param_wv_en,