diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 9c7f03eeecd0574898912946b93b76769b4faa6c..f1b63d57c6f47d6f2d54688d1e884bda33526980 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -735,7 +735,7 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
     uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
     int len = mavlink_msg_to_send_buffer(buffer, &message);
 
-    link->writeBytes((const char*)buffer, len);
+    link->writeBytesSafe((const char*)buffer, len);
     _messagesSent++;
     emit messagesSentChanged();
 }
diff --git a/src/comm/BluetoothLink.cc b/src/comm/BluetoothLink.cc
index 3f814b93ba3142a24838563d8602feff7c20fced..f10161ed3d4aeebf694172ad704ebad186c808c9 100644
--- a/src/comm/BluetoothLink.cc
+++ b/src/comm/BluetoothLink.cc
@@ -89,19 +89,14 @@ QString BluetoothLink::getName() const
     return _config->name();
 }
 
-void BluetoothLink::writeBytes(const char* data, qint64 size)
-{
-    _sendBytes(data, size);
-}
-
-void BluetoothLink::_sendBytes(const char* data, qint64 size)
+void BluetoothLink::_writeBytes(const QByteArray bytes)
 {
     if(_targetSocket)
     {
         if(_targetSocket->isWritable())
         {
-            if(_targetSocket->write(data, size) > 0) {
-                _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch());
+            if(_targetSocket->write(bytes) > 0) {
+                _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch());
             }
             else
                 qWarning() << "Bluetooth write error";
diff --git a/src/comm/BluetoothLink.h b/src/comm/BluetoothLink.h
index 1767cc1a44bd1ae76f269e93e8f801e7e831c45b..c2a83f4d5a6a5acefccc89269f1864a97c489ee3 100644
--- a/src/comm/BluetoothLink.h
+++ b/src/comm/BluetoothLink.h
@@ -165,9 +165,7 @@ public:
     LinkConfiguration* getLinkConfiguration() { return _config; }
 
 public slots:
-
     void    readBytes               ();
-    void    writeBytes              (const char* data, qint64 length);
     void    deviceConnected         ();
     void    deviceDisconnected      ();
     void    deviceError             (QBluetoothSocket::SocketError error);
@@ -193,7 +191,11 @@ private:
 
     bool _hardwareConnect       ();
     void _restartConnection     ();
-    void _sendBytes             (const char* data, qint64 size);
+
+private slots:
+    void _writeBytes            (const QByteArray bytes);
+
+private:
     void _createSocket          ();
 
 private:
diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h
index ff7393454c294d4da900aa66eacf533a56f1e08f..bd0b48e2fc00927df05b35242c00257453d4e7e2 100644
--- a/src/comm/LinkInterface.h
+++ b/src/comm/LinkInterface.h
@@ -151,16 +151,24 @@ public slots:
      *
      * If the underlying communication is packet oriented,
      * one write command equals a datagram. In case of serial
-     * communication arbitrary byte lengths can be written
+     * communication arbitrary byte lengths can be written. The method ensures
+     * thread safety regardless of the underlying LinkInterface implementation.
      *
      * @param bytes The pointer to the byte array containing the data
      * @param length The length of the data array
      **/
-    virtual void writeBytes(const char *bytes, qint64 length) = 0;
+    void writeBytesSafe(const char *bytes, int length)
+    {
+        emit _invokeWriteBytes(QByteArray(bytes, length));
+    }
+
+private slots:
+    virtual void _writeBytes(const QByteArray) = 0;
     
 signals:
     void autoconnectChanged(bool autoconnect);
     void activeChanged(bool active);
+    void _invokeWriteBytes(QByteArray);
 
     /**
      * @brief New data arrived
@@ -212,6 +220,7 @@ protected:
         memset(_outDataWriteAmounts,0, sizeof(_outDataWriteAmounts));
         memset(_outDataWriteTimes,  0, sizeof(_outDataWriteTimes));
         
+        QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::_writeBytes);
         qRegisterMetaType<LinkInterface*>("LinkInterface*");
     }
 
diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc
index 6840343c8e0d5073d776cf17f89535457de84e06..e1310ba496acf6eec8b0a3674e5c10af76580d59 100644
--- a/src/comm/LinkManager.cc
+++ b/src/comm/LinkManager.cc
@@ -881,7 +881,7 @@ void LinkManager::_activeLinkCheck(void)
     if (!found && link) {
         // See if we can get an NSH prompt on this link
         bool foundNSHPrompt = false;
-        link->writeBytes("\r", 1);
+        link->writeBytesSafe("\r", 1);
         QSignalSpy spy(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)));
         if (spy.wait(100)) {
             QList<QVariant> arguments = spy.takeFirst();
diff --git a/src/comm/LogReplayLink.cc b/src/comm/LogReplayLink.cc
index f3ba81eacbdd82dc6469b0fbe7b925b26da2fa61..81ac1ccc15251b1ff6046565e9356ad777fe76ff 100644
--- a/src/comm/LogReplayLink.cc
+++ b/src/comm/LogReplayLink.cc
@@ -151,10 +151,9 @@ void LogReplayLink::_replayError(const QString& errorMsg)
 }
 
 /// Since this is log replay, we just drops writes on the floor
-void LogReplayLink::writeBytes(const char* bytes, qint64 cBytes)
+void LogReplayLink::_writeBytes(const QByteArray bytes)
 {
     Q_UNUSED(bytes);
-    Q_UNUSED(cBytes);
 }
 
 /// Parses a BigEndian quint64 timestamp
diff --git a/src/comm/LogReplayLink.h b/src/comm/LogReplayLink.h
index 034e5ae0fa370fae56b7d1696a95f13e491525b1..e54d27e712cbd34472873403ff3cc28138009029 100644
--- a/src/comm/LogReplayLink.h
+++ b/src/comm/LogReplayLink.h
@@ -98,8 +98,8 @@ public:
     bool connect(void);
     bool disconnect(void);
 
-public slots:
-    virtual void writeBytes(const char *bytes, qint64 cBytes);
+private slots:
+    virtual void _writeBytes(const QByteArray bytes);
 
 signals:
     void logFileStats(bool logTimestamped, int logDurationSecs, int binaryBaudRate);
diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index 453ab1b43e9ca96d70f79dd48a69d5e8b756e69a..ebde1f123e2785ffdec4d94ab67bca3ecc656d1d 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -438,7 +438,7 @@ void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t messag
     if (link->isConnected())
     {
         // Send the portion of the buffer now occupied by the message
-        link->writeBytes((const char*)buffer, len);
+        link->writeBytesSafe((const char*)buffer, len);
     }
 }
 
@@ -461,7 +461,7 @@ void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t messag
     if (link->isConnected())
     {
         // Send the portion of the buffer now occupied by the message
-        link->writeBytes((const char*)buffer, len);
+        link->writeBytesSafe((const char*)buffer, len);
     }
 }
 
diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc
index 64780a16f698806480e81536180e3b1cda21a91b..9d161c6cf0c9f3b0afb5c33f4bfd99c6aec6a664 100644
--- a/src/comm/MockLink.cc
+++ b/src/comm/MockLink.cc
@@ -115,7 +115,6 @@ MockLink::MockLink(MockConfiguration* config)
     moveToThread(this);
 
     _loadParams();
-    QObject::connect(this, &MockLink::_incomingBytes, this, &MockLink::_handleIncomingBytes);
 }
 
 MockLink::~MockLink(void)
@@ -314,16 +313,7 @@ void MockLink::respondWithMavlinkMessage(const mavlink_message_t& msg)
 }
 
 /// @brief Called when QGC wants to write bytes to the MAV
-void MockLink::writeBytes(const char* bytes, qint64 cBytes)
-{
-    // Package up the data so we can signal it over to the right thread
-    QByteArray byteArray(bytes, cBytes);
-
-    emit _incomingBytes(byteArray);
-}
-
-/// @brief Handles bytes from QGC on the thread
-void MockLink::_handleIncomingBytes(const QByteArray bytes)
+void MockLink::_writeBytes(const QByteArray bytes)
 {
     if (_inNSH) {
         _handleIncomingNSHBytes(bytes.constData(), bytes.count());
diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h
index 304b941241661e11543271f52162edc637c8e1d2..395907e8e660915c27ebe3fdffc1fc84c29b71e7 100644
--- a/src/comm/MockLink.h
+++ b/src/comm/MockLink.h
@@ -150,12 +150,8 @@ public:
     static MockLink* startAPMArduCopterMockLink  (bool sendStatusText);
     static MockLink* startAPMArduPlaneMockLink   (bool sendStatusText);
 
-signals:
-    /// @brief Used internally to move data to the thread.
-    void _incomingBytes(const QByteArray bytes);
-
-public slots:
-    virtual void writeBytes(const char *bytes, qint64 cBytes);
+private slots:
+    virtual void _writeBytes(const QByteArray bytes);
 
 private slots:
     void _run1HzTasks(void);
@@ -172,7 +168,6 @@ private:
 
     // MockLink methods
     void _sendHeartBeat(void);
-    void _handleIncomingBytes(const QByteArray bytes);
     void _handleIncomingNSHBytes(const char* bytes, int cBytes);
     void _handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes);
     void _loadParams(void);
diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc
index 16d2ed28305743612ed353972a35728a258f05c2..5f0ec849b845b29f2e145a3f1d949a552caed098 100644
--- a/src/comm/QGCFlightGearLink.cc
+++ b/src/comm/QGCFlightGearLink.cc
@@ -234,7 +234,7 @@ void QGCFlightGearLink::updateControls(quint64 time, float rollAilerons, float p
     {
         QString state("%1\t%2\t%3\t%4\t%5\n");
         state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle);
-        writeBytes(state.toLatin1().constData(), state.length());
+        emit _invokeWriteBytes(state.toLatin1());
         //qDebug() << "Updated controls" << rollAilerons << pitchElevator << yawRudder << throttle;
         //qDebug() << "Updated controls" << state;
     }
@@ -244,13 +244,13 @@ void QGCFlightGearLink::updateControls(quint64 time, float rollAilerons, float p
     }
 }
 
-void QGCFlightGearLink::writeBytes(const char* data, qint64 size)
+void QGCFlightGearLink::_writeBytes(const QByteArray data)
 {
     //#define QGCFlightGearLink_DEBUG
 #ifdef QGCFlightGearLink_DEBUG
     QString bytes;
     QString ascii;
-    for (int i=0; i<size; i++)
+    for (int i=0, size = data.size(); i<size; i++)
     {
         unsigned char v = data[i];
         bytes.append(QString().sprintf("%02x ", v));
@@ -267,7 +267,7 @@ void QGCFlightGearLink::writeBytes(const char* data, qint64 size)
     qDebug() << bytes;
     qDebug() << "ASCII:" << ascii;
 #endif
-    if (connectState && _udpCommSocket) _udpCommSocket->writeDatagram(data, size, currentHost, currentPort);
+    if (connectState && _udpCommSocket) _udpCommSocket->writeDatagram(data, currentHost, currentPort);
 }
 
 /**
diff --git a/src/comm/QGCFlightGearLink.h b/src/comm/QGCFlightGearLink.h
index 8945b202496713e31c3d6dd08f75542c7339d562..b9887ef6bf9a4a6444d515adf56c75499e3af2c4 100644
--- a/src/comm/QGCFlightGearLink.h
+++ b/src/comm/QGCFlightGearLink.h
@@ -121,13 +121,17 @@ public slots:
     }
 
     void readBytes();
+
+private slots:
     /**
      * @brief Write a number of bytes to the interface.
      *
      * @param data Pointer to the data byte array
      * @param size The size of the bytes array
      **/
-    void writeBytes(const char* data, qint64 length);
+    void _writeBytes(const QByteArray data);
+
+public slots:
     bool connectSimulation();
     bool disconnectSimulation();
 
diff --git a/src/comm/QGCHilLink.h b/src/comm/QGCHilLink.h
index abc1d36852eb154cb8cbc2b98deadeb2c0a717e1..9ac69813a53d465683bfacd366bb465915a603f3 100644
--- a/src/comm/QGCHilLink.h
+++ b/src/comm/QGCHilLink.h
@@ -64,13 +64,26 @@ public slots:
      * @param data Pointer to the data byte array
      * @param size The size of the bytes array
      **/
-    virtual void writeBytes(const char* data, qint64 length) = 0;
+    void writeBytesSafe(const char* data, int length)
+    {
+        emit _invokeWriteBytes(QByteArray(data, length));
+    }
+
     virtual bool connectSimulation() = 0;
     virtual bool disconnectSimulation() = 0;
 
+private slots:
+    virtual void _writeBytes(const QByteArray) = 0;
+
 protected:
     virtual void setName(QString name) = 0;
 
+    QGCHilLink() :
+        QThread()
+    {
+        connect(this, &QGCHilLink::_invokeWriteBytes, this, &QGCHilLink::_writeBytes);
+    }
+
 signals:
     /**
      * @brief This signal is emitted instantly when the link is connected
@@ -130,6 +143,9 @@ signals:
 
     /** @brief Sensor leve HIL state changed */
     void sensorHilChanged(bool enabled);
+
+    /** @brief Helper signal to force execution on the correct thread */
+    void _invokeWriteBytes(QByteArray);
 };
 
 #endif // QGCHILLINK_H
diff --git a/src/comm/QGCJSBSimLink.cc b/src/comm/QGCJSBSimLink.cc
index 487240c92aeaada598b50d09d71969e9fdb1f60c..fe8254749c887e530d07aaaebbb19bc77f5808e9 100644
--- a/src/comm/QGCJSBSimLink.cc
+++ b/src/comm/QGCJSBSimLink.cc
@@ -246,7 +246,7 @@ void QGCJSBSimLink::updateControls(quint64 time, float rollAilerons, float pitch
     {
         QString state("%1\t%2\t%3\t%4\t%5\n");
         state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle);
-        writeBytes(state.toLatin1().constData(), state.length());
+        emit _invokeWriteBytes(state.toLatin1());
     }
     else
     {
@@ -255,13 +255,13 @@ void QGCJSBSimLink::updateControls(quint64 time, float rollAilerons, float pitch
     //qDebug() << "Updated controls" << state;
 }
 
-void QGCJSBSimLink::writeBytes(const char* data, qint64 size)
+void QGCJSBSimLink::_writeBytes(const QByteArray data)
 {
     //#define QGCJSBSimLink_DEBUG
 #ifdef QGCJSBSimLink_DEBUG
     QString bytes;
     QString ascii;
-    for (int i=0; i<size; i++)
+    for (int i=0, size = data.size(); i<size; i++)
     {
         unsigned char v = data[i];
         bytes.append(QString().sprintf("%02x ", v));
@@ -278,7 +278,7 @@ void QGCJSBSimLink::writeBytes(const char* data, qint64 size)
     qDebug() << bytes;
     qDebug() << "ASCII:" << ascii;
 #endif
-    if (connectState && socket) socket->writeDatagram(data, size, currentHost, currentPort);
+    if (connectState && socket) socket->writeDatagram(data, currentHost, currentPort);
 }
 
 /**
diff --git a/src/comm/QGCJSBSimLink.h b/src/comm/QGCJSBSimLink.h
index 41bc4463a2a572412e7a7a1c4dbe2769ec1faf4a..e09d43b0983e50bcc45d843ed878639005df9ef0 100644
--- a/src/comm/QGCJSBSimLink.h
+++ b/src/comm/QGCJSBSimLink.h
@@ -114,13 +114,17 @@ public slots:
     }
 
     void readBytes();
+
+private slots:
     /**
      * @brief Write a number of bytes to the interface.
      *
      * @param data Pointer to the data byte array
      * @param size The size of the bytes array
      **/
-    void writeBytes(const char* data, qint64 length);
+    void _writeBytes(const QByteArray data);
+
+public slots:
     bool connectSimulation();
     bool disconnectSimulation();
 
diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc
index 487f5b2ea6556c670642a87611e2fc534c8c35be..244b143617a075fc26b99c4c11ca1e28a52b9f4f 100644
--- a/src/comm/QGCXPlaneLink.cc
+++ b/src/comm/QGCXPlaneLink.cc
@@ -229,7 +229,7 @@ void QGCXPlaneLink::run()
     strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6));
     ip.use_ip = 1;
 
-    writeBytes((const char*)&ip, sizeof(ip));
+    writeBytesSafe((const char*)&ip, sizeof(ip));
 
     _should_exit = false;
 
@@ -395,10 +395,10 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
     {
         // Ail / Elevon / Rudder
         p.index = 12;   // XPlane, wing sweep
-        writeBytes((const char*)&p, sizeof(p));
+        writeBytesSafe((const char*)&p, sizeof(p));
 
         p.index = 8;    // XPlane, joystick? why?
-        writeBytes((const char*)&p, sizeof(p));
+        writeBytesSafe((const char*)&p, sizeof(p));
 
         p.index = 25;   // Thrust
         memset(p.f, 0, sizeof(p.f));
@@ -408,13 +408,13 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
         p.f[3] = throttle;
 
         // Throttle
-        writeBytes((const char*)&p, sizeof(p));
+        writeBytesSafe((const char*)&p, sizeof(p));
     }
     else
     {
         qDebug() << "Transmitting p.index = 25";
         p.index = 25;   // XPlane, throttle command.
-        writeBytes((const char*)&p, sizeof(p));
+        writeBytesSafe((const char*)&p, sizeof(p));
     }
 
 }
