Fixed link forwarding

src/comm/MAVLinkProtocol.cc

@@ -429,7 +429,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                     {
                         // Only forward this message to the other links,
                         // not the link the message was received on
-                        if (currLink != link) sendMessage(currLink, message);
+                        if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
                     }
                 }
             }
@@ -475,7 +475,7 @@ void MAVLinkProtocol::sendMessage(mavlink_message_t message)
     for (i = links.begin(); i != links.end(); ++i)
     {
         sendMessage(*i, message);
-        //qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
+        qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
     }
 }
 
@@ -500,6 +500,29 @@ void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message
     }
 }
 
+/**
+ * @param link the link to send the message over
+ * @param message message to send
+ * @param systemid id of the system the message is originating from
+ * @param componentid id of the component the message is originating from
+ */
+void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid)
+{
+    // Create buffer
+    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+    // Rewriting header to ensure correct link ID is set
+    static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
+    if (link->getId() != 0) mavlink_finalize_message_chan(&message, systemid, componentid, link->getId(), message.len, messageKeys[message.msgid]);
+    // Write message into buffer, prepending start sign
+    int len = mavlink_msg_to_send_buffer(buffer, &message);
+    // If link is connected
+    if (link->isConnected())
+    {
+        // Send the portion of the buffer now occupied by the message
+        link->writeBytes((const char*)buffer, len);
+    }
+}
+
 /**
  * The heartbeat is sent out of order and does not reset the
  * periodic heartbeat emission. It will be just sent in addition.

src/comm/MAVLinkProtocol.h

@@ -130,8 +130,10 @@ public slots:
     void receiveBytes(LinkInterface* link, QByteArray b);
     /** @brief Send MAVLink message through serial interface */
     void sendMessage(mavlink_message_t message);
-    /** @brief Send MAVLink message through serial interface */
+    /** @brief Send MAVLink message */
     void sendMessage(LinkInterface* link, mavlink_message_t message);
+    /** @brief Send MAVLink message with correct system / component ID */
+    void sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid);
     /** @brief Set the rate at which heartbeats are emitted */
     void setHeartbeatRate(int rate);
     /** @brief Set the system id of this application */

src/comm/UDPLink.cc

@@ -123,11 +123,9 @@ void UDPLink::addHost(const QString& host)
                 }
             }
             hosts.append(address);
-			this->setAddress(address);
             //qDebug() << "Address:" << address.toString();
             // Set port according to user input
             ports.append(host.split(":").last().toInt());
-			this->setPort(host.split(":").last().toInt());
         }
     }
     else

src/ui/MAVLinkSettingsWidget.cc

@@ -186,9 +186,11 @@ void MAVLinkSettingsWidget::chooseLogfileName()
 
 void MAVLinkSettingsWidget::enableDroneOS(bool enable)
 {
+    // Enable multiplexing
+    protocol->enableMultiplexing(enable);
     // Get current selected host and port
     QString hostString = m_ui->droneOSComboBox->currentText();
-    QString host = hostString.split(":").first();
+    //QString host = hostString.split(":").first();
 
     // Delete from all lists first
     UDPLink* firstUdp = NULL;