diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index a9f654859db3b49df093c906bdba5af6ef0ea29b..450f45c1c4c7ba7cbb2523c354457d1554c44d83 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -44,7 +44,7 @@ Q_DECLARE_METATYPE(mavlink_message_t)
  * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
  */
 MAVLinkProtocol::MAVLinkProtocol() :
-    heartbeatTimer(),
+    heartbeatTimer(NULL),
     heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
     m_heartbeatsEnabled(true),
     m_multiplexingEnabled(false),
@@ -58,17 +58,14 @@ MAVLinkProtocol::MAVLinkProtocol() :
     m_actionGuardEnabled(false),
     m_actionRetransmissionTimeout(100),
     versionMismatchIgnore(false),
-    systemId(QGC::defaultSystemId)
+    systemId(QGC::defaultSystemId),
+    _should_exit(false)
 {
     qRegisterMetaType<mavlink_message_t>("mavlink_message_t");
 
     m_authKey = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx";
     loadSettings();
     moveToThread(this);
-    heartbeatTimer.moveToThread(this);
-    // Start heartbeat timer, emitting a heartbeat at the configured rate
-    connect(&heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
-    heartbeatTimer.start(1000/heartbeatRate);
 
     // All the *Counter variables are not initialized here, as they should be initialized
     // on a per-link basis before those links are used. @see resetMetadataForLink().
@@ -171,7 +168,7 @@ MAVLinkProtocol::~MAVLinkProtocol()
     }
 
     // Tell the thread to exit
-    quit();
+    _should_exit = true;
     // Wait for it to exit
     wait();
 }
@@ -182,7 +179,22 @@ MAVLinkProtocol::~MAVLinkProtocol()
  **/
 void MAVLinkProtocol::run()
 {
-    exec();
+    heartbeatTimer = new QTimer();
+    heartbeatTimer->moveToThread(this);
+    // Start heartbeat timer, emitting a heartbeat at the configured rate
+    connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
+    heartbeatTimer->start(1000/heartbeatRate);
+
+    while(!_should_exit) {
+
+        if (isFinished()) {
+            qDebug() << "MAVLINK WORKER DONE!";
+            return;
+        }
+
+        QCoreApplication::processEvents();
+        QGC::SLEEP::msleep(2);
+    }
 }
 
 QString MAVLinkProtocol::getLogfileName()
@@ -782,7 +794,7 @@ void MAVLinkProtocol::enableVersionCheck(bool enabled)
 void MAVLinkProtocol::setHeartbeatRate(int rate)
 {
     heartbeatRate = rate;
-    heartbeatTimer.setInterval(1000/heartbeatRate);
+    heartbeatTimer->setInterval(1000/heartbeatRate);
 }
 
 /** @return heartbeat rate in Hertz */
diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h
index a2e106316effe26fa4be152d78af0286ea9dbcf2..158890dbab600a9a71fcebb15e254d288f398056 100644
--- a/src/comm/MAVLinkProtocol.h
+++ b/src/comm/MAVLinkProtocol.h
@@ -214,7 +214,7 @@ public slots:
     void storeSettings();
 
 protected:
-    QTimer heartbeatTimer;    ///< Timer to emit heartbeats
+    QTimer *heartbeatTimer;    ///< Timer to emit heartbeats
     int heartbeatRate;         ///< Heartbeat rate, controls the timer interval
     bool m_heartbeatsEnabled;  ///< Enabled/disable heartbeat emission
     bool m_multiplexingEnabled; ///< Enable/disable packet multiplexing
@@ -237,6 +237,8 @@ protected:
     int currLossCounter[MAVLINK_COMM_NUM_BUFFERS];        ///< Lost messages during this sample time window. Used for calculating loss %.
     bool versionMismatchIgnore;
     int systemId;
+    bool _should_exit;
+
 #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
     mavlink::ProtobufManager protobufManager;
 #endif
diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc
index e7bd6795644c95e10eefb3aa5bb7e19c93d224d9..b30b5216e46ea1c14fa815c2c2caa6b8479a936c 100644
--- a/src/comm/QGCXPlaneLink.cc
+++ b/src/comm/QGCXPlaneLink.cc
@@ -57,7 +57,8 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress
     simUpdateLastText(QGC::groundTimeMilliseconds()),
     simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()),
     simUpdateHz(0),
