Figure_of_eight: Make configuration dependent on defined mavlink_mess…

…age_id.
PX4 · Oct 31, 2023 · 1089079 · 1089079
1 parent e3473a0
commit 1089079
Show file tree

Hide file tree

Showing 5 changed files with 51 additions and 60 deletions.
diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
@@ -53,11 +53,11 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
 	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
-#ifdef CONFIG_FIGURE_OF_EIGHT
-	_figure_eight(_npfg, _wind_vel, _eas2tas),
-#endif // CONFIG_FIGURE_OF_EIGHT
 	_launchDetector(this),
 	_runway_takeoff(this)
+#ifdef CONFIG_FIGURE_OF_EIGHT
+	, _figure_eight(_npfg, _wind_vel, _eas2tas)
+#endif // CONFIG_FIGURE_OF_EIGHT
 {
 	if (vtol) {
 		_param_handle_airspeed_trans = param_find("VT_ARSP_TRANS");
@@ -1377,6 +1377,21 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
 	// Yaw
 	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 }
+
+void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp)
+{
+	figure_eight_status_s figure_eight_status{};
+	figure_eight_status.timestamp = hrt_absolute_time();
+	figure_eight_status.major_radius = pos_sp.loiter_radius * (pos_sp.loiter_direction_counter_clockwise ? -1.f : 1.f);
+	figure_eight_status.minor_radius = pos_sp.loiter_minor_radius;
+	figure_eight_status.orientation = pos_sp.loiter_orientation;
+	figure_eight_status.frame = 5; //MAV_FRAME_GLOBAL_INT
+	figure_eight_status.x = static_cast<int32_t>(pos_sp.lat * 1e7);
+	figure_eight_status.y = static_cast<int32_t>(pos_sp.lon * 1e7);
+	figure_eight_status.z = pos_sp.alt;
+
+	_figure_eight_status_pub.publish(figure_eight_status);
+}
 #endif // CONFIG_FIGURE_OF_EIGHT
 
 void
@@ -2926,23 +2941,6 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_
 	_orbit_status_pub.publish(orbit_status);
 }
 
-#ifdef CONFIG_FIGURE_OF_EIGHT
-void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp)
-{
-	figure_eight_status_s figure_eight_status{};
-	figure_eight_status.timestamp = hrt_absolute_time();
-	figure_eight_status.major_radius = pos_sp.loiter_radius * (pos_sp.loiter_direction_counter_clockwise ? -1.f : 1.f);
-	figure_eight_status.minor_radius = pos_sp.loiter_minor_radius;
-	figure_eight_status.orientation = pos_sp.loiter_orientation;
-	figure_eight_status.frame = 5; //MAV_FRAME_GLOBAL_INT
-	figure_eight_status.x = static_cast<int32_t>(pos_sp.lat * 1e7);
-	figure_eight_status.y = static_cast<int32_t>(pos_sp.lon * 1e7);
-	figure_eight_status.z = pos_sp.alt;
-
-	_figure_eight_status_pub.publish(figure_eight_status);
-}
-#endif // CONFIG_FIGURE_OF_EIGHT
-
 void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, const Vector2f &end_waypoint,
 		const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
 {

diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp
@@ -94,13 +94,11 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>
 #include <uORB/topics/orbit_status.h>
-#ifdef CONFIG_FIGURE_OF_EIGHT
-#include <uORB/topics/figure_eight_status.h>
-#endif // CONFIG_FIGURE_OF_EIGHT
 #include <uORB/uORB.h>
 
 #ifdef CONFIG_FIGURE_OF_EIGHT
 #include "figure_eight/FigureEight.hpp"
+#include <uORB/topics/figure_eight_status.h>
 #endif // CONFIG_FIGURE_OF_EIGHT
 
 using namespace launchdetection;
@@ -220,9 +218,6 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	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)};
-#ifdef CONFIG_FIGURE_OF_EIGHT
-	uORB::Publication<figure_eight_status_s> _figure_eight_status_pub {ORB_ID(figure_eight_status)};
-#endif // CONFIG_FIGURE_OF_EIGHT
 	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)};
@@ -281,11 +276,6 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 
 	bool _landed{true};
 
-#ifdef CONFIG_FIGURE_OF_EIGHT
-	/* Loitering */
-	FigureEight _figure_eight;
-#endif // CONFIG_FIGURE_OF_EIGHT
-
 	// indicates whether the plane was in the air in the previous interation
 	bool _was_in_air{false};
 