@@ -448,14 +448,14 @@ Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) {
   return wRo;
 }
 
-void QGCXPlaneLink::writeBytes(const char* data, qint64 size)
+void QGCXPlaneLink::_writeBytes(const QByteArray data)
 {
-    if (!data) return;
+    if (data.isEmpty()) return;
 
     // If socket exists and is connected, transmit the data
     if (socket && connectState)
     {
-        socket->writeDatagram(data, size, remoteHost, remotePort);
+        socket->writeDatagram(data, remoteHost, remotePort);
     }
 }
 
@@ -899,7 +899,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub
     pos.gear_flap_vect[1] = 0.0f;
     pos.gear_flap_vect[2] = 0.0f;
 
-    writeBytes((const char*)&pos, sizeof(pos));
+    writeBytesSafe((const char*)&pos, sizeof(pos));
 
 //    pos.header[0] = 'V';
 //    pos.header[1] = 'E';
@@ -917,7 +917,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub
 //    pos.gear_flap_vect[1] = -999;
 //    pos.gear_flap_vect[2] = -999;
 
-//    writeBytes((const char*)&pos, sizeof(pos));
+//    writeBytesSafe((const char*)&pos, sizeof(pos));
 }
 
 /**
diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h
index 3ead098e0d2927cf6f237d7157f04f21ec81ae6c..03ebf120c29cdcb0b684af298b8c7cda925f9e8e 100644
--- a/src/comm/QGCXPlaneLink.h
+++ b/src/comm/QGCXPlaneLink.h
@@ -126,13 +126,17 @@ public slots:
     void processError(QProcess::ProcessError err);
 
     void readBytes();
+
+private slots:
     /**
      * @brief Write a number of bytes to the interface.
      *
      * @param data Pointer to the data byte array
      * @param size The size of the bytes array
      **/
