diff --git a/README.md b/README.md
index 5771904be0d5dcb1bb2578b527d14f701fe481df..2f1b49674fd8b93b882bb92ccf6e2a7c3f7076d9 100644
--- a/README.md
+++ b/README.md
@@ -90,7 +90,7 @@ To build on Linux:
 - - -
 1. Install base dependencies (QT + phonon/webkit, SDL)
   * For Ubuntu: `sudo apt-get install libqt4-dev libphonon-dev libphonon4 phonon-backend-gstreamer qtcreator libsdl1.2-dev build-essential libudev-dev`
-  * For Fedora: `sudo yum install qt qt-creator qt-webkit-devel SDL-devel SDL-static systemd-devel`
+  * For Fedora: `sudo yum install qt qt-creator qt-webkit-devel phonon-devel SDL-devel SDL-static systemd-devel`
   * For Arch Linux: `pacman -Sy qtwebkit phonon-qt4`
 
 2. **[OPTIONAL]** Install additional libraries
diff --git a/src/QGC.cc b/src/QGC.cc
index 12dbcb688bbaf10f743a81312d5e61f1c8f1e0e3..3f1b6c2be33df3de4a3f6abffb52ad3605372aa8 100644
--- a/src/QGC.cc
+++ b/src/QGC.cc
@@ -30,29 +30,17 @@ namespace QGC
 
 quint64 groundTimeUsecs()
 {
-    QDateTime time = QDateTime::currentDateTime();
-    time = time.toUTC();
-    /* Return seconds and milliseconds, in milliseconds unit */
-    quint64 microseconds = time.toTime_t() * static_cast<quint64>(1000000);
-    return static_cast<quint64>(microseconds + (time.time().msec()*1000));
+    return groundTimeMilliseconds() * 1000;
 }
 
 quint64 groundTimeMilliseconds()
 {
-    QDateTime time = QDateTime::currentDateTime();
-    time = time.toUTC();
-    /* Return seconds and milliseconds, in milliseconds unit */
-    quint64 seconds = time.toTime_t() * static_cast<quint64>(1000);
-    return static_cast<quint64>(seconds + (time.time().msec()));
+    return static_cast<quint64>(QDateTime::currentMSecsSinceEpoch());
 }
 
 qreal groundTimeSeconds()
 {
-    QDateTime time = QDateTime::currentDateTime();
-    time = time.toUTC();
-    /* Return time in seconds unit */
-    quint64 seconds = time.toTime_t();
-    return static_cast<qreal>(seconds + (time.time().msec() / 1000.0));
+    return static_cast<qreal>(groundTimeMilliseconds()) / 1000.0f;
 }
 
 float limitAngleToPMPIf(float angle)
diff --git a/src/QGC.h b/src/QGC.h
index e49b2a4ce76c27b6452519d64740fc0466f6c634..c0a548a5e8db33b39259ad787e654c2623a0609b 100644
--- a/src/QGC.h
+++ b/src/QGC.h
@@ -75,11 +75,17 @@ const QColor colorDarkYellow(180, 180, 0);
 const QColor colorBackground("#050508");
 const QColor colorBlack(0, 0, 0);
 
-/** @brief Get the current ground time in microseconds */
+/**
+ * @brief Get the current ground time in microseconds.
+ * @note This does not have microsecond precision, it is limited to millisecond precision.
+ */
 quint64 groundTimeUsecs();
 /** @brief Get the current ground time in milliseconds */
 quint64 groundTimeMilliseconds();
-/** @brief Get the current ground time in seconds */
+/** 
+ * @brief Get the current ground time in fractional seconds
+ * @note Precision is limited to milliseconds.
+ */
 qreal groundTimeSeconds();
 /** @brief Returns the angle limited to -pi - pi */
 float limitAngleToPMPIf(float angle);
diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc
index ac628f91e332e2452b38bc07fa1802ac42e3b1df..d1e09d1dbf90c27388417c2e9dd7172f4b3d5693 100644
--- a/src/comm/SerialLink.cc
+++ b/src/comm/SerialLink.cc
@@ -99,19 +99,11 @@ QList<QString> SerialLink::getCurrentPorts()
     m_ports.clear();
 
     QList<QSerialPortInfo> portList =  QSerialPortInfo::availablePorts();
-
-    if( portList.count() == 0){
-        qDebug() << "No Ports Found" << m_ports;
-    }
-
     foreach (const QSerialPortInfo &info, portList)
     {
-//        qDebug() << "PortName    : " << info.portName()
-//                 << "Description : " << info.description();
-//        qDebug() << "Manufacturer: " << info.manufacturer();
-
         m_ports.append(info.portName());
     }
+
     return m_ports;
 }
 
diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc
index 7f1bc7d7e8bb0deec4b84e9242e087d248389412..22305874850719d7161c6a55d33e2524db961aa0 100644
--- a/src/input/JoystickInput.cc
+++ b/src/input/JoystickInput.cc
@@ -272,7 +272,7 @@ void JoystickInput::setActiveUAS(UASInterface* uas)
         tmp = dynamic_cast<UAS*>(this->uas);
         if(tmp)
         {
-            disconnect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int)));
+            disconnect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16)), tmp, SLOT(setManualControlCommands(float,float,float,float,qint8,qint8,quint16)));
             disconnect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
         }
         uasCanReverse = false;
@@ -288,7 +288,7 @@ void JoystickInput::setActiveUAS(UASInterface* uas)
 
     if (this->uas && (tmp = dynamic_cast<UAS*>(this->uas)))
     {
-        connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int)));
+        connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16)), tmp, SLOT(setManualControlCommands(float,float,float,float,qint8,qint8,quint16)));
         connect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
         uasCanReverse = tmp->systemCanReverse();
 
@@ -439,10 +439,10 @@ void JoystickInput::run()
 
         // Build up vectors describing the hat position
         int hatPosition = SDL_JoystickGetHat(joystick, 0);
-        int newYHat = 0;
+        qint8 newYHat = 0;
         if ((SDL_HAT_UP & hatPosition) > 0) newYHat = 1;
         if ((SDL_HAT_DOWN & hatPosition) > 0) newYHat = -1;
-        int newXHat = 0;
+        qint8 newXHat = 0;
         if ((SDL_HAT_LEFT & hatPosition) > 0) newXHat = -1;
         if ((SDL_HAT_RIGHT & hatPosition) > 0) newXHat = 1;
         if (newYHat != yHat || newXHat != xHat)
diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h
index 66070990d2e36e07203b27ef83c7eb2e6bbc9dc3..3bca5cbbab2c35d37424d28a300daa7ac6ea78fd 100644
--- a/src/input/JoystickInput.h
+++ b/src/input/JoystickInput.h
@@ -205,7 +205,7 @@ protected:
     // Track the last state of the axes, buttons, and hats for only emitting change signals.
     QList<float> joystickAxes; ///< The values of every axes during the last sample.
     quint16 joystickButtons;   ///< The state of every button. Bitfield supporting 16 buttons with 1s indicating that the button is down.
-    int xHat, yHat;            ///< The horizontal/vertical hat directions. Values are -1, 0, 1, with (-1,-1) indicating bottom-left.
+    qint8 xHat, yHat;            ///< The horizontal/vertical hat directions. Values are -1, 0, 1, with (-1,-1) indicating bottom-left.
 
     /**
      * @brief Called before main run() event loop starts. Waits for joysticks to be connected.
@@ -224,7 +224,7 @@ signals:
      * @param xHat hat vector in forward-backward direction, +1 forward, 0 center, -1 backward
      * @param yHat hat vector in left-right direction, -1 left, 0 center, +1 right
      */
