fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: add position slow mode

Browse Source
This commit is contained in:
Matthias Grob 2023-09-19 17:53:45 +02:00
parent dbbf585adb
commit ef0926d64b
11 changed files with 50 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
msg/VehicleStatus.msg
View File

@ -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

12
src/drivers/osd/msp_osd/uorb_to_msp.cpp
View File

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

4
src/lib/events/enums.json
View File

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

5
src/lib/modes/ui.hpp
View File

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

17
src/modules/commander/Commander.cpp
View File

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

1
src/modules/commander/ModeUtil/control_mode.cpp
View File

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

4
src/modules/commander/ModeUtil/conversions.hpp
View File

@ -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;
}

8
src/modules/commander/ModeUtil/mode_requirements.cpp
View File

@ -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

1
src/modules/commander/module.yaml
View File

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

8
src/modules/commander/px4_custom_mode.h
View File

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

2
src/modules/manual_control/ManualControl.cpp
View File

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