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());