diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 6190346da4..4c711b9763 100644 --- a/msg/VehicleStatus.msg +++ b/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_UNUSED = 9 # Free slot +uint8 NAVIGATION_STATE_POSITION_SLOW = 6 +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 diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index e2cf9daf5e..b66661c114 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/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; diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index af0969b87f..fafe8c6e9b 100644 --- a/src/lib/events/enums.json +++ b/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" diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp index c75531e666..9e4156ae00 100644 --- a/src/lib/modes/ui.hpp +++ b/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", diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e0a270e835..e7ea1a2100 100644 --- a/src/modules/commander/Commander.cpp +++ b/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"); diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index 4d069b2c0c..fbebc7b93d 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/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; diff --git a/src/modules/commander/ModeUtil/conversions.hpp b/src/modules/commander/ModeUtil/conversions.hpp index 5997b97adc..045db4570f 100644 --- a/src/modules/commander/ModeUtil/conversions.hpp +++ b/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; } diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index f067275ea6..0718804df2 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/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 diff --git a/src/modules/commander/module.yaml b/src/modules/commander/module.yaml index 9e2cf1a0b4..d2113d9359 100644 --- a/src/modules/commander/module.yaml +++ b/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 diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 3d350e77de..de028bcef0 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/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; diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index ecdcade3d4..7633a9d163 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/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;