-    _sensorHilEnabled(true)
+    _sensorHilEnabled(true),
+    _should_exit(false)
 {
     // We're doing it wrong - because the Qt folks got the API wrong:
     // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
@@ -75,7 +76,7 @@ QGCXPlaneLink::~QGCXPlaneLink()
 {
     storeSettings();
     // Tell the thread to exit
-    quit();
+    _should_exit = true;
     // Wait for it to exit
     wait();
 
@@ -216,7 +217,10 @@ void QGCXPlaneLink::run()
 
     writeBytes((const char*)&ip, sizeof(ip));
 
-    exec();
+    while(!_should_exit) {
+        QCoreApplication::processEvents();
+        QGC::SLEEP::msleep(5);
+    }
 }
 
 void QGCXPlaneLink::setPort(int localPort)
diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h
index c5506db3f4c52906d0cb466d17e03fb6be8aee83..ae4497432da0a51d824d9e438b9270f0871491b6 100644
--- a/src/comm/QGCXPlaneLink.h
+++ b/src/comm/QGCXPlaneLink.h
@@ -210,6 +210,7 @@ protected:
     quint64 simUpdateLastGroundTruth;
     float simUpdateHz;
     bool _sensorHilEnabled;
+    bool _should_exit;
 
     void setName(QString name);
 };
diff --git a/src/configuration.h b/src/configuration.h
index 607af7d6f1e386f0c329ff7a9ccc1a3af9568eff..bbfa865eec7d14faf3aba1619be0ba1bbcb66d99 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -4,7 +4,7 @@
 #include <QString>
 
 /** @brief Polling interval in ms */
-#define SERIAL_POLL_INTERVAL 5
+#define SERIAL_POLL_INTERVAL 4
 
 /** @brief Heartbeat emission rate, in Hertz (times per second) */
 #define MAVLINK_HEARTBEAT_DEFAULT_RATE 1
diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc
index c55648c76fa4f35b382d460b716d2758aa4551f8..5a607c89076760dbb55dc9cf7454ede2f72b0000 100644
--- a/src/uas/QGCMAVLinkUASFactory.cc
+++ b/src/uas/QGCMAVLinkUASFactory.cc
@@ -32,8 +32,6 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         // Set the system type
         mav->setSystemType((int)heartbeat->type);
 
-        mav->moveToThread(worker);
-
         // Connect this robot to the UAS object
         connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
 #ifdef QGC_PROTOBUF_ENABLED
@@ -48,8 +46,6 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         // Set the system type
         mav->setSystemType((int)heartbeat->type);
 
-        mav->moveToThread(worker);
-
         // Connect this robot to the UAS object
         // it is IMPORTANT here to use the right object type,
         // else the slot of the parent object is called (and thus the special
@@ -67,8 +63,6 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         // Set the system type
         mav->setSystemType((int)heartbeat->type);
 
-        mav->moveToThread(worker);
-
         // Connect this robot to the UAS object
         // it is IMPORTANT here to use the right object type,
         // else the slot of the parent object is called (and thus the special
@@ -83,8 +77,6 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
         // Set the system type
         mav->setSystemType((int)heartbeat->type);
 
-        mav->moveToThread(worker);
-
         // Connect this robot to the UAS object
         // it is IMPORTANT here to use the right object type,
         // else the slot of the parent object is called (and thus the special
@@ -121,6 +113,10 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
     break;
     }
 
+    // Get the UAS ready
+    worker->start(QThread::HighPriority);
+    connect(uas, SIGNAL(destroyed()), worker, SLOT(quit()));
+
     // Set the autopilot type
     uas->setAutopilotType((int)heartbeat->autopilot);
 
@@ -130,8 +126,5 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
     // Now add UAS to "official" list, which makes the whole application aware of it
     UASManager::instance()->addUAS(uas);
 
-    worker->start(QThread::HighPriority);
-    connect(uas, SIGNAL(destroyed()), worker, SLOT(quit()));
-
     return uas;
 }
diff --git a/src/uas/QGCUASWorker.cc b/src/uas/QGCUASWorker.cc
index 6fb74b150d534cbc4f5a60c836a496c384d84cf6..b726e120b40b14b8988bafb84f272d41b3ec7a31 100644
--- a/src/uas/QGCUASWorker.cc
+++ b/src/uas/QGCUASWorker.cc
@@ -1,12 +1,24 @@
 #include "QGCUASWorker.h"
 
 #include <QGC.h>
+#include <QCoreApplication>
+#include <QDebug>
 
-QGCUASWorker::QGCUASWorker() : QThread()
+QGCUASWorker::QGCUASWorker() : QThread(),
+    _should_exit(false)
 {
 }
 
+void QGCUASWorker::quit()
+{
+    _should_exit = true;
+}
+
 void QGCUASWorker::run()
 {
-    QGC::SLEEP::msleep(100);
+    while(!_should_exit) {
+
+        QCoreApplication::processEvents();
+        QGC::SLEEP::msleep(2);
+    }
 }