-    void joystickChanged(double roll, double pitch, double yaw, double throttle, int xHat, int yHat, int buttons);
+    void joystickChanged(float roll, float pitch, float yaw, float throttle, qint8 xHat, qint8 yHat, quint16 buttons);
 
     /**
       * @brief Emit a new value for an axis
@@ -267,7 +267,7 @@ signals:
       * @param x vector in left-right direction
       * @param y vector in forward-backward direction
       */
-    void hatDirectionChanged(int x, int y);
+    void hatDirectionChanged(qint8 x, qint8 y);
 
     /** @brief Signal that the UAS has been updated for this JoystickInput
      * Note that any UI updates should NOT query this object for joystick details. That should be done in response to the joystickSettingsChanged signal.
diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc
index 047bfea71091fb9b862c78bbebf6a76f29f794a2..6928979d7cbee7deeddc95cffc3279cfce658ffa 100644
--- a/src/uas/SlugsMAV.cc
+++ b/src/uas/SlugsMAV.cc
@@ -126,19 +126,19 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
             switch (mlAction.actionId) {
             case SLUGS_ACTION_EEPROM:
                 if (mlAction.actionVal == SLUGS_ACTION_FAIL) {
-                    emit textMessageReceived(message.sysid, message.compid, 255, "EEPROM Write Fail, Data was not saved in Memory!");
+                    emit textMessageReceived(message.sysid, message.compid, MAV_SEVERITY_ERROR, "EEPROM Write Fail, Data was not saved in Memory!");
                 }
                 break;
 
             case SLUGS_ACTION_PT_CHANGE:
                 if (mlAction.actionVal == SLUGS_ACTION_SUCCESS) {
-                    emit textMessageReceived(message.sysid, message.compid, 0, "Passthrough Succesfully Changed");
+                    emit textMessageReceived(message.sysid, message.compid, MAV_SEVERITY_INFO, "Passthrough Succesfully Changed");
                 }
                 break;
 
             case SLUGS_ACTION_MLC_CHANGE:
                 if (mlAction.actionVal == SLUGS_ACTION_SUCCESS) {
-                    emit textMessageReceived(message.sysid, message.compid, 0, "Mid-level Commands Succesfully Changed");
+                    emit textMessageReceived(message.sysid, message.compid, MAV_SEVERITY_INFO, "Mid-level Commands Succesfully Changed");
                 }
                 break;
             }
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 64f133c8f730f7c685a44f96d5f1af6fe9bd7880..bb6e3c28ae612bbdf3bb903b6d627885c9e1dc7d 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -353,7 +353,7 @@ void UAS::updateState()
     // Connection gained
     if (connectionLost && (heartbeatInterval < timeoutIntervalHeartbeat))
     {
-        QString audiostring = QString("Link regained to system %1 after %2 seconds").arg(this->getUASID()).arg((int)(connectionLossTime/1000000));
+        QString audiostring = QString("Link regained to system %1").arg(this->getUASID());
         GAudioOutput::instance()->say(audiostring.toLower());
         connectionLost = false;
         connectionLossTime = 0;
@@ -958,7 +958,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                         setGroundSpeed(vel);
                         emit speedChanged(this, groundSpeed, airSpeed, time);
                     } else {
-                        emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
+                        emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
                     }
                 }
             }
@@ -1058,27 +1058,27 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
             case MAV_RESULT_ACCEPTED:
             {
-                emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed CMD: %1").arg(ack.command));
+                emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_INFO, tr("SUCCESS: Executed CMD: %1").arg(ack.command));
             }
                 break;
             case MAV_RESULT_TEMPORARILY_REJECTED:
             {
-                emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Temporarily rejected CMD: %1").arg(ack.command));
+                emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_WARNING, tr("FAILURE: Temporarily rejected CMD: %1").arg(ack.command));
             }
                 break;
             case MAV_RESULT_DENIED:
             {
-                emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Denied CMD: %1").arg(ack.command));
+                emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_ERROR, tr("FAILURE: Denied CMD: %1").arg(ack.command));
             }
                 break;
             case MAV_RESULT_UNSUPPORTED:
             {
-                emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Unsupported CMD: %1").arg(ack.command));
+                emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_WARNING, tr("FAILURE: Unsupported CMD: %1").arg(ack.command));
             }
                 break;
             case MAV_RESULT_FAILED:
             {
-                emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Failed CMD: %1").arg(ack.command));
+                emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_ERROR, tr("FAILURE: Failed CMD: %1").arg(ack.command));
             }
                 break;
             }
@@ -2735,7 +2735,7 @@ void UAS::requestParameter(int component, const QString& parameter)
     // Copy full param name or maximum max field size
     if (parameter.length() > MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN)
     {
-        emit textMessageReceived(uasId, 0, 255, QString("QGC WARNING: Parameter name %1 is more than %2 bytes long. This might lead to errors and mishaps!").arg(parameter).arg(MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN-1));
+        emit textMessageReceived(uasId, 0, MAV_SEVERITY_WARNING, QString("QGC WARNING: Parameter name %1 is more than %2 bytes long. This might lead to errors and mishaps!").arg(parameter).arg(MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN-1));
     }
     memcpy(read.param_id, parameter.toStdString().c_str(), qMax(parameter.length(), MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN));
     read.param_id[15] = '\0'; // Enforce null termination
@@ -2883,34 +2883,70 @@ void UAS::toggleAutonomy()
 * Set the manual control commands.
 * This can only be done if the system has manual inputs enabled and is armed.
 */
-void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons)
+void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons)
 {
     Q_UNUSED(xHat);
     Q_UNUSED(yHat);
 
-    // Scale values
-    double rollPitchScaling = 1.0f * 1000.0f;
-    double yawScaling = 1.0f * 1000.0f;
-    double thrustScaling = 1.0f * 1000.0f;
+    // Store the previous manual commands
+    static float manualRollAngle = 0.0;
+    static float manualPitchAngle = 0.0;
+    static float manualYawAngle = 0.0;
+    static float manualThrust = 0.0;
+    static quint16 manualButtons = 0;
+    static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission
 
-    manualRollAngle = roll * rollPitchScaling;
-    manualPitchAngle = pitch * rollPitchScaling;
-    manualYawAngle = yaw * yawScaling;
-    manualThrust = thrust * thrustScaling;
-
-    // If system has manual inputs enabled and is armed
+    // We only transmit manual command messages if the system has manual inputs enabled and is armed
     if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
     {
-        mavlink_message_t message;
-        mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualPitchAngle, (float)manualRollAngle, (float)manualThrust, (float)manualYawAngle, buttons);
-        sendMessage(message);
-        //qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
 
-        emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
-    }
-    else
-    {
-        //qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first";
+        // Transmit the manual commands only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with
+        // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate
+        // if no command inputs have changed.
+        // The default transmission rate is 50Hz, but when no inputs have changed it drops down to 5Hz.
+        bool sendCommand = false;
+        if (countSinceLastTransmission++ >= 10)
+        {
+            sendCommand = true;
+            countSinceLastTransmission = 0;
+        }
+        else if ((!isnan(roll) && roll != manualRollAngle) || (!isnan(pitch) && pitch != manualPitchAngle) ||
+                   (!isnan(yaw) && yaw != manualYawAngle) || (!isnan(thrust) && thrust != manualThrust) ||
+                   buttons != manualButtons)
+        {
+            sendCommand = true;
+
+            // Ensure that another message will be sent the next time this function is called
+            countSinceLastTransmission = 10;
+        }
+
+        // Now if we should trigger an update, let's do that
+        if (sendCommand)
+        {
+            // Save the new manual control inputs
+            manualRollAngle = roll;
+            manualPitchAngle = pitch;
+            manualYawAngle = yaw;
+            manualThrust = thrust;
+            manualButtons = buttons;
+
+            // Store scaling values for all 3 axes
+            const float axesScaling = 1.0 * 1000.0;
+
+            // Calculate the new commands for roll, pitch, yaw, and thrust
+            const float newRollCommand = roll * axesScaling;
+            const float newPitchCommand = pitch * axesScaling;
+            const float newYawCommand = yaw * axesScaling;
+            const float newThrustCommand = thrust * axesScaling;
+
+            // Send the MANUAL_COMMAND message
+            mavlink_message_t message;
+            mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
+            sendMessage(message);
+
+            // Emit an update in control values to other UI elements, like the HSI display
+            emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
+        }
     }
 }
 
