diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 3211ef69de..ed233e1e0f 100644 --- 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(pos_sp.lat * 1e7); + figure_eight_status.y = static_cast(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(pos_sp.lat * 1e7); - figure_eight_status.y = static_cast(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 index 004e90ea05..5d448b213f 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -94,13 +94,11 @@ #include #include #include -#ifdef CONFIG_FIGURE_OF_EIGHT -#include -#endif // CONFIG_FIGURE_OF_EIGHT #include #ifdef CONFIG_FIGURE_OF_EIGHT #include "figure_eight/FigureEight.hpp" +#include #endif // CONFIG_FIGURE_OF_EIGHT using namespace launchdetection; @@ -220,9 +218,6 @@ private: uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; uORB::Publication _launch_detection_status_pub{ORB_ID(launch_detection_status)}; uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; -#ifdef CONFIG_FIGURE_OF_EIGHT - uORB::Publication _figure_eight_status_pub {ORB_ID(figure_eight_status)}; -#endif // CONFIG_FIGURE_OF_EIGHT uORB::Publication _landing_gear_pub {ORB_ID(landing_gear)}; uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; @@ -281,11 +276,6 @@ private: 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 @@ private: 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_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 @@ 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); -#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 @@ private: 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 _airspeed_slew_rate_controller; diff --git a/src/modules/fw_pos_control/Kconfig b/src/modules/fw_pos_control/Kconfig index 128b117b7c..d0bffb8dd8 100644 --- 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 index 606f76e2be..7a7d57bb12 100644 --- 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 index e9112083d1..53d9dda834 100644 --- 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"