From 76dedfec5d1201dd5779271247213fe75efea5b9 Mon Sep 17 00:00:00 2001
From: Oleg <ev.mipt@gmail.com>
Date: Fri, 9 Jul 2021 12:36:06 +0300
Subject: [PATCH] mavlink: optimize message forwarding

---
 src/modules/mavlink/mavlink_main.cpp | 55 +++++++++++++---------------
 1 file changed, 25 insertions(+), 30 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index be0f0c88cf..4a26e52db2 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -382,43 +382,38 @@ Mavlink::serial_instance_exists(const char *device_name, Mavlink *self)
 void
 Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 {
-	LockGuard lg{mavlink_module_mutex};
+	const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid);
 
-	for (Mavlink *inst : mavlink_module_instances) {
-		if (inst && (inst != self)) {
-			const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid);
+	int target_system_id = 0;
+	int target_component_id = 0;
 
-			int target_system_id = 0;
-			int target_component_id = 0;
+	// might be nullptr if message is unknown
+	if (meta) {
+		// Extract target system and target component if set
+		if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) {
+			target_system_id = (_MAV_PAYLOAD(msg))[meta->target_system_ofs];
+		}
 
-			// might be nullptr if message is unknown
-			if (meta) {
-				// Extract target system and target component if set
-				if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) {
-					if (meta->target_system_ofs < msg->len) {
-						target_system_id = (_MAV_PAYLOAD(msg))[meta->target_system_ofs];
-					}
-				}
+		if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) {
+			target_component_id = (_MAV_PAYLOAD(msg))[meta->target_component_ofs];
+		}
+	}
 
-				if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) {
-					if (meta->target_component_ofs < msg->len) {
-						target_component_id = (_MAV_PAYLOAD(msg))[meta->target_component_ofs];
-					}
-				}
-			}
+	// If it's a message only for us, we keep it, otherwise, we forward it.
+	if (target_system_id == self->get_system_id() && target_component_id == self->get_component_id()) {
+		return;
+	}
 
-			// If it's a message only for us, we keep it, otherwise, we forward it.
-			const bool targeted_only_at_us =
-				(target_system_id == self->get_system_id() &&
-				 target_component_id == self->get_component_id());
+	// We don't forward heartbeats unless it's specifically enabled.
+	if (msg->msgid == MAVLINK_MSG_ID_HEARTBEAT && !self->forward_heartbeats_enabled()) {
+		return;
+	}
 
-			// We don't forward heartbeats unless it's specifically enabled.
-			const bool heartbeat_check_ok =
-				(msg->msgid != MAVLINK_MSG_ID_HEARTBEAT || self->forward_heartbeats_enabled());
+	LockGuard lg{mavlink_module_mutex};
 
-			if (!targeted_only_at_us && heartbeat_check_ok) {
-				inst->pass_message(msg);
-			}
+	for (Mavlink *inst : mavlink_module_instances) {
+		if (inst && (inst != self)) {
+			inst->pass_message(msg);
 		}
 	}
 }