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 <maowen801@gmail.com>

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];
 				}
 			}