diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 135006d5470bc28c8ca2d47dacfbf2e492015b6f..f52faf0a72f3a7453b49f6a07e17f79d05499694 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -562,7 +562,8 @@ HEADERS += \
     src/ui/designer/QGCXYPlot.h \
     src/ui/menuactionhelper.h \
     src/uas/UASManagerInterface.h \
-    src/uas/QGCUASParamManagerInterface.h
+    src/uas/QGCUASParamManagerInterface.h \
+    src/uas/QGCUASWorker.h
 
 SOURCES += \
     src/main.cc \
@@ -745,4 +746,5 @@ SOURCES += \
     src/ui/px4_configuration/QGCPX4MulticopterConfig.cc \
     src/ui/px4_configuration/QGCPX4SensorCalibration.cc \
     src/ui/designer/QGCXYPlot.cc \
-    src/ui/menuactionhelper.cpp
+    src/ui/menuactionhelper.cpp \
+    src/uas/QGCUASWorker.cc
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 b7658ae55ac48df78ea82ffa12a3002e0c23338f..5a607c89076760dbb55dc9cf7454ede2f72b0000 100644
--- a/src/uas/QGCMAVLinkUASFactory.cc
+++ b/src/uas/QGCMAVLinkUASFactory.cc
@@ -1,5 +1,6 @@
 #include "QGCMAVLinkUASFactory.h"
 #include "UASManager.h"
+#include "QGCUASWorker.h"
 
 QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) :
     QObject(parent)
@@ -21,7 +22,7 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
 
     UASInterface* uas;
 
-    QThread* worker = new QThread();
+    QGCUASWorker* worker = new QGCUASWorker();
 
     switch (heartbeat->autopilot)
     {
@@ -31,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
@@ -47,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
@@ -66,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
@@ -82,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
@@ -120,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);
 
@@ -129,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
new file mode 100644
index 0000000000000000000000000000000000000000..b726e120b40b14b8988bafb84f272d41b3ec7a31
--- /dev/null
+++ b/src/uas/QGCUASWorker.cc
@@ -0,0 +1,24 @@
+#include "QGCUASWorker.h"
+
+#include <QGC.h>
+#include <QCoreApplication>
+#include <QDebug>
+
+QGCUASWorker::QGCUASWorker() : QThread(),
+    _should_exit(false)
+{
+}
+
+void QGCUASWorker::quit()
+{
+    _should_exit = true;
+}
+
+void QGCUASWorker::run()
+{
+    while(!_should_exit) {
+
+        QCoreApplication::processEvents();
+        QGC::SLEEP::msleep(2);
+    }
+}
diff --git a/src/uas/QGCUASWorker.h b/src/uas/QGCUASWorker.h
new file mode 100644
index 0000000000000000000000000000000000000000..39ea5f011dc923ae9e29b7da057e94ec7525ceae
--- /dev/null
+++ b/src/uas/QGCUASWorker.h
@@ -0,0 +1,20 @@
+#ifndef QGCUASWORKER_H
+#define QGCUASWORKER_H
+
+#include <QThread>
+
+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 0ff565db25b964b0403e682d5e76409b0bcb01df..a338d43cbeadaa4e576de160ea924da418e7ca32 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(this)),
+    statusTimeout(thread),
 
     name(""),
     type(MAV_TYPE_GENERIC),
