diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index 18e4b83a6e784a7b6b3a48421328694ef1c55e8b..c9924110ba54113a8897e24fddf265c1e7aefb81 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -35,6 +35,7 @@ This file is part of the PIXHAWK project
 
 #include <QDebug>
 #include <QTime>
+#include <QApplication>
 
 #include "MG.h"
 #include "MAVLinkProtocol.h"
@@ -56,7 +57,9 @@ This file is part of the PIXHAWK project
 MAVLinkProtocol::MAVLinkProtocol() :
         heartbeatTimer(new QTimer(this)),
         heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
-        m_heartbeatsEnabled(false)
+        m_heartbeatsEnabled(false),
+        m_loggingEnabled(false),
+        m_logfile(NULL)
 {
     start(QThread::LowPriority);
     // Start heartbeat timer, emitting a heartbeat at the configured rate
@@ -77,12 +80,23 @@ MAVLinkProtocol::MAVLinkProtocol() :
 
 MAVLinkProtocol::~MAVLinkProtocol()
 {
+    if (m_logfile)
+    {
+        m_logfile->close();
+        delete m_logfile;
+    }
 }
 
 
 
 void MAVLinkProtocol::run()
 {
+
+}
+
+QString MAVLinkProtocol::getLogfileName()
+{
+    return QCoreApplication::applicationDirPath()+"/mavlink.log";
 }
 
 /**
@@ -111,6 +125,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
 
         if (decodeState == 1)
         {
+            // Log data
+            if (m_loggingEnabled)
+            {
+                uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+                mavlink_msg_to_send_buffer(buf, &message);
+                m_logfile->write((const char*) buf);
+                qDebug() << "WROTE LOGFILE";
+            }
+
             // ORDER MATTERS HERE!
             // If the matching UAS object does not yet exist, it has to be created
             // before emitting the packetReceived signal
@@ -316,11 +339,32 @@ void MAVLinkProtocol::enableHeartbeats(bool enabled)
     emit heartbeatChanged(enabled);
 }
 
+void MAVLinkProtocol::enableLogging(bool enabled)
+{
+    if (enabled && !m_loggingEnabled)
+    {
+       m_logfile = new QFile(getLogfileName());
+       m_logfile->open(QIODevice::WriteOnly | QIODevice::Append);
+    }
+    else
+    {
+       m_logfile->close();
+       delete m_logfile;
+       m_logfile = NULL;
+    }
+    m_loggingEnabled = enabled;
+}
+
 bool MAVLinkProtocol::heartbeatsEnabled(void)
 {
     return m_heartbeatsEnabled;
 }
 
+bool MAVLinkProtocol::loggingEnabled(void)
+{
+    return m_loggingEnabled;
+}
+
 /**
  * The default rate is 1 Hertz.
  *
diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h
index 05ba5f7e1ee91aff78862528aa4276dc4ec7fefe..82cf9b1132d25b8e2b0c218e932725603bd4e894 100644
--- a/src/comm/MAVLinkProtocol.h
+++ b/src/comm/MAVLinkProtocol.h
@@ -37,6 +37,7 @@ This file is part of the PIXHAWK project
 #include <QMutex>
 #include <QString>
 #include <QTimer>
+#include <QFile>
 #include <QMap>
 #include <QByteArray>
 #include "ProtocolInterface.h"
@@ -65,6 +66,10 @@ public:
     int getHeartbeatRate();
     /** @brief Get heartbeat state */
     bool heartbeatsEnabled(void);
+    /** @brief Get logging state */
+    bool loggingEnabled(void);
+    /** @brief Get the name of the packet log file */
+    static QString getLogfileName();
 
 public slots:
     /** @brief Receive bytes from a communication interface */
@@ -79,14 +84,19 @@ public slots:
     /** @brief Enable / disable the heartbeat emission */
     void enableHeartbeats(bool enabled);
 
+    /** @brief Enable/disable binary packet logging */
+    void enableLogging(bool enabled);
+
     /** @brief Send an extra heartbeat to all connected units */
     void sendHeartbeat();
 
 protected:
-    QTimer* heartbeatTimer;  ///< Timer to emit heartbeats
-    int heartbeatRate;       ///< Heartbeat rate, controls the timer interval
+    QTimer* heartbeatTimer;    ///< Timer to emit heartbeats
+    int heartbeatRate;         ///< Heartbeat rate, controls the timer interval
     bool m_heartbeatsEnabled;  ///< Enabled/disable heartbeat emission
-    QMutex receiveMutex;     ///< Mutex to protect receiveBytes function
+    bool m_loggingEnabled;     ///< Enable/disable packet logging
+    QFile* m_logfile;           ///< Logfile
+    QMutex receiveMutex;       ///< Mutex to protect receiveBytes function
     int lastIndex[256][256];
     int totalReceiveCounter;
     int totalLossCounter;
@@ -98,6 +108,8 @@ signals:
     void messageReceived(LinkInterface* link, mavlink_message_t message);
     /** @brief Emitted if heartbeat emission mode is changed */
     void heartbeatChanged(bool heartbeats);
+    /** @brief Emitted if logging is started / stopped */
+    void loggingChanged(bool enabled);
 };
 
 #endif // MAVLINKPROTOCOL_H_
diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index 8af036e9534c18c17a3b821843cb965962787e6c..2049e55f76b232d05ced9b380495dc56ef34abe1 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -38,6 +38,7 @@ This file is part of the PIXHAWK project
 #include <QDebug>
 #include "MG.h"
 #include "LinkManager.h"
+#include "MAVLinkProtocol.h"
 #include "MAVLinkSimulationLink.h"
 // MAVLINK includes
 #include <mavlink.h>
@@ -92,6 +93,10 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
     maxTimeNoise = 0;
     this->id = getNextLinkId();
     LinkManager::instance()->add(this);
+
+    // Open packet log
+    mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName());
+    mavlinkLogFile->open(QIODevice::ReadOnly);
 }
 
 MAVLinkSimulationLink::~MAVLinkSimulationLink()
@@ -121,6 +126,21 @@ void MAVLinkSimulationLink::run()
             if (_isConnected)
             {
                 mainloop();
+
+                // FIXME Hacky code to read from packet log file
+//                readyBufferMutex.lock();
+//                for (int i = 0; i < streampointer; i++)
+//                {
+//                    readyBuffer.enqueue(*(stream + i));
+//                }
+//                readyBufferMutex.unlock();
+
+
+
+
+
+
+
                 emit bytesReady(this);
             }
             last = MG::TIME::getGroundTimeNow();
diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h
index b6b55644d9dc0e2b01b265a72f05c9e5d9888ee1..0b10e375390a225f4f8d7a247dd41a3754b7c96c 100644
--- a/src/comm/MAVLinkSimulationLink.h
+++ b/src/comm/MAVLinkSimulationLink.h
@@ -98,6 +98,7 @@ protected:
     QTimer* timer;
     /** File which contains the input data (simulated robot messages) **/
     QFile* simulationFile;
+    QFile* mavlinkLogFile;
     QString simulationHeader;
     /** File where the commands sent by the groundstation are stored **/
     QFile* receiveFile;
diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc
index c1afa25fd0b48f51d87ac0c08783fb1743c3268d..62a47ee5571492543d0aaa46994a8f222fc2bd79 100644
--- a/src/comm/MAVLinkXMLParser.cc
+++ b/src/comm/MAVLinkXMLParser.cc
@@ -73,6 +73,8 @@ bool MAVLinkXMLParser::generate()
     QList< QPair<QString, QString> > cFiles;
     QString lcmStructDefs = "";
 
