fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 06:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Figure_of_eight: Make configuration dependent on defined mavlink_message_id.

Browse Source
This commit is contained in:
Konrad
2023-10-26 16:50:29 +02:00
committed by Daniel Agar
parent e3473a0f90
commit 1089079a32
5 changed files with 51 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+18 -20
View File
@@ -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)
{
src/modules/fw_pos_control/FixedwingPositionControl.hpp
+20 -27
View File
@@ -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 @@ 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)};
#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 @@ 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_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 @@ 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<float> _airspeed_slew_rate_controller;
src/modules/fw_pos_control/Kconfig
+1 -1
View File
@@ -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.
src/modules/mavlink/mavlink_main.cpp
+10 -10
View File
@@ -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;
src/modules/mavlink/mavlink_messages.cpp
+2 -2
View File
@@ -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"