@@ -3598,7 +3634,7 @@ void UAS::setBatterySpecs(const QString& specs)
         }
         else
         {
-            emit textMessageReceived(0, 0, 0, "Could not set battery options, format is wrong");
+            emit textMessageReceived(0, 0, MAV_SEVERITY_WARNING, "Could not set battery options, format is wrong");
         }
     }
     else
@@ -3624,7 +3660,7 @@ void UAS::setBatterySpecs(const QString& specs)
         }
         else
         {
-            emit textMessageReceived(0, 0, 0, "Could not set battery options, format is wrong");
+            emit textMessageReceived(0, 0, MAV_SEVERITY_WARNING, "Could not set battery options, format is wrong");
         }
     }
 }
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index ec992e5626598d733432a6783d66f21cb8874aab..880063655a2b53b1bd267897fdb9cd71e9d1b692 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -845,7 +845,7 @@ public slots:
     void toggleAutonomy();
 
     /** @brief Set the values for the manual control of the vehicle */
-    void setManualControlCommands(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons);
+    void setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons);
 
     /** @brief Set the values for the 6dof manual control of the vehicle */
     void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw);
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index ee7e019d2543cea8d67d0251d7120a06bcaa4022..d0ea85fb74797e6cea1bc8293c950595e945b799 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -533,7 +533,7 @@ signals:
     void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
     void attitudeChanged(UASInterface*, int component, double roll, double pitch, double yaw, quint64 usec);
     void attitudeRotationRatesChanged(int uas, double rollrate, double pitchrate, double yawrate, quint64 usec);
-    void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
+    void attitudeThrustSetPointChanged(UASInterface*, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec);
     /** @brief The MAV set a new setpoint in the local (not body) NED X, Y, Z frame */
     void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
     /** @brief A user (or an autonomous mission or obstacle avoidance planner) requested to set a new setpoint */
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index 4e05a479a6068578c6d521f623216e916f81289f..21d47924a5ab5eac2f5ced4ad6f2da7aacce2c27 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -921,7 +921,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
         disconnect(this->uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
         disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
-        disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
+        disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64)));
         disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
         disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
         disconnect(this->uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
@@ -957,8 +957,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
                 this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
         connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)),
                 this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
-        connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)),
-                this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
+        connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)),
+                this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64)));
         connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)),
                 this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
         connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)),
@@ -1100,7 +1100,7 @@ void HSIDisplay::sendBodySetPointCoordinates()
     }
 }
 
-void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec)
+void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec)
 {
     Q_UNUSED(uas);
     Q_UNUSED(usec);
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index 6ebfeb73880c4532fca3e752d087fd4b4a4faf54..b4edb3a25e9f4d8823cad0bd426678e3825d0947 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -55,7 +55,7 @@ public slots:
     /** @brief Set the width in meters this widget shows from top */
     void setMetricWidth(double width);
     void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
-    void updateAttitudeSetpoints(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
+    void updateAttitudeSetpoints(UASInterface*, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec);
     void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time);
     void updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
     void updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc
index 5b5150791f8d437107d26177ffeafcb0cce8804c..03df9be2ce40d8844d2ce9cffb4feb405d51309a 100644
--- a/src/ui/JoystickWidget.cc
+++ b/src/ui/JoystickWidget.cc
@@ -25,7 +25,7 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) :
     connect(this->joystick, SIGNAL(buttonPressed(int)), this, SLOT(joystickButtonPressed(int)));
     connect(this->joystick, SIGNAL(buttonReleased(int)), this, SLOT(joystickButtonReleased(int)));
     connect(this->joystick, SIGNAL(axisValueChanged(int,float)), this, SLOT(updateAxisValue(int,float)));
-    connect(this->joystick, SIGNAL(hatDirectionChanged(int,int)), this, SLOT(setHat(int,int)));
+    connect(this->joystick, SIGNAL(hatDirectionChanged(qint8,qint8)), this, SLOT(setHat(qint8,qint8)));
 
     // Also watch for when new settings were loaded for the current joystick to do a mass UI refresh.
     connect(this->joystick, SIGNAL(joystickSettingsChanged()), this, SLOT(updateUI()));
@@ -238,7 +238,7 @@ void JoystickWidget::updateAxisValue(int axis, float value)
     }
 }
 
-void JoystickWidget::setHat(int x, int y)
+void JoystickWidget::setHat(qint8 x, qint8 y)
 {
     m_ui->statusLabel->setText(tr("Hat position: x: %1, y: %2").arg(x).arg(y));
 }
diff --git a/src/ui/JoystickWidget.h b/src/ui/JoystickWidget.h
index c1eb1d51a88f9b5ac4937e0bc571c06fabc27bc5..71931b1e35c19670988ee0b06bc160b72904e12b 100644
--- a/src/ui/JoystickWidget.h
+++ b/src/ui/JoystickWidget.h
@@ -65,7 +65,7 @@ public slots:
     /** @brief Update the UI with new values for the hat.
      *  @see JoystickInput::hatDirectionChanged
      */
-    void setHat(int x, int y);
+    void setHat(qint8 x, qint8 y);
     /** @brief Trigger a UI change based on a button being pressed */
     void joystickButtonPressed(int key);
     /** @brief Trigger a UI change based on a button being released */
diff --git a/src/ui/MAVLinkDecoder.cc b/src/ui/MAVLinkDecoder.cc
index a0571cc06b4bb983d29da2f1e4bc08f66f1d270f..f59b5bbdb82c991f46e9c037a8934076ef726e12 100644
--- a/src/ui/MAVLinkDecoder.cc
+++ b/src/ui/MAVLinkDecoder.cc
@@ -302,7 +302,7 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
             // Enforce null termination
             str[messageInfo[msgid].fields[fieldid].array_length-1] = '\0';
             QString string(name + ": " + str);
-            if (!textMessageFilter.contains(msgid)) emit textMessageReceived(msg->sysid, msg->compid, 0, string);
+            if (!textMessageFilter.contains(msgid)) emit textMessageReceived(msg->sysid, msg->compid, MAV_SEVERITY_INFO, string);
         }
         else
         {
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index d862318539da12e53ce4d7f7aac3b8bd0adb5879..2883dadc3917b79d270eb576ceed8e2ff9372458 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -348,6 +348,23 @@ void MainWindow::init()
 
     }
 