+    QString pureFileName;
+
     // Run through root children
     while(!n.isNull())
     {
@@ -103,6 +105,7 @@ bool MAVLinkXMLParser::generate()
                             {
                                 QFileInfo rInfo(this->fileName);
                                 fileName = rInfo.absoluteDir().canonicalPath() + "/" + fileName;
+                                pureFileName = rInfo.baseName().split(".").first();
                             }
 
                             QFile file(fileName);
@@ -365,6 +368,7 @@ bool MAVLinkXMLParser::generate()
     // Mark all code as C code
     mainHeader += "#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n";
     mainHeader += "\n#include \"protocol.h\"\n";
+    mainHeader += "\n#define MAVLINK_ENABLED_" + pureFileName.toUpper() + "\n\n";
     QString includeLine = "#include \"%1\"\n";
     QString mainHeaderName = "mavlink.h";
     QString messagesDirName = "generated";
diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc
index 539b6e6cfae5347ef121721564cb0ef1d0e2a225..debea0f2a3db836f189d7a6ead468d5bbc25c869 100644
--- a/src/uas/PxQuadMAV.cc
+++ b/src/uas/PxQuadMAV.cc
@@ -20,93 +20,123 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
     UAS::receiveMessage(link, message);
     mavlink_message_t* msg = &message;
 
-        //qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid;
+    //qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid;
 
+// Only compile this portion if matching MAVLink packets have been compiled
+#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
 
-    // Handle your special messages
-    switch (msg->msgid)
+    if (message.sysid == uasId)
     {
-    case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT:
+        QString uasState;
+        QString stateDescription;
+        QString patternPath;
+        switch (message.msgid)
         {
-            mavlink_watchdog_heartbeat_t payload;
-            mavlink_msg_watchdog_heartbeat_decode(msg, &payload);
-            
-            emit watchdogReceived(this->uasId, payload.watchdog_id, payload.process_count);
-        }
-        break;
-        
+        case MAVLINK_MSG_ID_RAW_AUX:
+            {
+                mavlink_raw_aux_t raw;
+                mavlink_msg_raw_aux_decode(&message, &raw);
+                quint64 time = getUnixTime(0);
+                emit valueChanged(uasId, "Pressure", raw.baro, time);
+                emit valueChanged(uasId, "Temperature", raw.temp, time);
+            }
+            break;
+        case MAVLINK_MSG_ID_PATTERN_DETECTED:
+            {
+                QByteArray b;
+                b.resize(256);
+                mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data());
+                b.append('\0');
+                QString path = QString(b);
+                bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false );
+                emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected);
+            }
+            break;
+    case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT:
+            {
+                mavlink_watchdog_heartbeat_t payload;
+                mavlink_msg_watchdog_heartbeat_decode(msg, &payload);
+
+                emit watchdogReceived(this->uasId, payload.watchdog_id, payload.process_count);
+            }
+            break;
+
     case MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO:
-        {
-            mavlink_watchdog_process_info_t payload;
-            mavlink_msg_watchdog_process_info_decode(msg, &payload);
-            
-            emit processReceived(this->uasId, payload.watchdog_id, payload.process_id, QString((const char*)payload.name), QString((const char*)payload.arguments), payload.timeout);
-        }
-        break;
-        
+            {
+                mavlink_watchdog_process_info_t payload;
+                mavlink_msg_watchdog_process_info_decode(msg, &payload);
+
+                emit processReceived(this->uasId, payload.watchdog_id, payload.process_id, QString((const char*)payload.name), QString((const char*)payload.arguments), payload.timeout);
+            }
+            break;
+
     case MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS:
-        {
-            mavlink_watchdog_process_status_t payload;
-            mavlink_msg_watchdog_process_status_decode(msg, &payload);
-            emit processChanged(this->uasId, payload.watchdog_id, payload.process_id, payload.state, (payload.muted == 1) ? true : false, payload.crashes, payload.pid);
-        }
-        break;
+            {
+                mavlink_watchdog_process_status_t payload;
+                mavlink_msg_watchdog_process_status_decode(msg, &payload);
+                emit processChanged(this->uasId, payload.watchdog_id, payload.process_id, payload.state, (payload.muted == 1) ? true : false, payload.crashes, payload.pid);
+            }
+            break;
     case MAVLINK_MSG_ID_DEBUG_VECT:
-        {
-            mavlink_debug_vect_t vect;
-            mavlink_msg_debug_vect_decode(msg, &vect);
-            QString str((const char*)vect.name);
-            emit valueChanged(uasId, str+".x", vect.x, MG::TIME::getGroundTimeNow());
-            emit valueChanged(uasId, str+".y", vect.y, MG::TIME::getGroundTimeNow());
-            emit valueChanged(uasId, str+".z", vect.z, MG::TIME::getGroundTimeNow());
-        }
-        break;
+            {
+                mavlink_debug_vect_t vect;
+                mavlink_msg_debug_vect_decode(msg, &vect);
+                QString str((const char*)vect.name);
+                emit valueChanged(uasId, str+".x", vect.x, MG::TIME::getGroundTimeNow());
+                emit valueChanged(uasId, str+".y", vect.y, MG::TIME::getGroundTimeNow());
+                emit valueChanged(uasId, str+".z", vect.z, MG::TIME::getGroundTimeNow());
+            }
+            break;
     case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
