From 8a8496d57e0d5140d9210c255d18435cac1de57a Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Wed, 25 Feb 2026 18:29:59 +1100 Subject: [PATCH] mavlink:COMMAND_INT - reject invalid frame (#26408) * mavlink:COMMAND_INT - reject invalid frame * Add msg version for new command and use * Delete msg/px4_msgs_old/msg/VehicleCommandAck.msg:Zone.Identifier * Fix up translation * fix: translation_node build Signed-off-by: Beniamino Pozzan --------- Signed-off-by: Beniamino Pozzan Co-authored-by: Beniamino Pozzan --- msg/px4_msgs_old/msg/VehicleCommandAckV0.msg | 35 +++++++++++++ .../translations/all_translations.h | 3 +- .../translation_vehicle_command_ack_v1.h | 51 +++++++++++++++++++ msg/versioned/VehicleCommandAck.msg | 46 +++++++++-------- src/modules/mavlink/mavlink_receiver.cpp | 7 +++ 5 files changed, 121 insertions(+), 21 deletions(-) create mode 100644 msg/px4_msgs_old/msg/VehicleCommandAckV0.msg create mode 100644 msg/translation_node/translations/translation_vehicle_command_ack_v1.h diff --git a/msg/px4_msgs_old/msg/VehicleCommandAckV0.msg b/msg/px4_msgs_old/msg/VehicleCommandAckV0.msg new file mode 100644 index 0000000000..c3f679bfbc --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleCommandAckV0.msg @@ -0,0 +1,35 @@ +# Vehicle Command Ackonwledgement uORB message. +# Used for acknowledging the vehicle command being received. +# Follows the MAVLink COMMAND_ACK message definition + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +# Result cases. This follows the MAVLink MAV_RESULT enum definition +uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | +uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | +uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED | +uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED | +uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command executed, but failed | +uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command being executed | +uint8 VEHICLE_CMD_RESULT_CANCELLED = 6 # Command Canceled + +# Arming denied specific cases +uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0 +uint16 ARM_AUTH_DENIED_REASON_NONE = 1 +uint16 ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT = 2 +uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3 +uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4 +uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 + +uint8 ORB_QUEUE_LENGTH = 8 + +uint32 command # Command that is being acknowledged +uint8 result # Command result +uint8 result_param1 # Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS +int32 result_param2 # Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. +uint8 target_system +uint16 target_component # Target component / mode executor + +bool from_external # Indicates if the command came from an external source diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 31342547f9..c383142ab0 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -17,6 +17,7 @@ #include "translation_register_ext_component_reply_v1.h" #include "translation_register_ext_component_request_v1.h" #include "translation_vehicle_attitude_setpoint_v1.h" +#include "translation_vehicle_command_ack_v1.h" +#include "translation_vehicle_local_position_v1.h" #include "translation_vehicle_status_v1.h" #include "translation_vehicle_status_v2.h" -#include "translation_vehicle_local_position_v1.h" diff --git a/msg/translation_node/translations/translation_vehicle_command_ack_v1.h b/msg/translation_node/translations/translation_vehicle_command_ack_v1.h new file mode 100644 index 0000000000..993b20008c --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_command_ack_v1.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleCommandAck v0 <--> v1 +#include +#include + +class VehicleCommandAckV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleCommandAckV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::VehicleCommandAck; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/vehicle_command_ack"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + msg_newer.timestamp = msg_older.timestamp; + msg_newer.command = msg_older.command; + msg_newer.result = msg_older.result; + msg_newer.result_param1 = msg_older.result_param1; + msg_newer.result_param2 = msg_older.result_param2; + msg_newer.target_system = msg_older.target_system; + msg_newer.target_component = msg_older.target_component; + msg_newer.from_external = msg_older.from_external; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + // Set msg_older from msg_newer + msg_older.timestamp = msg_newer.timestamp; + msg_older.command = msg_newer.command; + if (msg_newer.result > msg_newer.VEHICLE_CMD_RESULT_CANCELLED) { + msg_older.result = msg_older.VEHICLE_CMD_RESULT_DENIED; + } + else { + msg_older.result = msg_newer.result; + } + msg_older.result_param1 = msg_newer.result_param1; + msg_older.result_param2 = msg_newer.result_param2; + msg_older.target_system = msg_newer.target_system; + msg_older.target_component = msg_newer.target_component; + msg_older.from_external = msg_newer.from_external; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleCommandAckV1Translation); diff --git a/msg/versioned/VehicleCommandAck.msg b/msg/versioned/VehicleCommandAck.msg index c3f679bfbc..1546577582 100644 --- a/msg/versioned/VehicleCommandAck.msg +++ b/msg/versioned/VehicleCommandAck.msg @@ -1,20 +1,32 @@ -# Vehicle Command Ackonwledgement uORB message. +# Vehicle Command Acknowledgement uORB message. +# # Used for acknowledging the vehicle command being received. # Follows the MAVLink COMMAND_ACK message definition -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # [us] time since system start -# Result cases. This follows the MAVLink MAV_RESULT enum definition -uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | -uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | -uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED | -uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED | -uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command executed, but failed | -uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command being executed | -uint8 VEHICLE_CMD_RESULT_CANCELLED = 6 # Command Canceled +uint8 ORB_QUEUE_LENGTH = 8 +uint32 command # [-] Command that is being acknowledged + +uint8 result # [@enum VEHICLE_CMD_RESULT] Command result +# VEHICLE_CMD_RESULT Result cases. Follows the MAVLink MAV_RESULT enum definition +uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED +uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED +uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED +uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED +uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command executed, but failed +uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command being executed +uint8 VEHICLE_CMD_RESULT_CANCELLED = 6 # Command Canceled +uint8 VEHICLE_CMD_RESULT_COMMAND_LONG_ONLY = 7 # Command is only accepted when sent as a COMMAND_LONG +uint8 VEHICLE_CMD_RESULT_COMMAND_INT_ONLY = 8 # Command is only accepted when sent as a COMMAND_INT +uint8 VEHICLE_CMD_RESULT_UNSUPPORTED_MAV_FRAME = 9 # Command does not support specified frame + +uint8 result_param1 # [-] Can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (%) + +int32 result_param2 # [enum ARM_AUTH_DENIED_REASON] Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied, or what ARM_AUTH_DENIED_REASON # Arming denied specific cases uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0 uint16 ARM_AUTH_DENIED_REASON_NONE = 1 @@ -23,13 +35,7 @@ uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3 uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4 uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 -uint8 ORB_QUEUE_LENGTH = 8 +uint8 target_system # [-] Target system +uint16 target_component # Target component / mode executor -uint32 command # Command that is being acknowledged -uint8 result # Command result -uint8 result_param1 # Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS -int32 result_param2 # Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. -uint8 target_system -uint16 target_component # Target component / mode executor - -bool from_external # Indicates if the command came from an external source +bool from_external # Indicates if the command came from an external source diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 41eaf4a355..3f35776368 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -517,6 +517,13 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) return; } + if (cmd_mavlink.frame != MAV_FRAME_GLOBAL_INT) { + // PX4 only supports global frame. + PX4_ERR("frame invalid for command %" PRIu16, cmd_mavlink.command); + acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED_MAV_FRAME); + return; + } + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ vcmd.param1 = cmd_mavlink.param1; vcmd.param2 = cmd_mavlink.param2;