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
-
-	bool _is_active = true;
+	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{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,

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -60,7 +60,6 @@ FlightModeManager::~FlightModeManager()
 		_current_task.task->~FlightTask();
 	}
 
-	delete _wv_controller;
 	perf_free(_loop_perf);
 }
 
@@ -107,33 +106,7 @@ void FlightModeManager::Run()
 		_home_position_sub.update();
 		_vehicle_control_mode_sub.update();
 		_vehicle_land_detected_sub.update();
-
-		if (_vehicle_status_sub.update()) {
-			if (_vehicle_status_sub.get().is_vtol && (_wv_controller == nullptr)) {
-				// if vehicle is a VTOL we want to enable weathervane capabilities
-				_wv_controller = new WeatherVane();
-			}
-		}
-
-		// 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 (_wv_controller != nullptr) {
-
-			// in manual mode we just want to use weathervane if position is controlled as well
-			// in mission, enabling wv is done in flight task
-			if (_vehicle_control_mode_sub.get().flag_control_manual_enabled) {
-				if (_vehicle_control_mode_sub.get().flag_control_position_enabled && _wv_controller->weathervane_enabled()) {
-					_wv_controller->activate();
-
-				} else {
-					_wv_controller->deactivate();
-				}
-			}
-
-			vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
-			_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint);
-			_wv_controller->update(matrix::Quatf(vehicle_attitude_setpoint.q_d).dcm_z(), vehicle_local_position.heading);
-		}
+		_vehicle_status_sub.update();
 
 		start_flight_task();
 
@@ -157,10 +130,6 @@ void FlightModeManager::updateParams()
 	if (isAnyTaskActive()) {
 		_current_task.task->handleParameterUpdate();
 	}
-
-	if (_wv_controller != nullptr) {
-		_wv_controller->update_parameters();
-	}
 }
 
 void FlightModeManager::start_flight_task()
@@ -461,8 +430,6 @@ void FlightModeManager::handleCommand()
 void FlightModeManager::generateTrajectorySetpoint(const float dt,
 		const vehicle_local_position_s &vehicle_local_position)
 {
-	_current_task.task->setYawHandler(_wv_controller);
-
 	// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
 	trajectory_setpoint_s setpoint = FlightTask::empty_setpoint;
 	vehicle_constraints_s constraints = FlightTask::empty_constraints;

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -131,7 +131,6 @@ private:
 		FlightTaskIndex index{FlightTaskIndex::None};
 	} _current_task{};
 
-	WeatherVane *_wv_controller{nullptr};
 	int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
 	uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_UNINITIALIZED};
 	int _task_failure_count{0};

src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt

@@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto
 	FlightTaskAuto.cpp
 )
 
-target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility)
+target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility WeatherVane)
 target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -40,14 +40,6 @@
 
 using namespace matrix;
 
-FlightTaskAuto::FlightTaskAuto() :
-	_obstacle_avoidance(this),
-	_sticks(this),
-	_stick_acceleration_xy(this)
-{
-
-}
-
 bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
@@ -257,9 +249,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
 		vertical_speed *= (1 + _sticks.getPositionExpo()(2));
 
 		// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
-		const bool weather_vane_active = (_ext_yaw_handler != nullptr) && _ext_yaw_handler->is_active();
-
-		if (!weather_vane_active || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) {
+		if (!_weathervane.isActive() || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) {
 			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
 						       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
 		}
@@ -455,11 +445,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_next_was_valid = _sub_triplet_setpoint.get().next.valid;
 	}
 
-	if (_ext_yaw_handler != nullptr) {
-		// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
-		(_param_wv_en.get() && !_sub_triplet_setpoint.get().current.disable_weather_vane) ?	_ext_yaw_handler->activate() :
-		_ext_yaw_handler->deactivate();
-	}
+	// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
+	_weathervane.setNavigatorForceDisabled(_sub_triplet_setpoint.get().current.disable_weather_vane);
 
 	// Calculate the current vehicle state and check if it has updated.
 	State previous_state = _current_state;
@@ -476,13 +463,15 @@ bool FlightTaskAuto::_evaluateTriplets()
 				_triplet_next_wp,
 				_sub_triplet_setpoint.get().next.yaw,
 				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
-				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
+				_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
 		_obstacle_avoidance.checkAvoidanceProgress(
 			_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
 	}
 
 	// set heading
-	if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
+	_weathervane.update();
+
+	if (_weathervane.isActive()) {
 		_yaw_setpoint = NAN;
 		// use the yawrate setpoint from WV only if not moving lateral (velocity setpoint below half of _param_mpc_xy_cruise)
 		// otherwise, keep heading constant (as output from WV is not according to wind in this case)
@@ -492,7 +481,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_yawspeed_setpoint = 0.0f;
 
 		} else {
-			_yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate();
+			_yawspeed_setpoint = _weathervane.getWeathervaneYawrate();
 		}
 
 

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -46,6 +46,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <lib/geo/geo.h>
+#include <lib/weather_vane/WeatherVane.hpp>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <lib/motion_planning/PositionSmoothing.hpp>
 #include "Sticks.hpp"
@@ -84,18 +85,13 @@ enum class State {
 class FlightTaskAuto : public FlightTask
 {
 public:
-	FlightTaskAuto();
-
+	FlightTaskAuto() = default;
 	virtual ~FlightTaskAuto() = default;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	void reActivate() override;
 	bool updateInitialize() override;
 	bool update() override;
 
-	/**
-	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
-	 */
-	void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
 	void overrideCruiseSpeed(const float cruise_speed_m_s) override;
 
 protected:
@@ -143,12 +139,12 @@ protected:
 	AlphaFilter<float> _yawspeed_filter;
 	bool _yaw_sp_aligned{false};
 
-	ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */
+	ObstacleAvoidance _obstacle_avoidance{this}; /**< class adjusting setpoints according to external avoidance module's input */
 
 	PositionSmoothing _position_smoothing;
 	Vector3f _unsmoothed_velocity_setpoint;
-	Sticks _sticks;
-	StickAccelerationXY _stick_acceleration_xy;
+	Sticks _sticks{this};
+	StickAccelerationXY _stick_acceleration_xy{this};
 	StickYaw _stick_yaw;
 	matrix::Vector3f _land_position;
 	float _land_heading;
@@ -164,7 +160,6 @@ protected:
 					(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
 					(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
 					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
-					(ParamBool<px4::params::WV_EN>) _param_wv_en, // enable/disable weather vane (VTOL)
 					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
@@ -208,8 +203,7 @@ private:
 	float _reference_altitude{NAN}; /**< Altitude relative to ground. */
 	hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
 
-	WeatherVane *_ext_yaw_handler{nullptr};	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
-
+	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 
 	void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */
 	bool _evaluateTriplets(); /**< Checks and sets triplets. */

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -55,7 +55,6 @@
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include <uORB/topics/home_position.h>
 #include <lib/geo/geo.h>
-#include <lib/weather_vane/WeatherVane.hpp>
 
 struct ekf_reset_counters_s {
 	uint8_t xy;
@@ -163,11 +162,6 @@ public:
 		updateParams();
 	}
 
-	/**
-	 * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy.
-	 * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method.
-	 */
-	virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}
 	virtual void overrideCruiseSpeed(const float cruise_speed_m_s) {}
 
 	void updateVelocityControllerFeedback(const matrix::Vector3f &vel_sp,

src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt

@@ -36,4 +36,4 @@ px4_add_library(FlightTaskManualAcceleration
 )
 target_include_directories(FlightTaskManualAcceleration PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManualAltitudeSmoothVel FlightTaskUtility)
+target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManualAltitudeSmoothVel FlightTaskUtility WeatherVane)

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -39,10 +39,6 @@
 
 using namespace matrix;
 
-FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
-	_stick_acceleration_xy(this)
-{};
-
 bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
@@ -78,13 +74,15 @@ bool FlightTaskManualAcceleration::update()
 	_constraints.want_takeoff = _checkTakeoff();
 
 	// check if an external yaw handler is active and if yes, let it update the yaw setpoints
-	if (_weathervane_yaw_handler && _weathervane_yaw_handler->is_active()) {
+	_weathervane.update();
+
+	if (_weathervane.isActive()) {
 		_yaw_setpoint = NAN;
 
 		// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
 		if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
 			// vehicle is steady
-			_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
+			_yawspeed_setpoint += _weathervane.getWeathervaneYawrate();
 		}
 	}
 

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -43,26 +43,22 @@
 #include "FlightTaskManualAltitudeSmoothVel.hpp"
 #include "StickAccelerationXY.hpp"
 #include "StickYaw.hpp"
+#include <lib/weather_vane/WeatherVane.hpp>
 
 class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
 {
 public:
-	FlightTaskManualAcceleration();
+	FlightTaskManualAcceleration() = default;
 	virtual ~FlightTaskManualAcceleration() = default;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool update() override;
 
-	/**
-	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
-	 */
-	void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
-
 private:
 	void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override;
 	void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
 
-	StickAccelerationXY _stick_acceleration_xy;
+	StickAccelerationXY _stick_acceleration_xy{this};
 	StickYaw _stick_yaw;
 
-	WeatherVane *_weathervane_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 };

src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt

@@ -39,6 +39,7 @@ px4_add_library(FlightTaskManualPosition
 target_link_libraries(FlightTaskManualPosition
 	PRIVATE
 		CollisionPrevention
+		WeatherVane
 	PUBLIC
 		FlightTaskManualAltitude
 )

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -41,11 +41,6 @@
 
 using namespace matrix;
 
-FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this)
-{
-
-}
-
 bool FlightTaskManualPosition::updateInitialize()
 {
 	bool ret = FlightTaskManualAltitude::updateInitialize();
@@ -141,14 +136,15 @@ void FlightTaskManualPosition::_updateSetpoints()
 
 	_updateXYlock(); // check for position lock
 
-	// check if an external yaw handler is active and if yes, let it update the yaw setpoints
-	if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) {
+	_weathervane.update();
+
+	if (_weathervane.isActive()) {
 		_yaw_setpoint = NAN;
 
 		// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
 		if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
 			// vehicle is steady
-			_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
+			_yawspeed_setpoint += _weathervane.getWeathervaneYawrate();
 		}
 	}
 }

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -41,23 +41,17 @@
 #pragma once
 
 #include <lib/collision_prevention/CollisionPrevention.hpp>
+#include <lib/weather_vane/WeatherVane.hpp>
 #include "FlightTaskManualAltitude.hpp"
 
 class FlightTaskManualPosition : public FlightTaskManualAltitude
 {
 public:
-	FlightTaskManualPosition();
-
+	FlightTaskManualPosition() = default;
 	virtual ~FlightTaskManualPosition() = default;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 
-	/**
-	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
-	 */
-	void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
-
-
 protected:
 	void _updateXYlock(); /**< applies position lock based on stick and velocity */
 	void _updateSetpoints() override;
@@ -71,8 +65,6 @@ protected:
 private:
 	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
 
-	WeatherVane *_weathervane_yaw_handler =
-		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
-
-	CollisionPrevention _collision_prevention;	/**< collision avoidance setpoint amendment */
+	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+	CollisionPrevention _collision_prevention{this}; /**< collision avoidance setpoint amendment */
 };

src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp

@@ -50,7 +50,6 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
 {
 public:
 	FlightTaskManualPositionSmoothVel() = default;
-
 	virtual ~FlightTaskManualPositionSmoothVel() = default;
 
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;