@@ -164,12 +164,10 @@ 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);
 
     for (unsigned int i = 0; i<255;++i)
     {
@@ -180,66 +178,66 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
     // Store a list of available actions for this UAS.
     // Basically everything exposed as a SLOT with no return value or arguments.
 
-    QAction* newAction = new QAction(tr("Arm"), this);
+    QAction* newAction = new QAction(tr("Arm"), thread);
     newAction->setToolTip(tr("Enable the UAS so that all actuators are online"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(armSystem()));
     actions.append(newAction);
 
-    newAction = new QAction(tr("Disarm"), this);
+    newAction = new QAction(tr("Disarm"), thread);
     newAction->setToolTip(tr("Disable the UAS so that all actuators are offline"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(disarmSystem()));
     actions.append(newAction);
 
-    newAction = new QAction(tr("Toggle armed"), this);
+    newAction = new QAction(tr("Toggle armed"), thread);
     newAction->setToolTip(tr("Toggle between armed and disarmed"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy()));
     actions.append(newAction);
 
-    newAction = new QAction(tr("Go home"), this);
+    newAction = new QAction(tr("Go home"), thread);
     newAction->setToolTip(tr("Command the UAS to return to its home position"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(home()));
     actions.append(newAction);
 
-    newAction = new QAction(tr("Land"), this);
+    newAction = new QAction(tr("Land"), thread);
     newAction->setToolTip(tr("Command the UAS to land"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(land()));
     actions.append(newAction);
 
-    newAction = new QAction(tr("Launch"), this);
+    newAction = new QAction(tr("Launch"), thread);
     newAction->setToolTip(tr("Command the UAS to launch itself and begin its mission"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(launch()));
     actions.append(newAction);
 
-    newAction = new QAction(tr("Resume"), this);
+    newAction = new QAction(tr("Resume"), thread);
     newAction->setToolTip(tr("Command the UAS to continue its mission"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(go()));
     actions.append(newAction);
 
-    newAction = new QAction(tr("Stop"), this);
+    newAction = new QAction(tr("Stop"), thread);
     newAction->setToolTip(tr("Command the UAS to halt and hold position"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(halt()));
     actions.append(newAction);
 
-    newAction = new QAction(tr("Go autonomous"), this);
+    newAction = new QAction(tr("Go autonomous"), thread);
     newAction->setToolTip(tr("Set the UAS into an autonomous control mode"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(goAutonomous()));
     actions.append(newAction);
 
-    newAction = new QAction(tr("Go manual"), this);
+    newAction = new QAction(tr("Go manual"), thread);
     newAction->setToolTip(tr("Set the UAS into a manual control mode"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(goManual()));
     actions.append(newAction);
 
-    newAction = new QAction(tr("Toggle autonomy"), this);
+    newAction = new QAction(tr("Toggle autonomy"), thread);
     newAction->setToolTip(tr("Toggle between manual and full-autonomy"));
     connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy()));
     actions.append(newAction);
 
     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 +253,11 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
 UAS::~UAS()
 {
     writeSettings();
+
+    _thread->quit();
+    _thread->wait();
+
     delete links;
-    delete statusTimeout;
     delete simulation;
 }
 
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/MainWindow.cc b/src/ui/MainWindow.cc
index 70a7e9a0e4ee1b529a31c8f90cd8467248b3791e..da0a84964439e6738ec2f5d6137421abeba92156 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -760,6 +760,7 @@ void MainWindow::loadDockWidget(const QString& name)
 {
     if(menuActionHelper->containsDockWidget(currentView, name))
         return;
+
     if (name.startsWith("HIL_CONFIG"))
     {
         //It's a HIL widget.
@@ -826,7 +827,7 @@ void MainWindow::loadDockWidget(const QString& name)
     }
     else if (name == "HEAD_UP_DISPLAY_DOCKWIDGET")
     {
-        createDockWidget(centerStack->currentWidget(),new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",currentView,Qt::RightDockWidgetArea);
+        createDockWidget(centerStack->currentWidget(),new HUD(320,240,this),tr("Video Downlink"),"HEAD_UP_DISPLAY_DOCKWIDGET",currentView,Qt::RightDockWidgetArea);
     }
     else if (name == "UAS_INFO_QUICKVIEW_DOCKWIDGET")
     {
diff --git a/src/ui/PrimaryFlightDisplay.cc b/src/ui/PrimaryFlightDisplay.cc
index 35063a38065519e3fb6f17b92fc0e236fee8e756..7997a1d3f1190203d9703d66e2b43ecb59809569 100644
--- a/src/ui/PrimaryFlightDisplay.cc
+++ b/src/ui/PrimaryFlightDisplay.cc
@@ -141,7 +141,9 @@ PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *paren
     instrumentOpagueBackground(QColor::fromHsvF(0, 0, 0.3, 1.0)),
 
     font("Bitstream Vera Sans"),
-    refreshTimer(new QTimer(this))
+    refreshTimer(new QTimer(this)),
+    _valuesChanged(false),
+    _valuesLastPainted(QGC::groundTimeMilliseconds())
 {
     Q_UNUSED(width);
     Q_UNUSED(height);
@@ -159,7 +161,7 @@ PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *paren
     // Refresh timer
     refreshTimer->setInterval(updateInterval);
     //    connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD()));
-    connect(refreshTimer, SIGNAL(timeout()), this, SLOT(update()));
+    connect(refreshTimer, SIGNAL(timeout()), this, SLOT(checkUpdate()));
 }
 
 PrimaryFlightDisplay::~PrimaryFlightDisplay()
@@ -224,6 +226,15 @@ void PrimaryFlightDisplay::paintEvent(QPaintEvent *event)
     doPaint();
 }
 
+void PrimaryFlightDisplay::checkUpdate()
+{
+    if (uas && (_valuesChanged || (QGC::groundTimeMilliseconds() - _valuesLastPainted) > 260)) {
+        update();
+        _valuesChanged = false;
+        _valuesLastPainted = QGC::groundTimeMilliseconds();
+    }
+}
+
 ///*
 // * Interface towards qgroundcontrol
 // */
@@ -280,24 +291,45 @@ void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, double roll, double
 {
     Q_UNUSED(uas);
     Q_UNUSED(timestamp);
+
         // Called from UAS.cc l. 616
         if (isinf(roll)) {
             this->roll = std::numeric_limits<double>::quiet_NaN();
         } else {
-            this->roll = roll * (180.0 / M_PI);
+
+            float rolldeg = roll * (180.0 / M_PI);
+
+            if (fabsf(roll - rolldeg) > 2.5f) {
+                _valuesChanged = true;
+            }
+
+            this->roll = rolldeg;
         }
 
         if (isinf(pitch)) {
             this->pitch = std::numeric_limits<double>::quiet_NaN();
         } else {
-            this->pitch = pitch * (180.0 / M_PI);
+
+            float pitchdeg = pitch * (180.0 / M_PI);
+
+            if (fabsf(pitch - pitchdeg) > 2.5f) {
+                _valuesChanged = true;
+            }
+
+            this->pitch = pitchdeg;
         }
 
         if (isinf(yaw)) {
             this->heading = std::numeric_limits<double>::quiet_NaN();
         } else {
+
             yaw = yaw * (180.0 / M_PI);
             if (yaw<0) yaw+=360;
+
+            if (fabsf(heading - yaw) > 10.0f) {
+                _valuesChanged = true;
+            }
+
             this->heading = yaw;
         }
 
@@ -314,6 +346,14 @@ void PrimaryFlightDisplay::updateSpeed(UASInterface* uas, double _groundSpeed, d
     Q_UNUSED(uas);
     Q_UNUSED(timestamp);
 
+    if (fabsf(groundSpeed - _groundSpeed) > 0.5f) {
+        _valuesChanged = true;
+    }
+
+    if (fabsf(airSpeed - _airSpeed) > 1.0f) {
+        _valuesChanged = true;
+    }
+
     groundSpeed = _groundSpeed;
     airSpeed = _airSpeed;
 }
@@ -321,6 +361,19 @@ void PrimaryFlightDisplay::updateSpeed(UASInterface* uas, double _groundSpeed, d
 void PrimaryFlightDisplay::updateAltitude(UASInterface* uas, double _altitudeAMSL, double _altitudeRelative, double _climbRate, quint64 timestamp) {
     Q_UNUSED(uas);
     Q_UNUSED(timestamp);
+
+    if (fabsf(altitudeAMSL - _altitudeAMSL) > 0.5f) {
+        _valuesChanged = true;
+    }
+
+    if (fabsf(altitudeRelative - _altitudeRelative) > 0.5f) {
+        _valuesChanged = true;
+    }
+
+    if (fabsf(climbRate - _climbRate) > 0.5f) {
+        _valuesChanged = true;
+    }
+
     altitudeAMSL = _altitudeAMSL;
     altitudeRelative = _altitudeRelative;
     climbRate = _climbRate;
diff --git a/src/ui/PrimaryFlightDisplay.h b/src/ui/PrimaryFlightDisplay.h
index 9dfb8c3ecf2f6c9b9931d1701b39618f758a0a1b..b4236e339a400cc994d7de0613d2025a3be7f16e 100644
--- a/src/ui/PrimaryFlightDisplay.h
+++ b/src/ui/PrimaryFlightDisplay.h
@@ -27,7 +27,13 @@ public slots:
     void forgetUAS(UASInterface* uas);
     void setActiveUAS(UASInterface* uas);
 
+    void checkUpdate();
+
 protected:
+
+    bool _valuesChanged;
+    quint64 _valuesLastPainted;
+
     enum Layout {
         COMPASS_INTEGRATED,
         COMPASS_SEPARATED               // For a very high container. Feature panels are at bottom.
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());