From e25db0162019e5f0bb7171ec6f3db2f456672d95 Mon Sep 17 00:00:00 2001 From: Mark Owen Date: Tue, 13 Aug 2019 08:16:09 -0600 Subject: [PATCH] Mavlink: Fix forwarding of messages with target system/component id (#12559) Mavlink does not correctly forward messages that have the target_system or target_component routing fields in the message. Some investigation revealed that the Mavlink::forward_message function is incorrectly utilizing the mavlink_msg_entry_t.target_system_ofs and mavlink_msg_entry_t.target_component_ofs fields. These offsets are intended to be used relative to the start of the message payload. But, as implemented, these offsets are incorrectly being used relative to the start of the message. This pull-request corrects that problem. I also correctly made use of the mavlink_msg_entry_t.flags field to determine if a message contains a target_system or target component field. The previous check incorrectly assumed that they would always be non-zero if present. Signed-off-by: Mark Owen --- src/modules/mavlink/mavlink_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5f4ca41b8a..3c82eab471 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -450,12 +450,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) // might be nullptr if message is unknown if (meta) { // Extract target system and target component if set - if (meta->target_system_ofs != 0) { - target_system_id = ((uint8_t *)msg)[meta->target_system_ofs]; + if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { + target_system_id = (_MAV_PAYLOAD(msg))[meta->target_system_ofs]; } - if (meta->target_component_ofs != 0) { - target_component_id = ((uint8_t *)msg)[meta->target_component_ofs]; + if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) { + target_component_id = (_MAV_PAYLOAD(msg))[meta->target_component_ofs]; } }