+    // Make sure the proper fullscreen/normal menu item is checked properly.
+    if (isFullScreen())
+    {
+        ui.actionFullscreen->setChecked(true);
+        ui.actionNormal->setChecked(false);
+    }
+    else
+    {
+        ui.actionFullscreen->setChecked(false);
+        ui.actionNormal->setChecked(true);
+    }
+
+    // And that they will stay checked properly after user input
+    QObject::connect(ui.actionFullscreen, SIGNAL(triggered()), this, SLOT(fullScreenActionItemCallback()));
+    QObject::connect(ui.actionNormal, SIGNAL(triggered()), this,SLOT(normalActionItemCallback()));
+
+
     // Set OS dependent keyboard shortcuts for the main window, non OS dependent shortcuts are set in MainWindow.ui
 #ifdef Q_OS_MACX
     ui.actionFlightView->setShortcut(QApplication::translate("MainWindow", "Meta+1", 0, QApplication::UnicodeUTF8));
@@ -662,10 +679,10 @@ void MainWindow::buildCommonWidgets()
     menuActionHelper->createToolAction(tr("Actuator Status"), "HEAD_DOWN_DISPLAY_2_DOCKWIDGET");
     menuActionHelper->createToolAction(tr("Radio Control"));
 
-    createDockWidget(engineeringView,new HUD(320,240,this),tr("Video Downlink"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_ENGINEER,Qt::RightDockWidgetArea,QSize(this->width()/1.5,0));
+    createDockWidget(engineeringView,new HUD(320,240,this),tr("Video Downlink"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_ENGINEER,Qt::RightDockWidgetArea);
 
-    createDockWidget(simView,new PrimaryFlightDisplay(320,240,this),tr("Primary Flight Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",VIEW_SIMULATION,Qt::RightDockWidgetArea,QSize(this->width()/1.5,0));
-    createDockWidget(pilotView,new PrimaryFlightDisplay(320,240,this),tr("Primary Flight Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea,QSize(this->width()/1.8,0));
+    createDockWidget(simView,new PrimaryFlightDisplay(this),tr("Primary Flight Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",VIEW_SIMULATION,Qt::RightDockWidgetArea);
+    createDockWidget(pilotView,new PrimaryFlightDisplay(this),tr("Primary Flight Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea);
 
     QGCTabbedInfoView *infoview = new QGCTabbedInfoView(this);
     infoview->addSource(mavlinkDecoder);
@@ -760,6 +777,16 @@ void MainWindow::showDockWidget(const QString& name, bool show)
         loadDockWidget(name);
 }
 
+void MainWindow::fullScreenActionItemCallback()
+{
+    ui.actionNormal->setChecked(false);
+}
+
+void MainWindow::normalActionItemCallback()
+{
+    ui.actionFullscreen->setChecked(false);
+}
+
 void MainWindow::loadDockWidget(const QString& name)
 {
     if(menuActionHelper->containsDockWidget(currentView, name))
@@ -827,7 +854,7 @@ void MainWindow::loadDockWidget(const QString& name)
     }
     else if (name == "PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET")
     {
-        createDockWidget(centerStack->currentWidget(),new PrimaryFlightDisplay(320,240,this),tr("Primary Flight Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",currentView,Qt::RightDockWidgetArea);
+        createDockWidget(centerStack->currentWidget(),new PrimaryFlightDisplay(this),tr("Primary Flight Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",currentView,Qt::RightDockWidgetArea);
     }
     else if (name == "HEAD_UP_DISPLAY_DOCKWIDGET")
     {
diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h
index 6bebb48d7bfdad21e4262e074e38145a5dba4be9..887407beffb3f9d79c3a01880c675aef43f02d29 100644
--- a/src/ui/MainWindow.h
+++ b/src/ui/MainWindow.h
@@ -299,6 +299,18 @@ public slots:
 
 protected slots:
     void showDockWidget(const QString &name, bool show);
+    /**
+     * @brief Unchecks the normalActionItem.
+     * Used as a triggered() callback by the fullScreenAction to make sure only one of it or the
+     * normalAction are checked at a time, as they're mutually exclusive.
+     */
+    void fullScreenActionItemCallback();
+    /**
+     * @brief Unchecks the fullScreenActionItem.
+     * Used as a triggered() callback by the normalAction to make sure only one of it or the
+     * fullScreenAction are checked at a time, as they're mutually exclusive.
+     */
+    void normalActionItemCallback();
 
 signals:
     void styleChanged(MainWindow::QGC_MAINWINDOW_STYLE newTheme);
diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui
index 0eadd06077cb4dc1a59858bbed6590c4c7fcfa4d..a457b98e46c6c05a6b91c6d57469990ae0fedcd5 100644
--- a/src/ui/MainWindow.ui
+++ b/src/ui/MainWindow.ui
@@ -379,11 +379,23 @@
    </property>
   </action>
   <action name="actionFullscreen">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="checked">
+    <bool>false</bool>
+   </property>
    <property name="text">
     <string>Fullscreen</string>
    </property>
   </action>
   <action name="actionNormal">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="checked">
+    <bool>false</bool>
+   </property>
    <property name="text">
     <string>Normal</string>
    </property>
diff --git a/src/ui/PrimaryFlightDisplay.cc b/src/ui/PrimaryFlightDisplay.cc
index c22fbf30efa2d396ed3c958fea20155c5a57ffa1..0bdb91b336bfc8703797dd17d77756a638ba418d 100644
--- a/src/ui/PrimaryFlightDisplay.cc
+++ b/src/ui/PrimaryFlightDisplay.cc
@@ -1,7 +1,6 @@
 #include "PrimaryFlightDisplay.h"
 #include "UASManager.h"
 
-//#include "ui_primaryflightdisplay.h"
 #include <QDebug>
 #include <QRectF>
 #include <cmath>
@@ -10,12 +9,7 @@
 #include <QPainterPath>
 #include <QResizeEvent>
 #include <QtCore/qmath.h>
-//#include <cmath>
 
-#if 0
-// Left in but ifdef'ed out since there is commented out code below that uses it
-static const float SEPARATE_COMPASS_ASPECTRATIO = 3.0f/4.0f;
-#endif
 static const float LINEWIDTH = 0.0036f;
 static const float SMALL_TEXT_SIZE = 0.028f;
 static const float MEDIUM_TEXT_SIZE = SMALL_TEXT_SIZE*1.2f;
@@ -107,7 +101,7 @@ const QString PrimaryFlightDisplay::compassWindNames[] = {
     QString("NW")
 };
 
-PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *parent) :
+PrimaryFlightDisplay::PrimaryFlightDisplay(QWidget *parent) :
     QWidget(parent),
 
     _valuesChanged(false),
@@ -146,22 +140,17 @@ PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *paren
     font("Bitstream Vera Sans"),
     refreshTimer(new QTimer(this))
 {
-    Q_UNUSED(width);
-    Q_UNUSED(height);
-
     setMinimumSize(120, 80);
     setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
 
     setActiveUAS(UASManager::instance()->getActiveUAS());
 
     // Connect with UAS signal
-    //connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)));
     connect(UASManager::instance(), SIGNAL(UASDeleted(UASInterface*)), this, SLOT(forgetUAS(UASInterface*)));
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
 
     // Refresh timer
     refreshTimer->setInterval(updateInterval);
-    //    connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD()));
     connect(refreshTimer, SIGNAL(timeout()), this, SLOT(checkUpdate()));
 }
 
@@ -170,16 +159,15 @@ PrimaryFlightDisplay::~PrimaryFlightDisplay()
     refreshTimer->stop();
 }
 
-
 QSize PrimaryFlightDisplay::sizeHint() const
 {
-    return QSize(width(), (width()*3.0f)/4);
+    return QSize(width(), (int)(width() * 3.0f / 4.0f));
 }
 
+
 void PrimaryFlightDisplay::showEvent(QShowEvent* event)
 {
-    // React only to internal (pre-display)
-    // events
+    // React only to internal (pre-display) events
     QWidget::showEvent(event);
     refreshTimer->start(updateInterval);
     emit visibilityChanged(true);
@@ -187,8 +175,7 @@ void PrimaryFlightDisplay::showEvent(QShowEvent* event)
 
 void PrimaryFlightDisplay::hideEvent(QHideEvent* event)
 {
-    // React only to internal (pre-display)
-    // events
+    // React only to internal (pre-display) events
     refreshTimer->stop();
     QWidget::hideEvent(event);
     emit visibilityChanged(false);
@@ -198,27 +185,15 @@ void PrimaryFlightDisplay::resizeEvent(QResizeEvent *e) {
     QWidget::resizeEvent(e);
 
     qreal size = e->size().width();
-    //if(e->size().height()<size) size = e->size().height();
 
     lineWidth = PrimaryFlightDisplay_constrain(size*LINEWIDTH, 1, 6);
     fineLineWidth = PrimaryFlightDisplay_constrain(size*LINEWIDTH*2/3, 1, 2);
 
     instrumentEdgePen.setWidthF(fineLineWidth);
-    //AIEdgePen.setWidthF(fineLineWidth);
 
     smallTextSize = size * SMALL_TEXT_SIZE;
     mediumTextSize = size * MEDIUM_TEXT_SIZE;
     largeTextSize = size * LARGE_TEXT_SIZE;
-
-    /*
-     * Try without layout Change-O-Matic. It was too complicated.
-    qreal aspect = e->size().width() / e->size().height();
-    if (aspect <= SEPARATE_COMPASS_ASPECTRATIO)
-        layout = COMPASS_SEPARATED;
-    else
-        layout = COMPASS_INTEGRATED;
-    */
-    // qDebug("Width %d height %d decision %d", e->size().width(), e->size().height(), layout);
 }
 
 void PrimaryFlightDisplay::paintEvent(QPaintEvent *event)
@@ -236,20 +211,6 @@ void PrimaryFlightDisplay::checkUpdate()
     }
 }
 
-///*
-// * Interface towards qgroundcontrol
-// */
-//void PrimaryFlightDisplay::addUAS(UASInterface* uas)
-//{
-//    if (uas)
-//    {
-//        if (!this->uas)
-//        {
-//            setActiveUAS(uas);
-//        }
-//    }
-//}
-
 void PrimaryFlightDisplay::forgetUAS(UASInterface* uas)
 {
     if (this->uas != NULL && this->uas == uas) {
@@ -293,7 +254,6 @@ 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 {
@@ -427,7 +387,7 @@ void PrimaryFlightDisplay::drawTextCenter (
     QFontMetrics metrics = QFontMetrics(font);
     QRect bounds = metrics.boundingRect(text);
     int flags = Qt::AlignCenter |  Qt::TextDontClip; // For some reason the bounds rect is too small!
-    painter.drawText(x /*+bounds.x()*/ -bounds.width()/2, y /*+bounds.y()*/ -bounds.height()/2, bounds.width(), bounds.height(), flags, text);
+    painter.drawText(x - bounds.width()/2, y - bounds.height()/2, bounds.width(), bounds.height(), flags, text);
 }
 
 void PrimaryFlightDisplay::drawTextLeftCenter (
@@ -443,7 +403,7 @@ void PrimaryFlightDisplay::drawTextLeftCenter (
     QFontMetrics metrics = QFontMetrics(font);
     QRect bounds = metrics.boundingRect(text);
     int flags = Qt::AlignLeft | Qt::TextDontClip; // For some reason the bounds rect is too small!
-    painter.drawText(x /*+bounds.x()*/, y /*+bounds.y()*/ -bounds.height()/2, bounds.width(), bounds.height(), flags, text);
+    painter.drawText(x, y - bounds.height()/2, bounds.width(), bounds.height(), flags, text);
 }
 
 void PrimaryFlightDisplay::drawTextRightCenter (
@@ -459,7 +419,7 @@ void PrimaryFlightDisplay::drawTextRightCenter (
     QFontMetrics metrics = QFontMetrics(font);
     QRect bounds = metrics.boundingRect(text);
     int flags = Qt::AlignRight | Qt::TextDontClip; // For some reason the bounds rect is too small!
-    painter.drawText(x /*+bounds.x()*/ -bounds.width(), y /*+bounds.y()*/ -bounds.height()/2, bounds.width(), bounds.height(), flags, text);
+    painter.drawText(x - bounds.width(), y - bounds.height()/2, bounds.width(), bounds.height(), flags, text);
 }
 
 void PrimaryFlightDisplay::drawTextCenterTop (
@@ -475,7 +435,7 @@ void PrimaryFlightDisplay::drawTextCenterTop (
     QFontMetrics metrics = QFontMetrics(font);
     QRect bounds = metrics.boundingRect(text);
     int flags = Qt::AlignCenter | Qt::TextDontClip; // For some reason the bounds rect is too small!
-    painter.drawText(x /*+bounds.x()*/ -bounds.width()/2, y+bounds.height() /*+bounds.y()*/, bounds.width(), bounds.height(), flags, text);
+    painter.drawText(x - bounds.width()/2, y+bounds.height(), bounds.width(), bounds.height(), flags, text);
 }
 
 void PrimaryFlightDisplay::drawTextCenterBottom (
@@ -491,7 +451,7 @@ void PrimaryFlightDisplay::drawTextCenterBottom (
     QFontMetrics metrics = QFontMetrics(font);
     QRect bounds = metrics.boundingRect(text);
     int flags = Qt::AlignCenter;
-    painter.drawText(x /*+bounds.x()*/ -bounds.width()/2, y /*+bounds.y()*/, bounds.width(), bounds.height(), flags, text);
+    painter.drawText(x - bounds.width()/2, y, bounds.width(), bounds.height(), flags, text);
 }
 
 void PrimaryFlightDisplay::drawInstrumentBackground(QPainter& painter, QRectF edge) {
@@ -585,11 +545,6 @@ void PrimaryFlightDisplay::drawAIGlobalFeatures(
     qreal pitchPixels = pitchAngleToTranslation(mainArea.height(), pitch);
     qreal gradientEnd = pitchAngleToTranslation(mainArea.height(), 60);
 
-    //painter.rotate(-roll);
-    //painter.translate(0, pitchPixels);
-
-    //    QTransform forwardTransform;
-    //forwardTransform.translate(mainArea.center().x(), mainArea.center().y());
     painter.rotate(-displayRoll);
     painter.translate(0, pitchPixels);
 
@@ -600,6 +555,7 @@ void PrimaryFlightDisplay::drawAIGlobalFeatures(
     QPointF topRight = rtx.map(paintArea.topRight());
     QPointF bottomLeft = rtx.map(paintArea.bottomLeft());
     QPointF bottomRight = rtx.map(paintArea.bottomRight());
+
     // Just KISS... make a rectangluar basis.
     qreal minx = min4(topLeft.x(), topRight.x(), bottomLeft.x(), bottomRight.x());
     qreal maxx = max4(topLeft.x(), topRight.x(), bottomLeft.x(), bottomRight.x());
@@ -730,7 +686,7 @@ void PrimaryFlightDisplay::drawRollScale(
     pen.setColor(Qt::white);
     painter.setPen(pen);
 
-    // We should really do these transforms but they are assumed done by caller.
+    // We should really do these transforms but they are assumed done by caller:
     // painter.resetTransform();
     // painter.translate(area.center());
     // painter.rotate(roll);
@@ -738,13 +694,11 @@ void PrimaryFlightDisplay::drawRollScale(
     qreal _size = w * ROLL_SCALE_RADIUS*2;
     QRectF arcArea(-_size/2, - _size/2, _size, _size);
     painter.drawArc(arcArea, (90-ROLL_SCALE_RANGE)*16, ROLL_SCALE_RANGE*2*16);
-    // painter.drawEllipse(QPoint(0,0),200,200);
     if (drawTicks) {
         int length = sizeof(tickValues)/sizeof(int);
         qreal previousRotation = 0;
         for (int i=0; i<length*2+1; i++) {
             int degrees = (i==length) ? 0 : (i>length) ?-tickValues[i-length-1] : tickValues[i];
-            //degrees = 180 - degrees;
             painter.rotate(degrees - previousRotation);
             previousRotation = degrees;
 
@@ -753,7 +707,7 @@ void PrimaryFlightDisplay::drawRollScale(
 
             painter.drawLine(start, end);
 
-            QString s_number; //= QString("%d").arg(degrees);
+            QString s_number;
             if (SHOW_ZERO_ON_SCALES || degrees)
                 s_number.sprintf("%d", abs(degrees));
 
@@ -812,7 +766,7 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo
 
         // yaw is in center.
         float off = tickYaw - displayHeading;
-        // wrap that to ]-180..180]
+        // wrap that to [-180..180]
         if (off<=-180) off+= 360; else if (off>180) off -= 360;
 
         painter.translate(area.center());
@@ -865,7 +819,6 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo
     }
 
     painter.setPen(scalePen);
-    //painter.setBrush(Qt::SolidPattern);
     painter.translate(area.center());
     QPainterPath markerPath(QPointF(0, -radius-2));
     markerPath.lineTo(radius*COMPASS_DISK_MARKERWIDTH/2,  -radius-radius*COMPASS_DISK_MARKERHEIGHT-2);
@@ -902,10 +855,6 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo
 
     drawTextCenter(painter, s_digitalCompass, largeTextSize, 0, -radius*0.38-digitalCompassUpshift);
 
-//  dummy
-//  navigationTargetBearing = 10;
-//  navigationCrosstrackError = 500;
-
     // The CDI
     if (shouldDisplayNavigationData() && !isnan(navigationTargetBearing) && !isinf(navigationCrosstrackError)) {
         painter.resetTransform();
@@ -963,7 +912,7 @@ void PrimaryFlightDisplay::drawAltimeter(
 
     // not yet implemented: Display of secondary altitude.
     if (!isnan(secondaryAltitude)) {
-        effectiveHalfHeight-= secondaryAltitudeBoxHeight;
+        effectiveHalfHeight -= secondaryAltitudeBoxHeight;
     }
 
     float markerHalfHeight = mediumTextSize*0.8;
@@ -1037,12 +986,11 @@ void PrimaryFlightDisplay::drawAltimeter(
 
         float vvSign = vvPixHeight>0 ? 1 : -1; // reverse y sign
 
-        // QRectF vvRect(rightEdge - w*ALTIMETER_VVI_WIDTH, markerHalfHeight*vvSign, w*ALTIMETER_VVI_WIDTH, abs(vvPixHeight)*vvSign);
         QPointF vvArrowBegin(rightEdge - w*ALTIMETER_VVI_WIDTH/2, markerHalfHeight*vvSign);
         QPointF vvArrowEnd(rightEdge - w*ALTIMETER_VVI_WIDTH/2, vvPixHeight);
         painter.drawLine(vvArrowBegin, vvArrowEnd);
 
-        // Yeah this is a repitition of above code but we are goigd to trash it all anyway, so no fix.
+        // Yeah this is a repetition of above code but we are going to trash it all anyway, so no fix.
         float vvArowHeadSize = abs(vvPixHeight - markerHalfHeight*vvSign);
         if (vvArowHeadSize > w*ALTIMETER_VVI_WIDTH/3) vvArowHeadSize = w*ALTIMETER_VVI_WIDTH/3;
 
@@ -1064,8 +1012,6 @@ void PrimaryFlightDisplay::drawAltimeter(
         s_salt.sprintf("%3.0f", secondaryAltitude);
         drawTextCenter(painter, s_salt, mediumTextSize, 0, 0);
     }
-
-    // print target altitude (if applicable)
 }
 
 void PrimaryFlightDisplay::drawVelocityMeter(
@@ -1145,7 +1091,7 @@ void PrimaryFlightDisplay::drawVelocityMeter(
     else
         s_alt.sprintf("%3.1f", speed);
     float xCenter = (markerTip+leftEdge)/2;
-    drawTextCenter(painter, s_alt, /* TAPES_TEXT_SIZE*width()*/ mediumTextSize, xCenter, 0);
+    drawTextCenter(painter, s_alt, mediumTextSize, xCenter, 0);
 }
 
 static const int TOP = (1<<0);
@@ -1246,90 +1192,6 @@ void PrimaryFlightDisplay::doPaint() {
     float compassAIIntrusion = 0;
 
     switch(layout) {
-    /*
-    case FEATUREPANELS_IN_CORNERS: {
-        tapeGaugeWidth = tapesGaugeWidthFor(width(), height());
-
-        // A layout optimal for a container wider than it is high.
-        // The AI gets full height and if tapes are transparent, also full width. If tapes are opague, then
-        // the AI gets a width to be perfectly square.
-        AIMainArea = QRectF(
-                    style == NO_OVERLAYS ? tapeGaugeWidth : 0,
-                    0,
-                    style == NO_OVERLAYS ? width() - tapeGaugeWidth * 2: width(),
-                    height());
-
-        AIPaintArea = AIMainArea;
-
-        // Tape gauges get so much width that the AI area not covered by them is perfectly square.
-
-        qreal sidePanelsHeight = height();
-
-        altimeterArea = QRectF(AIMainArea.right(), height()/5, tapeGaugeWidth, sidePanelsHeight*3/5);
-        velocityMeterArea = QRectF (0, height()/5, tapeGaugeWidth, sidePanelsHeight*3/5);
-
-        sensorsStatsArea = QRectF(0, 0, tapeGaugeWidth, sidePanelsHeight/5);
-        linkStatsArea = QRectF(AIMainArea.right(), 0, tapeGaugeWidth, sidePanelsHeight/5);
-        sysStatsArea = QRectF(0, sidePanelsHeight*4/5, tapeGaugeWidth, sidePanelsHeight/5);
-        missionStatsArea =QRectF(AIMainArea.right(), sidePanelsHeight*4/5, tapeGaugeWidth, sidePanelsHeight/5);
-
-        if (style == NO_OVERLAYS) {
-            applyMargin(AIMainArea, margin, TOP|BOTTOM);
-            applyMargin(altimeterArea, margin, TOP|BOTTOM|RIGHT);
-            applyMargin(velocityMeterArea, margin, TOP|BOTTOM|LEFT);
-            setMarginsForCornerLayout(margin, sensorsStatsArea, linkStatsArea, sysStatsArea, missionStatsArea);
-        }
-
-        // Compass is inside the AI ans within its margins also.
-        compassArea = QRectF(AIMainArea.x()+AIMainArea.width()*0.60, AIMainArea.y()+AIMainArea.height()*0.80,
-                             AIMainArea.width()/2, AIMainArea.width()/2);
-        break;
-    }
-
-    case FEATUREPANELS_AT_BOTTOM: {
-        // A layout for containers with about the same width and height.
-        // qreal minor = min(width(), height());
-        // qreal major = max(width(), height());
-
-        qreal aiheight = height()*4.0f/5;
-
-        tapeGaugeWidth = tapesGaugeWidthFor(width(), aiheight);
-
-        AIMainArea = QRectF(
-                    style == NO_OVERLAYS ? tapeGaugeWidth : 0,
-                    0,
-                    style == NO_OVERLAYS ? width() - tapeGaugeWidth*2 : width(),
-                    aiheight);
-
-        AIPaintArea = AIMainArea;
-
-        // Tape gauges get so much width that the AI area not covered by them is perfectly square.
-        altimeterArea = QRectF(AIMainArea.right(), 0, tapeGaugeWidth, aiheight);
-        velocityMeterArea = QRectF (0, 0, tapeGaugeWidth, aiheight);
-
-        qreal panelsHeight = height() / 5.0f;
-        qreal panelsWidth = width() / 4.0f;
-
-        sensorsStatsArea = QRectF(0, AIMainArea.bottom(), panelsWidth, panelsHeight);
-        linkStatsArea = QRectF(panelsWidth, AIMainArea.bottom(), panelsWidth, panelsHeight);
-        sysStatsArea = QRectF(panelsWidth*2, AIMainArea.bottom(), panelsWidth, panelsHeight);
-        missionStatsArea =QRectF(panelsWidth*3, AIMainArea.bottom(), panelsWidth, panelsHeight);
-
-        if (style == NO_OVERLAYS) {
-            applyMargin(AIMainArea, margin, TOP|BOTTOM);
-            applyMargin(altimeterArea, margin, TOP|BOTTOM|RIGHT);
-            applyMargin(velocityMeterArea, margin, TOP|BOTTOM|LEFT);
-            setMarginsForInlineLayout(margin, sensorsStatsArea, linkStatsArea, sysStatsArea, missionStatsArea);
-        }
-
-        // Compass is inside the AI ans within its margins also.
-        compassArea = QRectF(AIMainArea.x()+AIMainArea.width()*0.60, AIMainArea.y()+AIMainArea.height()*0.80,
-                             AIMainArea.width()/2, AIMainArea.width()/2);
-        break;
-    }
-
-    */
-
     case COMPASS_INTEGRATED: {
         tapeGaugeWidth = tapesGaugeWidthFor(width(), width());
         qreal aiheight = height();
@@ -1365,7 +1227,6 @@ void PrimaryFlightDisplay::doPaint() {
         qreal compassSize = compassRelativeWidth  * AIMainArea.width();  // Diameter is this times the width.
 
         qreal compassCenterY;
-        //if (heightSurplus >= 0) compassCenterY =  AIMainArea.bottom() + compassSize/2;
         compassCenterY = AIMainArea.bottom() + compassSize / 4;
 
         if (height() - compassCenterY > AIMainArea.width()/2*compassBottomMargin)
@@ -1412,50 +1273,6 @@ void PrimaryFlightDisplay::doPaint() {
         velocityMeterArea = QRectF (0, 0, tapeGaugeWidth, aiheight);
         altimeterArea = QRectF(AIMainArea.right(), 0, tapeGaugeWidth, aiheight);
 
-        /*
-        qreal panelsWidth = width() / 4.0f;
-        if(remainingHeight > width()) {
-            // very tall layout, place panels below compass.
-            sensorsStatsArea = QRectF(0, height()-panelsHeight, panelsWidth, panelsHeight);
-            linkStatsArea = QRectF(panelsWidth, height()-panelsHeight, panelsWidth, panelsHeight);
-            sysStatsArea = QRectF(panelsWidth*2, height()-panelsHeight, panelsWidth, panelsHeight);
-            missionStatsArea =QRectF(panelsWidth*3, height()-panelsHeight, panelsWidth, panelsHeight);
-            if (style == OPAGUE_TAPES) {
-                setMarginsForInlineLayout(margin, sensorsStatsArea, linkStatsArea, sysStatsArea, missionStatsArea);
-            }
-            compassCenter = QPoint(width()/2, (AIArea.bottom()+height()-panelsHeight)/2);
-            maxCompassDiam = fmin(width(), height()-AIArea.height()-panelsHeight);
-        } else {
-            // Remaining part is wider than high; place panels in corners around compass
-            // Naaah it is really ugly, do not do that.
-            sensorsStatsArea = QRectF(0, AIArea.bottom(), panelsWidth, panelsHeight);
-            linkStatsArea = QRectF(width()-panelsWidth, AIArea.bottom(), panelsWidth, panelsHeight);
-            sysStatsArea = QRectF(0, height()-panelsHeight, panelsWidth, panelsHeight);
-            missionStatsArea =QRectF(width()-panelsWidth, height()-panelsHeight, panelsWidth, panelsHeight);
-            if (style == OPAGUE_TAPES) {
-                setMarginsForCornerLayout(margin, sensorsStatsArea, linkStatsArea, sysStatsArea, missionStatsArea);
-            }
-            compassCenter = QPoint(width()/2, (AIArea.bottom()+height())/2);
-            // diagonal between 2 panel corners
-            qreal xd = width()-panelsWidth*2;
-            qreal yd = height()-panelsHeight - AIArea.bottom();
-            maxCompassDiam = qSqrt(xd*xd + yd*yd);
-            if (maxCompassDiam > remainingHeight) {
-                maxCompassDiam = width() - panelsWidth*2;
-                if (maxCompassDiam > remainingHeight) {
-                    // If still too large, lower.
-                    // compassCenter.setY();
-                }
-            }
-        }
-        */
-        /*
-        sensorsStatsArea = QRectF(0, height()-panelsHeight, panelsWidth, panelsHeight);
-        linkStatsArea = QRectF(panelsWidth, height()-panelsHeight, panelsWidth, panelsHeight);
-        sysStatsArea = QRectF(panelsWidth*2, height()-panelsHeight, panelsWidth, panelsHeight);
-        missionStatsArea =QRectF(panelsWidth*3, height()-panelsHeight, panelsWidth, panelsHeight);
-        */
-
         QPoint compassCenter = QPoint(width()/2, AIMainArea.bottom()+width()/2);
         qreal compassDiam = width() * 0.8;
         compassArea = QRectF(compassCenter.x()-compassDiam/2, compassCenter.y()-compassDiam/2, compassDiam, compassDiam);
@@ -1472,10 +1289,7 @@ void PrimaryFlightDisplay::doPaint() {
     drawAIAttitudeScales(painter, AIMainArea, compassAIIntrusion);
     drawAIAirframeFixedFeatures(painter, AIMainArea);
 
-   // if(layout ==COMPASS_SEPARATED)
-        //drawSeparateCompassDisk(painter, compassArea);
-   // else
-        drawAICompassDisk(painter, compassArea, compassHalfSpan);
+    drawAICompassDisk(painter, compassArea, compassHalfSpan);
 
     painter.setClipping(hadClip);
 
@@ -1483,19 +1297,5 @@ void PrimaryFlightDisplay::doPaint() {
 
     drawVelocityMeter(painter, velocityMeterArea);
 
-    /*
-    drawSensorsStatsPanel(painter, sensorsStatsArea);
-    drawLinkStatsPanel(painter, linkStatsArea);
-    drawSysStatsPanel(painter, sysStatsArea);
-    drawMissionStatsPanel(painter, missionStatsArea);
-    */
-    /*
-    if (style == OPAGUE_TAPES) {
-        drawInstrumentBackground(painter, AIArea);
-    }
-    */
-
     painter.end();
 }
-
-void PrimaryFlightDisplay:: createActions() {}
diff --git a/src/ui/PrimaryFlightDisplay.h b/src/ui/PrimaryFlightDisplay.h
index b4236e339a400cc994d7de0613d2025a3be7f16e..4c7dc6bf0f30ff8ea0eef7a3fe6f67d883ff683c 100644
--- a/src/ui/PrimaryFlightDisplay.h
+++ b/src/ui/PrimaryFlightDisplay.h
@@ -9,7 +9,7 @@ class PrimaryFlightDisplay : public QWidget
 {
     Q_OBJECT
 public:
-    PrimaryFlightDisplay(int width = 640, int height = 480, QWidget* parent = NULL);
+    PrimaryFlightDisplay(QWidget* parent = NULL);
     ~PrimaryFlightDisplay();
 
 public slots:
@@ -23,7 +23,6 @@ public slots:
     void updateNavigationControllerErrors(UASInterface* uas, double altitudeError, double speedError, double xtrackError);
 
     /** @brief Set the currently monitored UAS */
-    //void addUAS(UASInterface* uas);
     void forgetUAS(UASInterface* uas);
     void setActiveUAS(UASInterface* uas);
 
@@ -57,34 +56,10 @@ protected:
     /** @brief Stop updating widget */
     void hideEvent(QHideEvent* event);
 
-    // dongfang: We have no context menu. Viewonly.
-    // void contextMenuEvent (QContextMenuEvent* event);
-
-    // dongfang: What is that?
-    // dongfang: OK it's for UI interaction. Presently, there is none.
-    void createActions();
-
 signals:
     void visibilityChanged(bool visible);
 
 private:
-    /*
-    enum AltimeterMode {
-        PRIMARY_MAIN_GPS_SUB,   // Show the primary alt. on tape and GPS as extra info
-        GPS_MAIN                // Show GPS on tape and no extra info
-    };
-
-    enum AltimeterFrame {
-        ASL,                    // Show ASL altitudes (plane pilots' normal preference)
-        RELATIVE_TO_HOME        // Show relative-to-home altitude (copter pilots)
-    };
-
-    enum SpeedMode {
-        PRIMARY_MAIN_GROUND_SUB,// Show primary speed (often airspeed) on tape and groundspeed as extra
-        GROUND_MAIN             // Show groundspeed on tape and no extra info
-    };
-    */
-
     /*
      * There are at least these differences between airplane and copter PDF view:
      * - Airplane show absolute altutude in altimeter, copter shows relative to home
@@ -122,12 +97,6 @@ private:
 
     UASInterface* uas;          ///< The uas currently monitored
 
-    /*
-    AltimeterMode altimeterMode;
-    AltimeterFrame altimeterFrame;
-    SpeedMode speedMode;
-    */
-
     bool didReceiveSpeed;
 
     float roll;
diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc
index 3e7aa2f63963b9e0749ee15773a88ec0e8ec00bf..54ec4ae414fc290c8c91fe160f11f346973e4e02 100644
--- a/src/ui/QGCMAVLinkLogPlayer.cc
+++ b/src/ui/QGCMAVLinkLogPlayer.cc
@@ -572,10 +572,18 @@ void QGCMAVLinkLogPlayer::logLoop()
     if (mavlinkLogFormat)
     {
         // Now parse MAVLink messages, grabbing their timestamps as we go. We stop once we
-        // have at least 3ms until the next one.
+        // have at least 3ms until the next one. 
+
+        // We track what the next execution time should be in milliseconds, which we use to set
+        // the next timer interrupt.
         int nextExecutionTime = 0;
+
+        // We use the `findNextMavlinkMessage()` function to scan ahead for MAVLink messages. This
+        // is necessary because we don't know how big each MAVLink message is until we finish parsing
+        // one, and since we only output arrays of bytes, we need to know the size of that array.
         mavlink_message_t msg;
-        msg.len = 0;    // FIXME: Hack, remove once Issue #647 is fixed
+        findNextMavlinkMessage(&msg);
+
         while (nextExecutionTime < 3) {
 
             // Now we're sitting at the start of a MAVLink message, so read it all into a byte array for feeding to our parser.
diff --git a/src/ui/SerialConfigurationWindow.cc b/src/ui/SerialConfigurationWindow.cc
index a6fd2cfa169ef2db847800c61de10c5b9b783de1..052043e5d7416494426ad4f77bd792246a7935ab 100644
--- a/src/ui/SerialConfigurationWindow.cc
+++ b/src/ui/SerialConfigurationWindow.cc
@@ -55,7 +55,10 @@ SerialConfigurationWindow::SerialConfigurationWindow(LinkInterface* link, QWidge
         action = new QAction(QIcon(":/files/images/devices/network-wireless.svg"), "", this);
         setLinkName(link->getName());
 
-        setupPortList();
+        // Scan for serial ports. Let the user know if none were found for debugging purposes
+        if (!setupPortList()) {
+            qDebug() << "No serial ports found.";
+        }
 
         // Set up baud rates
         ui.baudRate->clear();
@@ -213,9 +216,9 @@ void SerialConfigurationWindow::configureCommunication()
     this->show();
 }
 
-void SerialConfigurationWindow::setupPortList()
+bool SerialConfigurationWindow::setupPortList()
 {
-    if (!link) return;
+    if (!link) return false;
 
     // Get the ports available on this system
     QList<QString> ports = link->getCurrentPorts();
@@ -240,6 +243,8 @@ void SerialConfigurationWindow::setupPortList()
 
     if (storedFound)
         ui.portName->setEditText(storedName);
+
+    return (ports.count() > 0);
 }
 
 void SerialConfigurationWindow::enableFlowControl(bool flow)
diff --git a/src/ui/SerialConfigurationWindow.h b/src/ui/SerialConfigurationWindow.h
index 41a95f59df246c71e4dba5fc0bc4120e5b79d075..57b8600ed8b8ac0f295ceeb67d73bfb95b49a9e3 100644
--- a/src/ui/SerialConfigurationWindow.h
+++ b/src/ui/SerialConfigurationWindow.h
@@ -60,7 +60,13 @@ public slots:
     void setParityEven(bool accept);
     void setPortName(QString port);
     void setLinkName(QString name);
-    void setupPortList();
+    /**
+     * @brief setupPortList Populates the dropdown with the list of available serial ports.
+     * This function is called at 1s intervals to check that the serial port still exists and to see if
+     * any new ones have been attached.
+     * @return True if any ports were found, false otherwise.
+     */
+    bool setupPortList();
 
 protected:
     void showEvent(QShowEvent* event);