-    void writeBytes(const char* data, qint64 length);
+    void _writeBytes(const QByteArray data);
+
+public slots:
     bool connectSimulation();
     bool disconnectSimulation();
     /**
diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc
index e20499ae2b46c0c8ae2642e5b69b1c53c3f9efab..ae702b5fb3a9a2fd9959736ae8637d86f939a8a4 100644
--- a/src/comm/SerialLink.cc
+++ b/src/comm/SerialLink.cc
@@ -82,11 +82,11 @@ bool SerialLink::_isBootloader()
     return false;
 }
 
-void SerialLink::writeBytes(const char* data, qint64 size)
+void SerialLink::_writeBytes(const QByteArray data)
 {
     if(_port && _port->isOpen()) {
-        _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch());
-        _port->write(data, size);
+        _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch());
+        _port->write(data);
     } else {
         // Error occured
         _emitLinkError(tr("Could not send data - link %1 is disconnected!").arg(getName()));
diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h
index 011dfb883ef62b8f8dcd11b57e13e1640094e648..f787b810a1fccf9f540b763e0ee9da0c6157ae3d 100644
--- a/src/comm/SerialLink.h
+++ b/src/comm/SerialLink.h
@@ -157,15 +157,16 @@ public:
     bool    connect(void);
     bool    disconnect(void);
 
-public slots:
+private slots:
     /**
      * @brief Write a number of bytes to the interface.
      *
      * @param data Pointer to the data byte array
      * @param size The size of the bytes array
      **/
