FixedwingPositionControl: Add support for figure 8 loitering.

The command is sent by a dedicated mavlink command and forwarded to the fixed wing position controller.

The pattern is defined by the radius of the major axis, the radius of the minor axis and the orientation. The pattern is then defined by:
The upper part of the pattern consist of a clockwise circle with radius defined by the minor axis. The center of the circle is defined by the major axis minus the minor axis away from the pattern center.
The lower part of the pattern consist of a counter-clockwise circle with the same definitions as above.
In between, the circles are connected with straight lines in a cross configuration. The lines are always tangetial to the circles.
The orientation rotates the major axis around the NED down axis.

The loitering logic is defined inside its own class used by the fixed wing position control module. It defines which segment (one of the circles or lines) is active and uses the path controller (npfg or l1-control) to determine the desired roll angle.

A feedback mavlink message is send with the executed pattern parameters.

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -47,6 +47,7 @@
 #ifndef FIXEDWINGPOSITIONCONTROL_HPP_
 #define FIXEDWINGPOSITIONCONTROL_HPP_
 
+#include "figure_eight/FigureEight.hpp"
 #include "launchdetection/LaunchDetector.h"
 #include "runway_takeoff/RunwayTakeoff.h"
 
@@ -94,6 +95,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>
 #include <uORB/topics/orbit_status.h>
+#include <uORB/topics/figure_eight_status.h>
 #include <uORB/uORB.h>
 
 using namespace launchdetection;
@@ -213,6 +215,7 @@ private:
 	uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
 	uORB::Publication<launch_detection_status_s> _launch_detection_status_pub{ORB_ID(launch_detection_status)};
 	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
+	uORB::Publication<figure_eight_status_s> _figure_eight_status_pub{ORB_ID(figure_eight_status)};
 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
 	uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
@@ -271,6 +274,9 @@ private:
 
 	bool _landed{true};
 
+	/* Loitering */
+	FigureEight _figure_eight;
+
 	// indicates whether the plane was in the air in the previous interation
 	bool _was_in_air{false};
 
@@ -584,6 +590,18 @@ private:
 	void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
 				 const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
 
+	/**
+	 * Vehicle control for the autonomous figure 8 mode.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos the current 2D absolute position of the vehicle in [deg].
+	 * @param ground_speed the 2D ground speed of the vehicle in [m/s].
+	 * @param pos_sp_prev the previous position setpoint.
+	 * @param pos_sp_curr the current position setpoint.
+	 */
+	void controlAutoFigureEight(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
+				    const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+
 	/**
 	 * @brief Controls a desired airspeed, bearing, and height rate.
 	 *
@@ -700,6 +718,7 @@ private:
 				    float airspeed_sp);
 
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
+	void publishFigureEightStatus(const position_setpoint_s pos_sp);
 
 	SlewRate<float> _airspeed_slew_rate_controller;