flight_task: added boilerplate code for dead-reckon rtl

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

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]"

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",

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;

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);

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;

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;