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

kconfig: Add option to enable figure of eight support

Browse Source
This commit is contained in:
Konrad 2023-07-14 13:49:04 +02:00 committed by Daniel Agar
parent e5e66370e7
commit 8edd7ce2c1
10 changed files with 88 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/px4/sitl/default.px4board
View File

@ -18,6 +18,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL=y
CONFIG_FIGURE_OF_EIGHT=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y

6
src/modules/commander/Commander.cpp
View File

@ -1116,6 +1116,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
break;
case vehicle_command_s::VEHICLE_CMD_DO_FIGUREEIGHT: {
#ifdef CONFIG_FIGURE_OF_EIGHT
if (!((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || (_vehicle_status.is_vtol))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
@ -1142,6 +1143,11 @@ Commander::handle_command(const vehicle_command_s &cmd)
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
mavlink_log_critical(&_mavlink_log_pub, "Figure 8 command rejected, Only available in fixed wing mode.");
}
#else
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
mavlink_log_critical(&_mavlink_log_pub, "Figure 8 command not supported.");
#endif // CONFIG_FIGURE_OF_EIGHT
}
break;

28
src/modules/fw_pos_control/CMakeLists.txt
View File

@ -33,7 +33,25 @@
add_subdirectory(launchdetection)
add_subdirectory(runway_takeoff)
add_subdirectory(figure_eight)
set(POSCONTROL_DEPENDENCIES
launchdetection
npfg
runway_takeoff
SlewRate
tecs
motion_planning
)
if(CONFIG_FIGURE_OF_EIGHT)
add_subdirectory(figure_eight)
set(POSCONTROL_DEPENDENCIES
${POSCONTROL_DEPENDENCIES}
figure_eight
)
endif()
px4_add_module(
MODULE modules__fw_pos_control
@ -42,11 +60,5 @@ px4_add_module(
FixedwingPositionControl.cpp
FixedwingPositionControl.hpp
DEPENDS
figure_eight
launchdetection
npfg
runway_takeoff
SlewRate
tecs
motion_planning
${POSCONTROL_DEPENDENCIES}
)

21
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -53,7 +53,9 @@ 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)
{
@ -896,10 +898,14 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER
|| current_sp.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
#ifdef CONFIG_FIGURE_OF_EIGHT
if (current_sp.loiter_pattern == position_setpoint_s::LOITER_TYPE_FIGUREEIGHT) {
publishFigureEightStatus(current_sp);
} else {
} else
#endif // CONFIG_FIGURE_OF_EIGHT
{
publishOrbitStatus(current_sp);
}
}
@ -920,10 +926,13 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
break;
case position_setpoint_s::SETPOINT_TYPE_LOITER:
#ifdef CONFIG_FIGURE_OF_EIGHT
if (current_sp.loiter_pattern == position_setpoint_s::LOITER_TYPE_FIGUREEIGHT) {
controlAutoFigureEight(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
} else {
} else
#endif // CONFIG_FIGURE_OF_EIGHT
{
control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
}
@ -931,6 +940,8 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
break;
}
#ifdef CONFIG_FIGURE_OF_EIGHT
/* reset loiter state */
if ((position_sp_type != position_setpoint_s::SETPOINT_TYPE_LOITER) ||
((position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER) &&
@ -938,6 +949,8 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
_figure_eight.resetPattern();
}
#endif // CONFIG_FIGURE_OF_EIGHT
/* Copy thrust output for publication, handle special cases */
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
@ -1307,6 +1320,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
_param_climbrate_target.get());
}
#ifdef CONFIG_FIGURE_OF_EIGHT
void
FixedwingPositionControl::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)
@ -1363,6 +1377,7 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
// Yaw
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
}
#endif // CONFIG_FIGURE_OF_EIGHT
void
FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
@ -2911,6 +2926,7 @@ 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{};
@ -2925,6 +2941,7 @@ void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_
_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)