@@ -444,6 +434,25 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	float _min_current_sp_distance_xy{FLT_MAX};
 	float _target_bearing{0.0f}; // [rad]
 
+#ifdef CONFIG_FIGURE_OF_EIGHT
+	/* Loitering */
+	FigureEight _figure_eight;
+	uORB::Publication<figure_eight_status_s> _figure_eight_status_pub {ORB_ID(figure_eight_status)};
+	/**
+	 * 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);
+
+	void publishFigureEightStatus(const position_setpoint_s pos_sp);
+#endif // CONFIG_FIGURE_OF_EIGHT
+
 	// Update our local parameter cache.
 	int parameters_update();
 
@@ -599,19 +608,6 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	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);
 
-#ifdef CONFIG_FIGURE_OF_EIGHT
-	/**
-	 * 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);
-#endif // CONFIG_FIGURE_OF_EIGHT
 
 	/**
 	 * @brief Controls a desired airspeed, bearing, and height rate.
@@ -729,9 +725,6 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 				    float airspeed_sp);
 
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
-#ifdef CONFIG_FIGURE_OF_EIGHT
-	void publishFigureEightStatus(const position_setpoint_s pos_sp);
-#endif // CONFIG_FIGURE_OF_EIGHT
 
 	SlewRate<float> _airspeed_slew_rate_controller;
 

diff --git a/src/modules/fw_pos_control/Kconfig b/src/modules/fw_pos_control/Kconfig
@@ -17,4 +17,4 @@ menuconfig FIGURE_OF_EIGHT
 	depends on MODULES_FW_POS_CONTROL
 	---help---
 		Enable support for the figure of eight loitering pattern in fixed wing.
-		NOTE: this needs the development mavlink dialect.
+		NOTE: Enable Mavlink development support to get feedback message.
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
@@ -1444,9 +1444,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("DEBUG_VECT", 1.0f);
 		configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
 		configure_stream_local("LINK_NODE_STATUS", 1.0f);
-#ifdef CONFIG_FIGURE_OF_EIGHT
+#if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS)
 		configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
-#endif // CONFIG_FIGURE_OF_EIGHT
+#endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS
 #endif // !CONSTRAINED_FLASH
 
 		break;
@@ -1512,9 +1512,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("DEBUG_VECT", 10.0f);
 		configure_stream_local("NAMED_VALUE_FLOAT", 10.0f);
 		configure_stream_local("LINK_NODE_STATUS", 1.0f);
-#ifdef CONFIG_FIGURE_OF_EIGHT
+#if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS)
 		configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
-#endif // CONFIG_FIGURE_OF_EIGHT
+#endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS
 #endif // !CONSTRAINED_FLASH
 
 		break;
@@ -1576,9 +1576,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("DEBUG_VECT", 1.0f);
 		configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
 		configure_stream_local("LINK_NODE_STATUS", 1.0f);
-#ifdef CONFIG_FIGURE_OF_EIGHT
+#if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS)
 		configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 2.0f);
-#endif // CONFIG_FIGURE_OF_EIGHT
+#endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS
 #endif // !CONSTRAINED_FLASH
 
 		break;
@@ -1672,9 +1672,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("DEBUG_VECT", 50.0f);
 		configure_stream_local("NAMED_VALUE_FLOAT", 50.0f);
 		configure_stream_local("LINK_NODE_STATUS", 1.0f);
-#ifdef CONFIG_FIGURE_OF_EIGHT
+#if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS)
 		configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
-#endif // CONFIG_FIGURE_OF_EIGHT
+#endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS
 #endif // !CONSTRAINED_FLASH
 
 		break;
@@ -1752,9 +1752,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f);
 		configure_stream_local("DEBUG_VECT", 1.0f);
 		configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
-#ifdef CONFIG_FIGURE_OF_EIGHT
+#if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS)
 		configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
-#endif // CONFIG_FIGURE_OF_EIGHT
+#endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS
 #endif // !CONSTRAINED_FLASH
 		break;
 

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
@@ -120,9 +120,9 @@
 #include "streams/VFR_HUD.hpp"
 #include "streams/VIBRATION.hpp"
 #include "streams/WIND_COV.hpp"
-#ifdef CONFIG_FIGURE_OF_EIGHT
+#if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS)
 #include "streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp"
-#endif // CONFIG_FIGURE_OF_EIGHT
+#endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS
 
 #if !defined(CONSTRAINED_FLASH)
 # include "streams/ADSB_VEHICLE.hpp"