-        {
-            mavlink_vision_position_estimate_t pos;
-            mavlink_msg_vision_position_estimate_decode(&message, &pos);
-            quint64 time = getUnixTime(pos.usec);
-            emit valueChanged(uasId, "vis. time", pos.usec, time);
-            emit valueChanged(uasId, "vis. roll", pos.roll, time);
-            emit valueChanged(uasId, "vis. pitch", pos.pitch, time);
-            emit valueChanged(uasId, "vis. yaw", pos.yaw, time);
-            emit valueChanged(uasId, "vis. x", pos.x, time);
-            emit valueChanged(uasId, "vis. y", pos.y, time);
-            emit valueChanged(uasId, "vis. z", pos.z, time);
-            emit valueChanged(uasId, "vis. vx", pos.vx, time);
-            emit valueChanged(uasId, "vis. vy", pos.vy, time);
-            emit valueChanged(uasId, "vis. vz", pos.vz, time);
-            emit valueChanged(uasId, "vis. vyaw", pos.vyaw, time);
-            // Set internal state
-            if (!positionLock)
             {
-                // If position was not locked before, notify positive
-                GAudioOutput::instance()->notifyPositive();
+                mavlink_vision_position_estimate_t pos;
+                mavlink_msg_vision_position_estimate_decode(&message, &pos);
+                quint64 time = getUnixTime(pos.usec);
+                emit valueChanged(uasId, "vis. time", pos.usec, time);
+                emit valueChanged(uasId, "vis. roll", pos.roll, time);
+                emit valueChanged(uasId, "vis. pitch", pos.pitch, time);
+                emit valueChanged(uasId, "vis. yaw", pos.yaw, time);
+                emit valueChanged(uasId, "vis. x", pos.x, time);
+                emit valueChanged(uasId, "vis. y", pos.y, time);
+                emit valueChanged(uasId, "vis. z", pos.z, time);
+                emit valueChanged(uasId, "vis. vx", pos.vx, time);
+                emit valueChanged(uasId, "vis. vy", pos.vy, time);
+                emit valueChanged(uasId, "vis. vz", pos.vz, time);
+                emit valueChanged(uasId, "vis. vyaw", pos.vyaw, time);
+                // Set internal state
+                if (!positionLock)
+                {
+                    // If position was not locked before, notify positive
+                    GAudioOutput::instance()->notifyPositive();
+                }
+                positionLock = true;
             }
-            positionLock = true;
-        }
-        break;
+            break;
     case MAVLINK_MSG_ID_AUX_STATUS:
-        {
-            mavlink_aux_status_t status;
-            mavlink_msg_aux_status_decode(&message, &status);
-            emit loadChanged(this, status.load/10.0f);
-            emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count);
-            emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count);
-            emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count);
-            emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count);
-            emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count);
-            emit valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow());
-        }
-        break;
+            {
+                mavlink_aux_status_t status;
+                mavlink_msg_aux_status_decode(&message, &status);
+                emit loadChanged(this, status.load/10.0f);
+                emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count);
+                emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count);
+                emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count);
+                emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count);
+                emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count);
+                emit valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow());
+            }
+            break;
     default:
-        // Do nothing
-        break;
+            // Do nothing
+            break;
+        }
     }
+
+#endif
 }
 
 void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command)
 {
+#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
     mavlink_watchdog_command_t payload;
     payload.target_system_id = uasId;
     payload.watchdog_id = watchdogId;
@@ -116,4 +146,5 @@ void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int c
     mavlink_message_t msg;
     mavlink_msg_watchdog_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &payload);
     sendMessage(msg);
+#endif
 }
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 54e10b78d084f126b08cd3fb49692c7ba06ef4a6..54164101feda4fc35813ffaee976b95d292bbfa7 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -267,15 +267,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "Mag. Z", raw.zmag, time);
             }
             break;
-        case MAVLINK_MSG_ID_RAW_AUX:
-            {
-                mavlink_raw_aux_t raw;
-                mavlink_msg_raw_aux_decode(&message, &raw);
-                quint64 time = getUnixTime(0);
-                emit valueChanged(uasId, "Pressure", raw.baro, time);
-                emit valueChanged(uasId, "Temperature", raw.temp, time);
-            }
-            break;
         case MAVLINK_MSG_ID_ATTITUDE:
             //std::cerr << std::endl;
             //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
