Commander: add position slow mode

msg/VehicleStatus.msg

@@ -37,15 +37,17 @@ uint8 NAVIGATION_STATE_POSCTL = 2               # Position control mode
 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_UNUSED3 = 8              # Free slot
+uint8 NAVIGATION_STATE_POSITION_SLOW = 6
-uint8 NAVIGATION_STATE_UNUSED = 9               # Free slot
+uint8 NAVIGATION_STATE_FREE5 = 7
+uint8 NAVIGATION_STATE_FREE4 = 8
+uint8 NAVIGATION_STATE_FREE3 = 9
 uint8 NAVIGATION_STATE_ACRO = 10                # Acro mode
-uint8 NAVIGATION_STATE_UNUSED1 = 11             # Free slot
+uint8 NAVIGATION_STATE_FREE2 = 11
 uint8 NAVIGATION_STATE_DESCEND = 12             # Descend mode (no position control)
 uint8 NAVIGATION_STATE_TERMINATION = 13         # Termination mode
 uint8 NAVIGATION_STATE_OFFBOARD = 14
 uint8 NAVIGATION_STATE_STAB = 15                # Stabilized mode
-uint8 NAVIGATION_STATE_UNUSED2 = 16             # Free slot
+uint8 NAVIGATION_STATE_FREE1 = 16
 uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17        # Takeoff
 uint8 NAVIGATION_STATE_AUTO_LAND = 18           # Land
 uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow

src/drivers/osd/msp_osd/uorb_to_msp.cpp

@@ -102,18 +102,10 @@ msp_name_t construct_display_message(const vehicle_status_s &vehicle_status,
 			display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_RTL");
 			break;
 
-		case vehicle_status_s::NAVIGATION_STATE_UNUSED:
-			display.set(MessageDisplayType::FLIGHT_MODE, "UNUSED");
-			break;
-
 		case vehicle_status_s::NAVIGATION_STATE_ACRO:
 			display.set(MessageDisplayType::FLIGHT_MODE, "ACRO");
 			break;
 
-		case vehicle_status_s::NAVIGATION_STATE_UNUSED1:
-			display.set(MessageDisplayType::FLIGHT_MODE, "UNUSED1");
-			break;
-
 		case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 			display.set(MessageDisplayType::FLIGHT_MODE, "DESCEND");
 			break;
@@ -130,10 +122,6 @@ msp_name_t construct_display_message(const vehicle_status_s &vehicle_status,
 			display.set(MessageDisplayType::FLIGHT_MODE, "STAB");
 			break;
 
-		case vehicle_status_s::NAVIGATION_STATE_UNUSED2:
-			display.set(MessageDisplayType::FLIGHT_MODE, "UNUSED2");
-			break;
-
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
 			display.set(MessageDisplayType::FLIGHT_MODE, "AUTO_TAKEOFF");
 			break;

src/lib/events/enums.json

@@ -540,6 +540,10 @@
                             "name": "stab",
                             "description": "Stabilized"
                         },
+                        "9": {
+                            "name": "position_slow",
+                            "description": "Position Slow"
+                        },
                         "10": {
                             "name": "auto_takeoff",
                             "description": "Takeoff"

src/lib/modes/ui.hpp

@@ -51,6 +51,7 @@ static inline uint32_t getValidNavStates()
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) |
 	       (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_ACRO) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) |
@@ -62,7 +63,7 @@ static inline uint32_t getValidNavStates()
 	       (1u << vehicle_status_s::NAVIGATION_STATE_ORBIT) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF);
 
-	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 31, "code requires update");
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 31, "update valid nav states");
 }
 
 const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
@@ -72,7 +73,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
 	"Mission",
 	"Hold",
 	"Return",
-	"6: unallocated",
+	"Position Slow",
 	"7: unallocated",
 	"8: unallocated",
 	"9: unallocated",

src/modules/commander/Commander.cpp

@@ -376,6 +376,10 @@ int Commander::custom_command(int argc, char *argv[])
 			} else if (!strcmp(argv[1], "posctl")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL);
 
+			} else if (!strcmp(argv[1], "position:slow")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL,
+						     PX4_CUSTOM_SUB_MODE_POSCTL_SLOW);
+
 			} else if (!strcmp(argv[1], "auto:mission")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_MISSION);
@@ -780,7 +784,16 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					desired_nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
-					desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+					switch (custom_sub_mode) {
+					default:
+					case PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL:
+						desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+						break;
+
+					case PX4_CUSTOM_SUB_MODE_POSCTL_SLOW:
+						desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW;
+						break;
+					}
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					if (custom_sub_mode > 0) {
@@ -2969,7 +2982,7 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_COMMAND("land");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
-	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
+	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|position:slow|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
 			"Flight mode", false);
 	PRINT_MODULE_USAGE_COMMAND("pair");
 	PRINT_MODULE_USAGE_COMMAND("lockdown");

src/modules/commander/ModeUtil/control_mode.cpp

@@ -72,6 +72,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
+	case vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW:
 		vehicle_control_mode.flag_control_manual_enabled = true;
 		vehicle_control_mode.flag_control_position_enabled = true;
 		vehicle_control_mode.flag_control_velocity_enabled = true;

src/modules/commander/ModeUtil/conversions.hpp

@@ -58,6 +58,8 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl;
 
+	case vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW: return navigation_mode_t::position_slow;
+
 	case vehicle_status_s::NAVIGATION_STATE_ACRO: return navigation_mode_t::acro;
 
 	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: return navigation_mode_t::offboard;
@@ -93,7 +95,7 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
 	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return navigation_mode_t::external8;
 	}
 
-	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 31, "code requires update");
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update navigation mode map");
 
 	return navigation_mode_t::unknown;
 }

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -75,8 +75,16 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_position_relaxed);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_manual_control);
 
+	// NAVIGATION_STATE_POSITION_SLOW
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_angular_velocity);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_attitude);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_local_alt);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_local_position_relaxed);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_manual_control);
+
 	if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 		setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position);
+		setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_global_position);
 	}
 
 	// NAVIGATION_STATE_AUTO_MISSION

src/modules/commander/module.yaml

@@ -27,6 +27,7 @@ parameters:
                 0: Manual
                 1: Altitude
                 2: Position
+                9: Position Slow
                 3: Mission
                 4: Hold
                 10: Takeoff

src/modules/commander/px4_custom_mode.h

@@ -77,7 +77,8 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 
 enum PX4_CUSTOM_SUB_MODE_POSCTL {
 	PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL = 0,
-	PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT
+	PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT,
+	PX4_CUSTOM_SUB_MODE_POSCTL_SLOW
 };
 
 union px4_custom_mode {
@@ -115,6 +116,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_POSCTL_SLOW;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;

src/modules/manual_control/ManualControl.cpp

@@ -553,6 +553,7 @@ Module consuming manual_control_inputs publishing one manual_control_setpoint.
 
 int8_t ManualControl::navStateFromParam(int32_t param_value)
 {
+	// See src/modules/commander/module.yaml COM_FLTMODE${i}
 	switch(param_value) {
 		case 0: return vehicle_status_s::NAVIGATION_STATE_MANUAL;
 		case 1: return vehicle_status_s::NAVIGATION_STATE_ALTCTL;
@@ -563,6 +564,7 @@ int8_t ManualControl::navStateFromParam(int32_t param_value)
 		case 6: return vehicle_status_s::NAVIGATION_STATE_ACRO;
 		case 7: return vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
 		case 8: return vehicle_status_s::NAVIGATION_STATE_STAB;
+		case 9: return vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW;
 		case 10: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
 		case 11: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 		case 12: return vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;