fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 03:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

flight_task: added boilerplate code for dead-reckon rtl

Browse Source
This commit is contained in:
oravla5
2025-05-05 17:02:06 +02:00
parent f7ffe27d4c
commit d86c7eea04
7 changed files with 31 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/versioned/VehicleStatus.msg
+1 -1
View File
@@ -40,7 +40,7 @@ uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode
uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode
uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode
uint8 NAVIGATION_STATE_POSITION_SLOW = 6
uint8 NAVIGATION_STATE_FREE5 = 7
uint8 NAVIGATION_STATE_AUTO_RTL_DR = 7 # Auto dead-reckoning return to launch mode
uint8 NAVIGATION_STATE_FREE4 = 8
uint8 NAVIGATION_STATE_FREE3 = 9
uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode
src/lib/events/enums.json
+4
View File
@@ -604,6 +604,10 @@
"name": "external8",
"description": "External 8"
},
"24": {
"name": "auto_rtl_dr",
"description": "Dead-Reckoning RTL"
},
"255": {
"name": "unknown",
"description": "[Unknown]"
src/lib/modes/ui.hpp
+2 -1
View File
@@ -52,6 +52,7 @@ static inline uint32_t getValidNavStates()
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) |
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) |
(1u << vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) |
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR) |
(1u << vehicle_status_s::NAVIGATION_STATE_ACRO) |
(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION) |
(1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) |
@@ -74,7 +75,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
"Hold",
"Return",
"Position Slow",
"7: unallocated",
"Dead-Reckoning Return",
"8: unallocated",
"9: unallocated",
"Acro",
src/modules/commander/ModeUtil/control_mode.cpp
+1
View File
@@ -106,6 +106,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
vehicle_control_mode.flag_control_allocation_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR:
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
vehicle_control_mode.flag_control_auto_enabled = true;
vehicle_control_mode.flag_control_climb_rate_enabled = true;
src/modules/commander/ModeUtil/mode_requirements.cpp
+6
View File
@@ -133,6 +133,12 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_home_position);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_prevent_arming);
// NAVIGATION_STATE_AUTO_RTL_DR
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR, flags.mode_req_angular_velocity);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR, flags.mode_req_attitude);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR, flags.mode_req_local_alt);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR, flags.mode_req_prevent_arming);
// NAVIGATION_STATE_ACRO
setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_angular_velocity);
setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_manual_control);
src/modules/commander/px4_custom_mode.h
+6
View File
@@ -73,6 +73,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_EXTERNAL6,
PX4_CUSTOM_SUB_MODE_EXTERNAL7,
PX4_CUSTOM_SUB_MODE_EXTERNAL8,
PX4_CUSTOM_SUB_MODE_AUTO_RTL_DR
};
enum PX4_CUSTOM_SUB_MODE_POSCTL {
@@ -226,6 +227,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL8;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL_DR;
break;
}
return custom_mode;
src/modules/flight_mode_manager/FlightModeManager.cpp
+11 -1
View File
@@ -152,7 +152,9 @@ void FlightModeManager::start_flight_task()
bool found_some_task = false;
bool matching_task_running = true;
bool task_failure = false;
const bool nav_state_descend = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
const bool nav_state_rtl_dr = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR);
// Follow me
if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
@@ -187,7 +189,8 @@ void FlightModeManager::start_flight_task()
// Navigator interface for autonomous modes
if (_vehicle_control_mode_sub.get().flag_control_auto_enabled
&& !nav_state_descend) {
&& !nav_state_descend
&& !nav_state_rtl_dr) {
found_some_task = true;
if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) {
@@ -249,6 +252,13 @@ void FlightModeManager::start_flight_task()
matching_task_running = matching_task_running && !task_failure;
}
// Dead-Reckoning Return
if (nav_state_rtl_dr) {
found_some_task = true;
PX4_INFO("Going into Dead-Reckoning Return mode");
task_failure = false;
}
// Emergency descend
if (nav_state_descend || task_failure) {
found_some_task = true;