From 7bf37616f197e50ccb59ee5688652c164fe58348 Mon Sep 17 00:00:00 2001
From: Jacob Dahl <dahl.jakejacob@gmail.com>
Date: Wed, 20 May 2020 11:58:23 -0600
Subject: [PATCH] moved mavlink forwarding logic to
 MAVLinkProtocol:receiveBytes

---
 src/comm/LinkManager.cc     | 15 ---------------
 src/comm/MAVLinkProtocol.cc | 14 ++++++++++++++
 src/comm/UDPLink.cc         |  2 +-
 3 files changed, 15 insertions(+), 16 deletions(-)

diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc
index f6de788ba..7eedc47f6 100644
--- a/src/comm/LinkManager.cc
+++ b/src/comm/LinkManager.cc
@@ -1016,21 +1016,6 @@ void LinkManager::_freeMavlinkChannel(int channel)
 
 void LinkManager::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) {
     link->startMavlinkMessagesTimer(message.sysid);
-
-    // Walk the list of Links. If mavlink forwarding is enabled for a given link then send the data out.
-    for (int i = 0; i < _sharedLinks.count(); i++) {
-        LinkConfiguration* linkConfig = _sharedLinks[i]->getLinkConfiguration();
-
-        bool isUniqueLink = linkConfig->link() != link; // We do not want to send messages back on the link from which they originated
-        bool forwardMavlink = isUniqueLink && linkConfig->isForwardMavlink();
-
-        if (forwardMavlink) {
-            qCDebug(LinkManagerLog) << "Forwarding mavlink packet";
-            uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-            int len = mavlink_msg_to_send_buffer(buffer, &message);
-            _sharedLinks[i]->writeBytesSafe((const char*)buffer, len);
-        }
-    }
 }
 
 LogReplayLink* LinkManager::startLogReplay(const QString& logFile)
diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index 2b1cf0e24..f3ef0c7b9 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -208,6 +208,20 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
         return;
     }
 
+    // Walk the list of Links. If mavlink forwarding is enabled for a given link then send the data out.
+    QList<LinkInterface*> links = _linkMgr->links();
+    for (int i = 0; i < links.count(); i++) {
+        LinkConfiguration* linkConfig = links[i]->getLinkConfiguration();
+
+        bool isUniqueLink = links[i] != link; // We do not want to send messages back on the link from which they originated
+        bool forwardMavlink = isUniqueLink && linkConfig->isForwardMavlink();
+
+        if (forwardMavlink) {
+            qDebug() << "Forwarding mavlink packet on: " << linkConfig->name();
+            links[i]->writeBytesSafe(b.data(), b.length());
+        }
+    }
+
     uint8_t mavlinkChannel = link->mavlinkChannel();
 
     static int  nonmavlinkCount = 0;
diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc
index f336424b6..c80a82c19 100644
--- a/src/comm/UDPLink.cc
+++ b/src/comm/UDPLink.cc
@@ -185,7 +185,7 @@ void UDPLink::_writeBytes(const QByteArray data)
 
 void UDPLink::_writeDataGram(const QByteArray data, const UDPCLient* target)
 {
-    //qDebug() << "UDP Out" << target->address << target->port;
+    // qDebug() << "UDP Out" << target->address << target->port;
     if(_socket->writeDatagram(data, target->address, target->port) < 0) {
         qWarning() << "Error writing to" << target->address << target->port;
     } else {
-- 
2.22.0