-    void writeBytes(const char* data, qint64 length);
+    void _writeBytes(const QByteArray data);
 
+public slots:
     void linkError(QSerialPort::SerialPortError error);
 
 protected:
diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc
index 19935e7b268e671cb256a8368fa0889f211392ce..75114d3b9eaae73f47a2fc1970adc89631d50b36 100644
--- a/src/comm/TCPLink.cc
+++ b/src/comm/TCPLink.cc
@@ -65,11 +65,11 @@ void TCPLink::run()
 }
 
 #ifdef TCPLINK_READWRITE_DEBUG
-void TCPLink::_writeDebugBytes(const char *data, qint16 size)
+void TCPLink::_writeDebugBytes(const QByteArray data)
 {
     QString bytes;
     QString ascii;
-    for (int i=0; i<size; i++)
+    for (int i=0, size = data.size(); i<size; i++)
     {
         unsigned char v = data[i];
         bytes.append(QString().sprintf("%02x ", v));
@@ -88,13 +88,16 @@ void TCPLink::_writeDebugBytes(const char *data, qint16 size)
 }
 #endif
 
-void TCPLink::writeBytes(const char* data, qint64 size)
+void TCPLink::_writeBytes(const QByteArray data)
 {
 #ifdef TCPLINK_READWRITE_DEBUG
-    _writeDebugBytes(data, size);
+    _writeDebugBytes(data);
 #endif
-    _socket->write(data, size);
-    _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch());
+    if (!_socket)
+        return;
+
+    _socket->write(data);
+    _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch());
 }
 
 /**
diff --git a/src/comm/TCPLink.h b/src/comm/TCPLink.h
index ff9ac78d4a19da077e54b8fda3c312954a39e1b6..593e8e9313ff6011e4d75347934e0e9e9cccf2b3 100644
--- a/src/comm/TCPLink.h
+++ b/src/comm/TCPLink.h
@@ -152,10 +152,11 @@ public:
     bool connect(void);
     bool disconnect(void);
 
-public slots:
-
+private slots:
     // From LinkInterface
-    void writeBytes(const char* data, qint64 length);
+    void _writeBytes(const QByteArray data);
+
+public slots:
     void waitForBytesWritten(int msecs);
     void waitForReadyRead(int msecs);
 
@@ -182,7 +183,7 @@ private:
     void _restartConnection();
 
 #ifdef TCPLINK_READWRITE_DEBUG
-    void _writeDebugBytes(const char *data, qint16 size);
+    void _writeDebugBytes(const QByteArray data);
 #endif
 
     TCPConfiguration* _config;
diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc
index c3cedf5f1ba09672d477f9324e112ea952a91a6b..7f24c3872803266caa27eada3e89733103fe32e6 100644
--- a/src/comm/UDPLink.cc
+++ b/src/comm/UDPLink.cc
@@ -29,12 +29,6 @@ This file is part of the QGROUNDCONTROL project
  */
 
 #include <QtGlobal>