@@ -339,7 +330,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "lon", pos.lon, time);
                 emit valueChanged(uasId, "alt", pos.alt, time);
                 emit valueChanged(uasId, "speed", pos.v, time);
-                qDebug() << "GOT GPS RAW";
+                //qDebug() << "GOT GPS RAW";
                 emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
             }
             break;
@@ -347,9 +338,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_gps_status_t pos;
                 mavlink_msg_gps_status_decode(&message, &pos);
-                for(int i = 0; i < pos.satellites_visible && i < sizeof(pos.satellite_used); i++)
+                for(int i = 0; i < (int)pos.satellites_visible; i++)
                 {
-                    emit gpsSatelliteStatusChanged(uasId, i, pos.satellite_azimuth[i], pos.satellite_direction[i], pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
+                    emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
                 }
             }
             break;
@@ -408,17 +399,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit textMessageReceived(uasId, severity, text);
             }
             break;
-        case MAVLINK_MSG_ID_PATTERN_DETECTED:
-            {
-                QByteArray b;
-                b.resize(256);
-                mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data());
-                b.append('\0');
-                QString path = QString(b);
-                bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false );
-                emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected);
-            }
-            break;
         default:
             {
                 if (!unknownPackets.contains(message.msgid))
diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc
index 6bbe2f4e87a57650549aba1a5ef2897e12f55b4a..454e5bdbecef954f57e1096edcdb5b4a46bf5115 100644
--- a/src/ui/HDDisplay.cc
+++ b/src/ui/HDDisplay.cc
@@ -119,7 +119,7 @@ void HDDisplay::paintEvent(QPaintEvent * event)
     Q_UNUSED(event);
     //paintGL();
     static quint64 interval = 0;
-    qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
+    //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
     interval = MG::TIME::getGroundTimeNow();
     paintDisplay();
 }
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index eb4ba2d1c1f998e4fe67ba36c93bc65493e28652..da02f68922f779e1f92b35fbf65b418eb1b5f459 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -40,9 +40,10 @@ This file is part of the PIXHAWK project
 
 HSIDisplay::HSIDisplay(QWidget *parent) :
         HDDisplay(NULL, parent),
-        gpsSatellites()
+        gpsSatellites(),
+        satellitesUsed(0)
 {
-    
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
 }
 
 void HSIDisplay::paintEvent(QPaintEvent * event)
@@ -91,6 +92,9 @@ void HSIDisplay::paintDisplay()
         drawCircle(vwidth/2.0f, vheight/2.0f, radius, 0.1f, ringColor, &painter);
     }
 
+    // Draw center indicator
+    drawCircle(vwidth/2.0f, vheight/2.0f, 1.0f, 0.1f, ringColor, &painter);
+
     drawGPS();
 
 
@@ -133,6 +137,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
         //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
     }
 
+    connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
+
     // Now connect the new UAS
 
     //if (this->uas != uas)
@@ -143,40 +149,88 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
     //}
 }
 
