mavlink: spacing, naming of command frame handling

This commit is contained in:
Matthias Grob
2024-05-02 17:20:38 +02:00
parent 6dbf39be7c
commit 3b21f3999f
3 changed files with 6 additions and 12 deletions
+4 -4
View File
@@ -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.
+1 -5
View File
@@ -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:
+1 -3
View File
@@ -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;
}