-#if QT_VERSION > 0x050401
-#define UDP_BROKEN_SIGNAL 1
-#else
-#define UDP_BROKEN_SIGNAL 0
-#endif
-
 #include <QTimer>
 #include <QList>
 #include <QDebug>
@@ -111,9 +105,6 @@ UDPLink::~UDPLink()
     quit();
     // Wait for it to exit
     wait();
-    while(_outQueue.count() > 0) {
-        delete _outQueue.dequeue();
-    }
     this->deleteLater();
 }
 
@@ -124,25 +115,7 @@ UDPLink::~UDPLink()
 void UDPLink::run()
 {
     if(_hardwareConnect()) {
-        if(UDP_BROKEN_SIGNAL) {
-            bool loop = false;
-            while(true) {
-                //-- Anything to read?
-                loop = _socket->hasPendingDatagrams();
-                if(loop) {
-                    readBytes();
-                }
-                //-- Loop right away if busy
-                if((_dequeBytes() || loop) && _running)
-                    continue;
-                if(!_running)
-                    break;
-                //-- Settle down (it gets here if there is nothing to read or write)
-                _socket->waitForReadyRead(5);
-            }
-        } else {
-            exec();
-        }
+        exec();
     }
     if (_socket) {
         _deregisterZeroconf();
@@ -174,36 +147,11 @@ void UDPLink::removeHost(const QString& host)
     _config->removeHost(host);
 }
 
-void UDPLink::writeBytes(const char* data, qint64 size)
+void UDPLink::_writeBytes(const QByteArray data)
 {
-    if (!_socket) {
+    if (!_socket)
         return;
-    }
-    if(UDP_BROKEN_SIGNAL) {
-        QByteArray* qdata = new QByteArray(data, size);
-        QMutexLocker lock(&_mutex);
-        _outQueue.enqueue(qdata);
-    } else {
-        _sendBytes(data, size);
-    }
-}
-
 
-bool UDPLink::_dequeBytes()
-{
-    QMutexLocker lock(&_mutex);
-    if(_outQueue.count() > 0) {
-        QByteArray* qdata = _outQueue.dequeue();
-        lock.unlock();
-        _sendBytes(qdata->data(), qdata->size());
-        delete qdata;
-        lock.relock();
-    }
-    return (_outQueue.count() > 0);
-}
-
-void UDPLink::_sendBytes(const char* data, qint64 size)
-{
     QStringList goneHosts;
     // Send to all connected systems
     QString host;
@@ -211,7 +159,7 @@ void UDPLink::_sendBytes(const char* data, qint64 size)
     if(_config->firstHost(host, port)) {
         do {
             QHostAddress currentHost(host);
-            if(_socket->writeDatagram(data, size, currentHost, (quint16)port) < 0) {
+            if(_socket->writeDatagram(data, currentHost, (quint16)port) < 0) {
                 // This host is gone. Add to list to be removed
                 // We should keep track of hosts that were manually added (static) and
                 // hosts that were added because we heard from them (dynamic). Only
@@ -225,7 +173,7 @@ void UDPLink::_sendBytes(const char* data, qint64 size)
                 // "host not there" takes time too regardless of size of data. In fact,
                 // 1 byte or "UDP frame size" bytes are the same as that's the data
                 // unit sent by UDP.
-                _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch());
+                _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch());
             }
         } while (_config->nextHost(host, port));
         //-- Remove hosts that are no longer there
@@ -260,8 +208,6 @@ void UDPLink::readBytes()
         // would trigger this.
         // Add host to broadcast list if not yet present, or update its port
         _config->addHost(sender.toString(), (int)senderPort);
-        if(UDP_BROKEN_SIGNAL && !_running)
-            break;
     }
     //-- Send whatever is left
     if(databuffer.size()) {
@@ -326,10 +272,7 @@ bool UDPLink::_hardwareConnect()
         _socket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, 512 * 1024);
 #endif
         _registerZeroconf(_config->localPort(), kZeroconfRegistration);
-        //-- Connect signal if this version of Qt is not broken
-        if(!UDP_BROKEN_SIGNAL) {
-            QObject::connect(_socket, &QUdpSocket::readyRead, this, &UDPLink::readBytes);
-        }
+        QObject::connect(_socket, &QUdpSocket::readyRead, this, &UDPLink::readBytes);
         emit connected();
     } else {
         emit communicationError("UDP Link Error", "Error binding UDP port");
diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h
index e354d1ad02a07c5ec54f17c09672d01fbd81612e..23c1151c7a0c5ae2f8cfc48774163ecc81510ff1 100644
--- a/src/comm/UDPLink.h
+++ b/src/comm/UDPLink.h
@@ -201,13 +201,14 @@ public slots:
 
     void readBytes();
 
+private slots:
     /*!
      * @brief Write a number of bytes to the interface.
      *
      * @param data Pointer to the data byte array
      * @param size The size of the bytes array
      **/
-    void writeBytes(const char* data, qint64 length);
+    void _writeBytes(const QByteArray data);
 
 protected:
 
@@ -235,12 +236,6 @@ private:
 #endif
 
     bool                _running;
-    QMutex              _mutex;
-    QQueue<QByteArray*> _outQueue;
-
-    bool _dequeBytes    ();
-    void _sendBytes     (const char* data, qint64 size);
-
 };
 
 #endif // UDPLINK_H
