From 889b6a1a78d541a98308fe7d021c2c8816a1885b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 5 Dec 2022 14:47:55 +0100 Subject: [PATCH] commander: add VEHICLE_CMD_SET_NAV_STATE internal command --- msg/VehicleCommand.msg | 1 + src/modules/commander/Commander.cpp | 12 ++++++++++++ 2 files changed, 13 insertions(+) diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 80043d5deb..3c8d778c12 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -103,6 +103,7 @@ uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch. # PX4 vehicle commands (beyond 16 bit mavlink commands) uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX) uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude| +uint32 VEHICLE_CMD_SET_NAV_STATE = 100001 # Change mode by specifying nav_state directly. |nav_state|Empty|Empty|Empty|Empty|Empty|Empty| uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization | uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 63322ecb6e..b2687ea50f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -878,6 +878,18 @@ Commander::handle_command(const vehicle_command_s &cmd) } break; + case vehicle_command_s::VEHICLE_CMD_SET_NAV_STATE: { // Used from ROS + uint8_t desired_nav_state = (uint8_t)(cmd.param1 + 0.5f); + + if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: { // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints