From 3b21f3999f98690efec104ef344e426693c0edac Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 2 May 2024 17:20:38 +0200 Subject: [PATCH] mavlink: spacing, naming of command frame handling --- msg/VehicleCommand.msg | 8 ++++---- src/modules/mavlink/mavlink_receiver.cpp | 6 +----- src/modules/navigator/navigator_main.cpp | 4 +--- 3 files changed, 6 insertions(+), 12 deletions(-) diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 4bce9173d7..2b5e4dc090 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -165,10 +165,10 @@ int8 ARMING_ACTION_ARM = 1 uint8 GRIPPER_ACTION_RELEASE = 0 uint8 GRIPPER_ACTION_GRAB = 1 -uint8 frame # The coordinate system of the COMMAND. -uint8 FRAME_UNKNOWN = 0 # Not specified. -uint8 FRAME_GLOBAL = 1 # Global (WGS84) coordinate frame + MSL altitude. -uint8 FRAME_GLOBAL_RELATIVE_ALT = 2 # Global (WGS84) coordinate frame + altitude relative to the home position. +uint8 frame # The coordinate system of the Command +uint8 FRAME_UNKNOWN = 0 # Not specified +uint8 FRAME_GLOBAL = 1 # Global WGS84 coordinate frame + altitude relative to mean sea level +uint8 FRAME_GLOBAL_RELATIVE_ALTITUDE = 2 # Global WGS84 coordinate frame + altitude relative to the home position float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. float32 param2 # Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum. diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fb97e5ee1d..81faf07f98 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -471,17 +471,13 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) switch (cmd_mavlink.frame) { case MAV_FRAME_GLOBAL: - - // FALLTHROUGH case MAV_FRAME_GLOBAL_INT: vcmd.frame = vehicle_command_s::FRAME_GLOBAL; break; case MAV_FRAME_GLOBAL_RELATIVE_ALT: - - // FALLTHROUGH case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: - vcmd.frame = vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALT; + vcmd.frame = vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALTITUDE; break; default: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 280a84b92f..1a31d0f20e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -281,13 +281,11 @@ void Navigator::run() if (PX4_ISFINITE(cmd.param7)) { switch (cmd.frame) { default: - - // FALLTHROUGH case vehicle_command_s::FRAME_GLOBAL: position_setpoint.alt = cmd.param7; break; - case vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALT: + case vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALTITUDE: position_setpoint.alt = cmd.param7 + get_home_position()->alt; break; }