mc_pos_control: use weathervane library to make vehicle turn into relative wind

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

src/lib/FlightTasks/FlightTasks.hpp

@@ -44,6 +44,7 @@
 #include "FlightTask.hpp"
 #include "SubscriptionArray.hpp"
 #include "FlightTasks_generated.hpp"
+#include <lib/WeatherVane/WeatherVane.hpp>
 
 #include <new>
 
@@ -125,6 +126,11 @@ public:
 	 */
 	const char *errorToString(const int error);
 
+	/**
+	 * Sets an external yaw handler. The active flight task can use the yaw handler to implement a different yaw control strategy.
+	 */
+	void set_yaw_handler(WeatherVane *ext_yaw_handler) {_current_task.task->set_yaw_handler(ext_yaw_handler);}
+
 private:
 
 	/**

src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -62,6 +62,12 @@ bool FlightTaskAutoMapper::update()
 		_reset();
 	}
 
+	// check if an external yaw handler is active and if yes, let it compute the yaw setpoints
+	if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
+		_yaw_setpoint = _yaw;
+		_yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate();
+	}
+
 	// The only time a thrust set-point is sent out is during
 	// idle. Hence, reset thrust set-point to NAN in case the
 	// vehicle exits idle.

src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -50,6 +50,11 @@ public:
 	bool activate() override;
 	bool update() override;
 
+	/**
+	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+	 */
+	void set_yaw_handler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
+
 protected:
 
 	float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
@@ -73,6 +78,9 @@ protected:
 	void updateParams() override; /**< See ModuleParam class */
 
 private:
+	WeatherVane *_ext_yaw_handler =
+		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
 	void _reset(); /**< Resets member variables to current vehicle state */
 	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
 	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -51,6 +51,7 @@
 #include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <lib/WeatherVane/WeatherVane.hpp>
 #include "SubscriptionArray.hpp"
 
 class FlightTask : public ModuleParams
@@ -140,6 +141,12 @@ 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 set_yaw_handler(WeatherVane *ext_yaw_handler) {};
+
 protected:
 
 	uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};

src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp

@@ -88,6 +88,12 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
 			}
 		}
 	}
+
+	// check if an external yaw handler is active and if yes, let it compute the yaw setpoints
+	if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
+		_yaw_setpoint = NAN;
+		_yawspeed_setpoint += _ext_yaw_handler->get_weathervane_yawrate();
+	}
 }
 
 void FlightTaskManualStabilized::_updateThrustSetpoints()

src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp

@@ -52,6 +52,11 @@ public:
 	bool updateInitialize() override;
 	bool update() override;
 
+	/**
+	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+	 */
+	void set_yaw_handler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
+
 protected:
 	virtual void _updateSetpoints(); /**< updates all setpoints*/
 	virtual void _scaleSticks(); /**< scales sticks to yaw and thrust */
@@ -64,6 +69,9 @@ private:
 
 	float _throttle{}; /** mapped from stick z */
 
+	WeatherVane *_ext_yaw_handler =
+		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,
 					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */
 					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _tilt_max_man, /**< maximum tilt allowed for manual flight */

src/lib/WeatherVane/CMakeLists.txt

@@ -31,4 +31,4 @@
 #
 ############################################################################
 
-px4_add_library(WeatherVane WeatherVane.cpp)
+px4_add_library(WeatherVane WeatherVane.cpp)

src/lib/WeatherVane/WeatherVane.cpp

@@ -79,4 +79,4 @@ float WeatherVane::get_weathervane_yawrate()
 
 	return math::constrain(roll_exceeding_treshold * _wv_gain, -_wv_yawrate_max_rad,
 			       _wv_yawrate_max_rad);
-}
+}

src/lib/WeatherVane/WeatherVane.hpp

@@ -77,4 +77,4 @@ private:
 	float _wv_min_roll_rad = 0.01f;		// minimum roll angle setpoint for the controller to output a non-zero yawrate setpoint
 	float _wv_yawrate_max_rad = 1.0f;	// maximum yaw-rate the controller will output
 
-};
+};