19
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -47,7 +47,6 @@
#ifndef FIXEDWINGPOSITIONCONTROL_HPP_
#define FIXEDWINGPOSITIONCONTROL_HPP_
#include "figure_eight/FigureEight.hpp"
#include "launchdetection/LaunchDetector.h"
#include "runway_takeoff/RunwayTakeoff.h"
@ -95,9 +94,15 @@
#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"
#endif // CONFIG_FIGURE_OF_EIGHT
using namespace launchdetection;
using namespace runwaytakeoff;
using namespace time_literals;
@ -215,8 +220,10 @@ 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)};
#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)};
@ -274,8 +281,10 @@ 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};
@ -590,6 +599,7 @@ 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.
*
@ -601,6 +611,7 @@ private:
*/
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.
@ -718,7 +729,9 @@ 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;

8
src/modules/fw_pos_control/Kconfig
View File

@ -10,3 +10,11 @@ menuconfig USER_FW_POS_CONTROL
depends on BOARD_PROTECTED && MODULES_FW_POS_CONTROL
---help---
Put fw_pos_control in userspace memory
menuconfig FIGURE_OF_EIGHT
bool "fw_pos_control figure of eight loiter support"
default n
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.

4
src/modules/fw_pos_control/figure_eight/FigureEight.cpp
View File

@ -325,6 +325,7 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat
const float dist_to_center = vector_center_to_vehicle.norm();
Vector2f unit_vec_center_to_closest_pt = vector_center_to_vehicle.normalized();
if (dist_to_center < 0.1f) {
// the logic breaks down at the circle center, employ some mitigation strategies
// until we exit this region
@ -372,7 +373,8 @@ void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
const Vector2f vector_A_to_vehicle = curr_pos_local - line_segment_start_position;
_closest_point_on_path = line_segment_start_position + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
_npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), line_segment_start_position, 0.0f);
_npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), line_segment_start_position,
0.0f);
_roll_setpoint = _npfg.getRollSetpoint();
_indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas;
}

10
src/modules/mavlink/mavlink_main.cpp
View File

@ -1444,7 +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
configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
#endif // CONFIG_FIGURE_OF_EIGHT
#endif // !CONSTRAINED_FLASH
break;
@ -1510,7 +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
configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
#endif // CONFIG_FIGURE_OF_EIGHT
#endif // !CONSTRAINED_FLASH
break;
@ -1572,7 +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
configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 2.0f);
#endif // CONFIG_FIGURE_OF_EIGHT
#endif // !CONSTRAINED_FLASH
break;
@ -1666,7 +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
configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
#endif // CONFIG_FIGURE_OF_EIGHT
#endif // !CONSTRAINED_FLASH
break;
@ -1744,7 +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
configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
#endif // CONFIG_FIGURE_OF_EIGHT
#endif // !CONSTRAINED_FLASH
break;

2
src/modules/mavlink/mavlink_messages.cpp
View File

@ -120,7 +120,9 @@
#include "streams/VFR_HUD.hpp"
#include "streams/VIBRATION.hpp"
#include "streams/WIND_COV.hpp"
#ifdef CONFIG_FIGURE_OF_EIGHT
#include "streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp"
#endif // CONFIG_FIGURE_OF_EIGHT
#if !defined(CONSTRAINED_FLASH)
# include "streams/ADSB_VEHICLE.hpp"

3
src/modules/navigator/navigator_main.cpp
View File

@ -556,6 +556,7 @@ void Navigator::run()
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_FIGUREEIGHT &&
get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
#ifdef CONFIG_FIGURE_OF_EIGHT
// Only valid for fixed wing mode
bool orbit_location_valid = true;
@ -606,6 +607,8 @@ void Navigator::run()
mavlink_log_critical(&_mavlink_log_pub, "Figure 8 is outside geofence");
}
#endif // CONFIG_FIGURE_OF_EIGHT
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
position_setpoint_triplet_s *rep = get_takeoff_triplet();