moved mavlink forwarding logic to MAVLinkProtocol:receiveBytes

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)

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;

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 {