diff --git a/src/comm/XbeeLink.cpp b/src/comm/XbeeLink.cpp
index d365eb85d65f0b1bf60aeccf06c0ba63b9ad1eca..c3a7b153790bfbc4da8e5453868b385e70f6df95 100644
--- a/src/comm/XbeeLink.cpp
+++ b/src/comm/XbeeLink.cpp
@@ -172,17 +172,11 @@ void XbeeLink::_disconnect(void)
 	emit disconnected();
 }
 
-void XbeeLink::writeBytes(const char *bytes, qint64 length)  // TO DO: delete the data array
+void XbeeLink::_writeBytes(const QByteArray bytes)
 {
-	char *data;
-	data = new char[length];
-	for(long i=0;i<length;i++)
+	if(!xbee_nsenddata(this->m_xbeeCon,const_cast<char*>(bytes.data()),bytes.size())) // return value of 0 is successful written
 	{
-		data[i] = bytes[i];
-	}
-	if(!xbee_nsenddata(this->m_xbeeCon,data,length)) // return value of 0 is successful written
-	{
-		_logOutputDataRate(length, QDateTime::currentMSecsSinceEpoch());
+		_logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch());
 	}
 	else
 	{
diff --git a/src/comm/XbeeLink.h b/src/comm/XbeeLink.h
index 78048be7ddac2a10953497727eaed157b17797e3..02180dd254415dfa0043dbbba2e774c6afe7cdc2 100644
--- a/src/comm/XbeeLink.h
+++ b/src/comm/XbeeLink.h
@@ -45,8 +45,8 @@ public:
     qint64 getCurrentOutDataRate() const;
     qint64 getCurrentInDataRate() const;
 
-public slots: // virtual functions from LinkInterface
-	void writeBytes(const char *bytes, qint64 length);
+private slots: // virtual functions from LinkInterface
+	void _writeBytes(const QByteArray bytes);
 
 protected slots: // virtual functions from LinkInterface
 	void readBytes();
diff --git a/src/qgcunittest/TCPLinkTest.cc b/src/qgcunittest/TCPLinkTest.cc
index 7d2c99f68d5ff277bc14790714db3054055c2a99..45319d90df69227dce6ac2e2a9f02e015f1963a1 100644
--- a/src/qgcunittest/TCPLinkTest.cc
+++ b/src/qgcunittest/TCPLinkTest.cc
@@ -154,7 +154,7 @@ void TCPLinkUnitTest::_connectSucceed_test(void)
     const char* bytesWrittenSignal = SIGNAL(bytesWritten(qint64));
     MultiSignalSpy bytesWrittenSpy;
     QCOMPARE(bytesWrittenSpy.init(_link->getSocket(), &bytesWrittenSignal, 1), true);
-    _link->writeBytes(bytesOut.data(), bytesOut.size());
+    _link->writeBytesSafe(bytesOut.data(), bytesOut.size());
     _multiSpy->clearAllSignals();
     
     // We emit this signal such that it will be queued and run on the TCPLink thread. This in turn