-void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used)
+void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float azimuth, float snr, bool used)
 {
     Q_UNUSED(uasid);
+    //qDebug() << "UPDATED SATELLITE";
     // If slot is empty, insert object
-    if (gpsSatellites.at(satid) == NULL)
+    if (gpsSatellites.contains(satid))
     {
-        gpsSatellites.insert(satid, new GPSSatellite(satid, azimuth, direction, snr, used));
+        gpsSatellites.value(satid)->update(satid, elevation, azimuth, snr, used);
     }
     else
     {
-        // Satellite exists, update it
-        gpsSatellites.at(satid)->update(satid, azimuth, direction, snr, used);
+        gpsSatellites.insert(satid, new GPSSatellite(satid, elevation, azimuth, snr, used));
     }
 }
 
 QColor HSIDisplay::getColorForSNR(float snr)
 {
-    return QColor(200, 250, 200);
+    QColor color;
+    if (snr > 0 && snr < 30)
+    {
+        color = QColor(250, 10, 10);
+    }
+    else if (snr >= 30 && snr < 35)
+    {
+        color = QColor(230, 230, 10);
+    }
+    else if (snr >= 35 && snr < 40)
+    {
+        color = QColor(90, 200, 90);
+    }
+    else if (snr >= 40)
+    {
+        color = QColor(20, 200, 20);
+    }
+    else
+    {
+        color = QColor(180, 180, 180);
+    }
+    return color;
 }
 
 void HSIDisplay::drawGPS()
 {
+    float xCenter = vwidth/2.0f;
+    float yCenter = vwidth/2.0f;
     QPainter painter(this);
     painter.setRenderHint(QPainter::Antialiasing, true);
     painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
 
     // Max satellite circle radius
 
-    const float margin = 0.2f;  // 20% margin of total width on each side
+    const float margin = 0.15f;  // 20% margin of total width on each side
     float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f;
+    quint64 currTime = MG::TIME::getGroundTimeNowUsecs();
+
+    // Draw satellite labels
+    //    QString label;
+    //    label.sprintf("%05.1f", value);
+    //    paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter);
 
-    for (int i = 0; i < gpsSatellites.size(); i++)
+    QMapIterator<int, GPSSatellite*> i(gpsSatellites);
+    while (i.hasNext())
     {
-        GPSSatellite* sat = gpsSatellites.at(i);
+        i.next();
+        GPSSatellite* sat = i.value();
+
+        // Check if update is not older than 5 seconds, else delete satellite
+        if (sat->lastUpdate + 1000000 < currTime)
+        {
+            // Delete and go to next satellite
+            gpsSatellites.remove(i.key());
+            if (i.hasNext())
+            {
+                i.next();
+                sat = i.value();
+            }
+            else
+            {
+                continue;
+            }
+        }
+
         if (sat)
         {
             // Draw satellite
@@ -195,10 +249,11 @@ void HSIDisplay::drawGPS()
             painter.setPen(color);
             painter.setBrush(brush);
 
-            float xPos = sin(sat->direction) * sat->azimuth * radius;
-            float yPos = cos(sat->direction) * sat->azimuth * radius;
+            float xPos = xCenter + (sin(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
+            float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
 
-            drawCircle(xPos, yPos, vwidth/10.0f, 1.0f, color, &painter);
+            drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter);
+            paintText(QString::number(sat->id), QColor(255, 255, 255), 2.9f, xPos+1.7f, yPos+2.0f, &painter);
         }
     }
 }
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index 838013daed55df642414caaf8c3327fec81f10df..321b62bc3883a061e55fe1fee58c405ca9b935dc 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -40,6 +40,7 @@ This file is part of the PIXHAWK project
 #include <cmath>
 
 #include "HDDisplay.h"
+#include "MG.h"
 
 class HSIDisplay : public HDDisplay {
     Q_OBJECT
@@ -49,6 +50,7 @@ public:
 
 public slots:
     void setActiveUAS(UASInterface* uas);
+    void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
     void paintEvent(QPaintEvent * event);
 
 protected slots:
@@ -59,7 +61,6 @@ protected slots:
 
 
 protected:
-    void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
     static QColor getColorForSNR(float snr);
 
     /**
@@ -68,35 +69,39 @@ protected:
     class GPSSatellite
     {
     public:
-        GPSSatellite(int id, float azimuth, float direction, float snr, bool used) :
-            id(id),
-            azimuth(azimuth),
-            direction(direction),
-            snr(snr),
-            used(used)
+        GPSSatellite(int id, float elevation, float azimuth, float snr, bool used) :
+                id(id),
+                elevation(elevation),
+                azimuth(azimuth),
+                snr(snr),
+                used(used),
+                lastUpdate(MG::TIME::getGroundTimeNowUsecs())
         {
 
         }
 
-        void update(int id, float azimuth, float direction, float snr, bool used)
+        void update(int id, float elevation, float azimuth, float snr, bool used)
         {
             this->id = id;
+            this->elevation = elevation;
             this->azimuth = azimuth;
-            this->direction = direction;
             this->snr = snr;
             this->used = used;
+            this->lastUpdate = MG::TIME::getGroundTimeNowUsecs();
         }
 
         int id;
+        float elevation;
         float azimuth;
-        float direction;
         float snr;
         bool used;
+        quint64 lastUpdate;
 
         friend class HSIDisplay;
     };
 
-    QVector<GPSSatellite*> gpsSatellites;
+    QMap<int, GPSSatellite*> gpsSatellites;
+    unsigned int satellitesUsed;
 
 private:
 };
diff --git a/src/ui/MAVLinkSettingsWidget.cc b/src/ui/MAVLinkSettingsWidget.cc
index 05f0bdd9e06ead478f8937acd32c2279d1593817..a64fbaa410e60c46fe142df966fb894054bba8da 100644
--- a/src/ui/MAVLinkSettingsWidget.cc
+++ b/src/ui/MAVLinkSettingsWidget.cc
@@ -11,9 +11,12 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget
     // Connect actions
     connect(protocol, SIGNAL(heartbeatChanged(bool)), m_ui->heartbeatCheckBox, SLOT(setChecked(bool)));
     connect(m_ui->heartbeatCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableHeartbeats(bool)));
+    connect(protocol, SIGNAL(loggingChanged(bool)), m_ui->loggingCheckBox, SLOT(setChecked(bool)));
+    connect(m_ui->loggingCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableLogging(bool)));
 
     // Initialize state
     m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled());
+    m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled());
 }
 
 MAVLinkSettingsWidget::~MAVLinkSettingsWidget()
diff --git a/src/ui/MAVLinkSettingsWidget.ui b/src/ui/MAVLinkSettingsWidget.ui
index de9b670737aad527b89e89a134f2b50e4a374d1d..c65627d3b23f6be6fc3292ec183701c2879810d4 100644
--- a/src/ui/MAVLinkSettingsWidget.ui
+++ b/src/ui/MAVLinkSettingsWidget.ui
@@ -24,6 +24,13 @@
      </property>
     </widget>
    </item>
+   <item>
+    <widget class="QCheckBox" name="loggingCheckBox">
+     <property name="text">
+      <string>Log all MAVLink packets</string>
+     </property>
+    </widget>
+   </item>
    <item>
     <spacer name="verticalSpacer">
      <property name="orientation">
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index fcfab3cf514f1334224be302697f5db1eabbe21f..3c48f41d4aa1a7f2f1a7f35e60c54e5cc4023be8 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -85,7 +85,7 @@ settings()
     waypoints->setVisible(false);
     info = new UASInfoWidget(this);
     info->setVisible(false);
-    detection = new ObjectDetectionView("test", this);
+    detection = new ObjectDetectionView("patterns", this);
     detection->setVisible(false);
     hud = new HUD(640, 480, this);
     hud->setVisible(false);
@@ -107,9 +107,9 @@ settings()
     acceptList->append("roll IMU");
     acceptList->append("pitch IMU");
     acceptList->append("yaw IMU");
-    acceptList->append("vx");
-    acceptList->append("vy");
-    acceptList->append("vz");
+    acceptList->append("rollspeed IMU");
+    acceptList->append("pitchspeed IMU");
+    acceptList->append("yawspeed IMU");
     headDown1 = new HDDisplay(acceptList, this);
     headDown1->setVisible(false);
 
diff --git a/src/ui/ObjectDetectionView.cc b/src/ui/ObjectDetectionView.cc
index 7b4d9be521b860497eb4ecd7bba9274116fc1ff1..3838acf4753c84da0519dfc060c60e7f04ec6b0e 100644
--- a/src/ui/ObjectDetectionView.cc
+++ b/src/ui/ObjectDetectionView.cc
@@ -94,7 +94,6 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i
         {
             //qDebug() << "REDETECTED";
 
-            /*
             QList<QAction*> actions = m_ui->listWidget->actions();
             // Find action and update it
             foreach (QAction* act, actions)
@@ -107,7 +106,9 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i
                     act->setText(patternPath + separator + "(#" + QString::number(count) + ")" + separator + QString::number(confidence));
                 }
             }
-            QPixmap image = QPixmap(patternFolder + "/" + patternPath);
+            QString filePath = MG::DIR::getSupportFilesDirectory() + "/" + patternFolder + "/" + patternPath.split("/").last();
+            qDebug() << "Loading:" << filePath;
+            QPixmap image = QPixmap(filePath);
             image = image.scaledToWidth(m_ui->imageLabel->width());
             m_ui->imageLabel->setPixmap(image);
             QString patternName = patternPath.split("//").last(); // Remove preceding folder names
@@ -115,7 +116,6 @@ void ObjectDetectionView::newDetection(int uasId, QString patternPath, int x1, i
 
             // Set name and label
             m_ui->nameLabel->setText(patternName);
-            */
         }
         else
         {