diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index f433d564e9..856e1a80e3 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -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 diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 3b73cff62c..717fd0bf7c 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -604,6 +604,10 @@ "name": "external8", "description": "External 8" }, + "24": { + "name": "auto_rtl_dr", + "description": "Dead-Reckoning RTL" + }, "255": { "name": "unknown", "description": "[Unknown]" diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp index 9e4156ae00..014fad4c87 100644 --- a/src/lib/modes/ui.hpp +++ b/src/lib/modes/ui.hpp @@ -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", diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index fbebc7b93d..8a2003bc3b 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -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; diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 5c6fa30295..a29d54fd02 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -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); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e4936982ff..2d7c0a8342 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -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; diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index f17e00aa96..686c59e88f 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -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;