diff --git a/src/uas/QGCUASWorker.h b/src/uas/QGCUASWorker.h
index a48672868d720c8ac973ccf50d8f194ab03f6d7b..39ea5f011dc923ae9e29b7da057e94ec7525ceae 100644
--- a/src/uas/QGCUASWorker.h
+++ b/src/uas/QGCUASWorker.h
@@ -8,8 +8,13 @@ class QGCUASWorker : public QThread
 public:
     QGCUASWorker();
 
+public slots:
+    void quit();
+
 protected:
     void run();
+
+    bool _should_exit;
 };
 
 #endif // QGCUASWORKER_H
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index fbc4bb3b2f48925e0792d870b83dba882b2e6891..c018e4f954047152a70dfe4eb6b66e09118d8122 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -51,7 +51,7 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
     commStatus(COMM_DISCONNECTED),
     receiveDropRate(0),
     sendDropRate(0),
-    statusTimeout(new QTimer()),
+    statusTimeout(thread),
 
     name(""),
     type(MAV_TYPE_GENERIC),
@@ -164,12 +164,13 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
     hilEnabled(false),
     sensorHil(false),
     lastSendTimeGPS(0),
-    lastSendTimeSensors(0)
+    lastSendTimeSensors(0),
+    _thread(thread)
 {
     moveToThread(thread);
     waypointManager.moveToThread(thread);
     paramMgr.moveToThread(thread);
-    statusTimeout->moveToThread(thread);
+    statusTimeout.moveToThread(thread);
 
     for (unsigned int i = 0; i<255;++i)
     {
@@ -237,9 +238,9 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
 
     color = UASInterface::getNextColor();
     setBatterySpecs(QString(""));
-    connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState()));
+    connect(&statusTimeout, SIGNAL(timeout()), this, SLOT(updateState()));
     connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings()));
-    statusTimeout->start(500);
+    statusTimeout.start(500);
     readSettings();
     //need to init paramMgr after readSettings have been loaded, to properly set autopilot and so forth
     paramMgr.initWithUAS(this);
@@ -255,8 +256,11 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
 UAS::~UAS()
 {
     writeSettings();
+
+    _thread->quit();
+    _thread->wait();
+
     delete links;
-    delete statusTimeout;
     delete simulation;
 }
 
@@ -369,6 +373,7 @@ void UAS::updateState()
             GAudioOutput::instance()->notifyNegative();
         }
     }
+    qDebug() << "UPDATE STATE:" << (heartbeatInterval / 1000) << "milliseconds, LOST:" << connectionLost;
 }
 
 /**
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 6c4c45cb6b791a50a242b40eb751275f0efc90cd..6b57f9c88105fc99a7c704b05b392be481d228e8 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -382,7 +382,7 @@ protected: //COMMENTS FOR TEST UNIT
     float receiveDropRate;        ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs)
     float sendDropRate;           ///< Percentage of packets that were not received from the MAV by the GCS
     quint64 lastHeartbeat;        ///< Time of the last heartbeat message
-    QTimer* statusTimeout;        ///< Timer for various status timeouts
+    QTimer statusTimeout;       ///< Timer for various status timeouts
 
     /// BASIC UAS TYPE, NAME AND STATE
     QString name;                 ///< Human-friendly name of the vehicle, e.g. bravo
@@ -526,6 +526,7 @@ protected: //COMMENTS FOR TEST UNIT
 
     /// SIMULATION
     QGCHilLink* simulation;         ///< Hardware in the loop simulation link
+    QThread* _thread;
 
 public:
     /** @brief Set the current battery type */
diff --git a/src/ui/QGCHilXPlaneConfiguration.cc b/src/ui/QGCHilXPlaneConfiguration.cc
index 4166d7e662c04d5cd449580303676953691b4165..dc1cd52b6ac278818cec038382119fe5272e3171 100644
--- a/src/ui/QGCHilXPlaneConfiguration.cc
+++ b/src/ui/QGCHilXPlaneConfiguration.cc
@@ -25,8 +25,9 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QWidget *
     {
 //        connect(ui->randomAttitudeButton, SIGNAL(clicked()), link, SLOT(setRandomAttitude()));
 //        connect(ui->randomPositionButton, SIGNAL(clicked()), link, SLOT(setRandomPosition()));
-        connect(ui->airframeComboBox, SIGNAL(activated(QString)), link, SLOT(selectAirframe(QString)));
+
         ui->airframeComboBox->setCurrentIndex(link->getAirFrameIndex());
+        connect(ui->airframeComboBox, SIGNAL(activated(QString)), link, SLOT(selectAirframe(QString)));
         // XXX not implemented yet
         ui->airframeComboBox->hide();
         ui->sensorHilCheckBox->setChecked(xplane->sensorHilEnabled());