From e387f302f9291d154c751e0f0fe214732b05b845 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 28 Jan 2022 14:16:11 -0500 Subject: [PATCH] mavlink: streams/COMMAND_LONG don't send internal vehicle_commands --- src/modules/mavlink/streams/COMMAND_LONG.hpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/streams/COMMAND_LONG.hpp b/src/modules/mavlink/streams/COMMAND_LONG.hpp index bd36968110..f216c34810 100644 --- a/src/modules/mavlink/streams/COMMAND_LONG.hpp +++ b/src/modules/mavlink/streams/COMMAND_LONG.hpp @@ -74,7 +74,16 @@ private: _vehicle_command_sub.get_last_generation()); } - if (!cmd.from_external && cmd.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) { + // mavlink mavlink commands are <= UINT16_MAX + const bool px4_internal_cmd = (cmd.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START); + + // internal commands + const bool target_system_internal = (cmd.target_system == _mavlink->get_system_id()) + && (cmd.target_component == _mavlink->get_component_id()) + && (cmd.source_system == cmd.target_system) + && (cmd.source_component == cmd.target_component); + + if (!cmd.from_external && !px4_internal_cmd && !target_system_internal) { PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());