diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 54164101feda4fc35813ffaee976b95d292bbfa7..06595e4812d4de9c9ca35a00a09a4cf922ff85ea 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -159,42 +159,42 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 if (state.status != this->status)
                 {
                     statechanged = true;
-                    this->status = state.status;
+                    this->status = (int)state.status;
                     getStatusForCode((int)state.status, uasState, stateDescription);
                     emit statusChanged(this, uasState, stateDescription);
                     stateAudio = " changed status to " + uasState;
                 }
 
-                if (static_cast<unsigned int>(this->mode) != static_cast<unsigned int>(state.mode))
+                if (this->mode != static_cast<unsigned int>(state.mode))
                 {
                     modechanged = true;
-                    this->mode = state.mode;
+                    this->mode = static_cast<unsigned int>(state.mode);
                     QString mode;
 
-                    switch ((unsigned int)(state.mode))
+                    switch (state.mode)
                     {
-                    case MAV_MODE_LOCKED:
+                    case (uint8_t)MAV_MODE_LOCKED:
                         mode = "LOCKED MODE";
                         break;
-                    case MAV_MODE_MANUAL:
+                    case (uint8_t)MAV_MODE_MANUAL:
                         mode = "MANUAL MODE";
                         break;
-                    case MAV_MODE_AUTO:
+                    case (uint8_t)MAV_MODE_AUTO:
                         mode = "AUTO MODE";
                         break;
-                    case MAV_MODE_GUIDED:
+                    case (uint8_t)MAV_MODE_GUIDED:
                         mode = "GUIDED MODE";
                         break;
-                    case MAV_MODE_READY:
+                    case (uint8_t)MAV_MODE_READY:
                         mode = "READY";
                         break;
-                    case MAV_MODE_TEST1:
+                    case (uint8_t)MAV_MODE_TEST1:
                         mode = "TEST1 MODE";
                         break;
-                    case MAV_MODE_TEST2:
+                    case (uint8_t)MAV_MODE_TEST2:
                         mode = "TEST2 MODE";
                         break;
-                    case MAV_MODE_TEST3:
+                    case (uint8_t)MAV_MODE_TEST3:
                         mode = "TEST3 MODE";
                         break;
                     default:
@@ -324,14 +324,33 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_gps_raw_t pos;
                 mavlink_msg_gps_raw_decode(&message, &pos);
+
+                // SANITY CHECK
+                // only accept values in a realistic range
                // quint64 time = getUnixTime(pos.usec);
                 quint64 time = MG::TIME::getGroundTimeNow();
                 emit valueChanged(uasId, "lat", pos.lat, time);
                 emit valueChanged(uasId, "lon", pos.lon, time);
+                // Check for NaN
+                int alt = pos.alt;
+                if (alt != alt)
+                {
+                    alt = 0;
+                    emit textMessageReceived(uasId, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
+                }
                 emit valueChanged(uasId, "alt", pos.alt, time);
-                emit valueChanged(uasId, "speed", pos.v, time);
-                //qDebug() << "GOT GPS RAW";
-                emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
+                // Smaller than threshold and not NaN
+                if (pos.v < 1000000 && pos.v == pos.v)
+                {
+                    emit valueChanged(uasId, "speed", pos.v, time);
+                    //qDebug() << "GOT GPS RAW";
+                    emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
+                }
+                else
+                {
+                     emit textMessageReceived(uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v));
+                }
+                emit globalPositionChanged(this, pos.lon, pos.lat, alt, time);
             }
             break;
         case MAVLINK_MSG_ID_GPS_STATUS:
@@ -454,13 +473,12 @@ quint64 UAS::getUnixTime(quint64 time)
 
 void UAS::setMode(int mode)
 {
-    if (mode >= MAV_MODE_LOCKED && mode <= MAV_MODE_TEST3)
+    if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_TEST3)
     {
-        this->mode = mode;
         mavlink_message_t msg;
-        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, (unsigned char)mode);
+        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode);
         sendMessage(msg);
-        qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << mode;
+        qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode;
     }
 }
 
@@ -838,7 +856,7 @@ void UAS::launch()
 {
     mavlink_message_t msg;
     // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(int)MAV_ACTION_LAUNCH);
+    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_LAUNCH);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);
@@ -852,7 +870,7 @@ void UAS::enable_motors()
 {
     mavlink_message_t msg;
     // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(int)MAV_ACTION_MOTORS_START);
+    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_MOTORS_START);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);
@@ -866,7 +884,7 @@ void UAS::disable_motors()
 {
     mavlink_message_t msg;
     // TODO Replace MG System ID with static function call and allow to change ID in GUI
-    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(int)MAV_ACTION_MOTORS_STOP);
+    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_MOTORS_STOP);
     // Send message twice to increase chance of reception
     sendMessage(msg);
     sendMessage(msg);
@@ -884,7 +902,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
     manualYawAngle = yaw * yawScaling;
     manualThrust = thrust * thrustScaling;
 
-    if(mode == MAV_MODE_MANUAL)
+    if(mode == (int)MAV_MODE_MANUAL)
     {
         mavlink_message_t message;
         mavlink_msg_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 5dd1c37f474eccf34dcb0ffa1dbc826c5b39fff2..5a644b8b13cbb570d2ed14e7e959119859b3648c 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -110,7 +110,7 @@ protected:
     double currentVoltage;      ///< Voltage currently measured
     float lpVoltage;            ///< Low-pass filtered voltage
     int timeRemaining;          ///< Remaining time calculated based on previous and current
-    int mode;                   ///< The current mode of the MAV
+    unsigned int mode;          ///< The current mode of the MAV
     int status;                 ///< The current status of the MAV
     quint64 onboardTimeOffset;
 
diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc
index 41375f29b55d272400e105bc6b28d7f5b8eef28f..ff2f5be357c668ba8dd58ae92088b1f219006852 100644
--- a/src/ui/uas/UASControlWidget.cc
+++ b/src/ui/uas/UASControlWidget.cc
@@ -42,19 +42,32 @@ This file is part of the PIXHAWK project
 #include <UAS.h>
 //#include <mavlink.h>
 
+#define CONTROL_MODE_LOCKED "MODE LOCKED"
+#define CONTROL_MODE_MANUAL "MODE MANUAL"
+#define CONTROL_MODE_GUIDED "MODE GUIDED"
+#define CONTROL_MODE_AUTO   "MODE AUTO"
+#define CONTROL_MODE_TEST1  "MODE TEST1"
+
+#define CONTROL_MODE_LOCKED_INDEX 2
+#define CONTROL_MODE_MANUAL_INDEX 3
+#define CONTROL_MODE_GUIDED_INDEX 4
+#define CONTROL_MODE_AUTO_INDEX   5
+#define CONTROL_MODE_TEST1_INDEX  6
+
 UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
         uas(NULL)
 {
     ui.setupUi(this);
 
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
-    ui.modeComboBox->insertItem(MAV_MODE_LOCKED, "MODE LOCKED");
-    ui.modeComboBox->insertItem(MAV_MODE_MANUAL, "MODE MANUAL");
-    ui.modeComboBox->insertItem(MAV_MODE_GUIDED, "MODE GUIDED");
-    ui.modeComboBox->insertItem(MAV_MODE_AUTO, "MODE AUTO");
-    ui.modeComboBox->insertItem(MAV_MODE_TEST1, "MODE TEST1");
-    ui.modeComboBox->insertItem(MAV_MODE_TEST2, "MODE TEST2");
-    ui.modeComboBox->insertItem(MAV_MODE_TEST3, "MODE TEST3");
+    ui.modeComboBox->insertItem(0, "Select..");
+    ui.modeComboBox->insertItem(CONTROL_MODE_LOCKED_INDEX, CONTROL_MODE_LOCKED);
+    ui.modeComboBox->insertItem(CONTROL_MODE_MANUAL_INDEX, CONTROL_MODE_MANUAL);
+    ui.modeComboBox->insertItem(CONTROL_MODE_GUIDED_INDEX, CONTROL_MODE_GUIDED);
+    ui.modeComboBox->insertItem(CONTROL_MODE_AUTO_INDEX, CONTROL_MODE_AUTO);
+    ui.modeComboBox->insertItem(CONTROL_MODE_TEST1_INDEX, CONTROL_MODE_TEST1);
+
+    ui.modeComboBox->setCurrentIndex(0);
 }
 
 void UASControlWidget::setUAS(UASInterface* uas)
@@ -92,36 +105,48 @@ UASControlWidget::~UASControlWidget() {
 void UASControlWidget::setMode(int mode)
 {
     // Adapt context button mode
-    switch (mode)
+    if (mode == CONTROL_MODE_LOCKED_INDEX)
     {
-        case MAV_MODE_LOCKED:
-        break;
-        case MAV_MODE_MANUAL:
-        break;
-        case MAV_MODE_GUIDED:
-        break;
-        case MAV_MODE_AUTO:
-        break;
-        case MAV_MODE_TEST1:
-        break;
-        case MAV_MODE_TEST2:
-        break;
-        case MAV_MODE_TEST3:
-        break;
+        uasMode = (unsigned int)MAV_MODE_LOCKED;
+        ui.modeComboBox->setCurrentIndex(mode);
     }
-
-    // Set mode on system
-    if (mode >= MAV_MODE_LOCKED && mode <= MAV_MODE_TEST3)
+    else if (mode == CONTROL_MODE_MANUAL_INDEX)
+    {
+        uasMode = (unsigned int)MAV_MODE_MANUAL;
+        ui.modeComboBox->setCurrentIndex(mode);
+    }
+    else if (mode == CONTROL_MODE_GUIDED_INDEX)
     {
-        uasMode = mode;
+        uasMode = (unsigned int)MAV_MODE_GUIDED;
         ui.modeComboBox->setCurrentIndex(mode);
     }
-    qDebug() << "SET MODE REQUESTED" << mode;
+    else if (mode == CONTROL_MODE_AUTO_INDEX)
+    {
+        uasMode = (unsigned int)MAV_MODE_AUTO;
+        ui.modeComboBox->setCurrentIndex(mode);
+    }
+    else if (mode == CONTROL_MODE_TEST1_INDEX)
+    {
+        uasMode = (unsigned int)MAV_MODE_TEST1;
+        ui.modeComboBox->setCurrentIndex(mode);
+    }
+    else
+    {
+        qDebug() << "ERROR! MODE NOT FOUND";
+        uasMode = 0;
+    }
+
+
+    qDebug() << "SET MODE REQUESTED" << uasMode;
 }
 
 void UASControlWidget::transmitMode()
 {
-    this->uas->setMode(uasMode);
+    if (uasMode != 0)
+    {
+        this->uas->setMode(uasMode);
+        ui.lastActionLabel->setText(QString("Set new mode for system %1").arg(uas->getUASName()));
+    }
 }
 
 void UASControlWidget::cycleContextButton()
@@ -139,12 +164,14 @@ void UASControlWidget::cycleContextButton()
             ui.controlButton->setText(tr("Stop Engine"));
             mav->setMode(MAV_MODE_MANUAL);
             mav->enable_motors();
+            ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(uas->getUASName()));
             state++;
             break;
         case 1:
             ui.controlButton->setText(tr("Activate Engine"));
             mav->setMode(MAV_MODE_LOCKED);
             mav->disable_motors();
+            ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(uas->getUASName()));
             state = 0;
             break;
         case 2:
diff --git a/src/ui/uas/UASControlWidget.h b/src/ui/uas/UASControlWidget.h
index efb892efe0a3dd6a797174c3240f7ecd298d24a1..33ad1e09f228eece5732f97591067c3426d94d36 100644
--- a/src/ui/uas/UASControlWidget.h
+++ b/src/ui/uas/UASControlWidget.h
@@ -58,7 +58,7 @@ public slots:
 
 protected:
     UASInterface* uas;
-    int uasMode;
+    unsigned int uasMode;
 
 private:
     Ui::uasControl ui;
diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc
index 10dbd9a14ef8425977c6fbcfea90a095707dd8ae..defc81c479a48ee3069beb6351d6fc6114a342e3 100644
--- a/src/ui/uas/UASView.cc
+++ b/src/ui/uas/UASView.cc
@@ -40,16 +40,26 @@ This file is part of the PIXHAWK project
 
 UASView::UASView(UASInterface* uas, QWidget *parent) :
         QWidget(parent),
+        startTime(0),
         timeRemaining(0),
+        chargeLevel(0),
+        uas(uas),
+        load(0),
         state("UNKNOWN"),
         stateDesc(tr("Unknown system state")),
         mode("MAV_MODE_UNKNOWN"),
         thrust(0),
         isActive(false),
+        x(0),
+        y(0),
+        z(0),
+        totalSpeed(0),
+        lat(0),
+        lon(0),
+        alt(0),
+        groundDistance(0),
         m_ui(new Ui::UASView)
 {
-    this->uas = uas;
-
     m_ui->setupUi(this);
 
     // Setup communication
@@ -316,9 +326,37 @@ void UASView::refresh()
     position = position.sprintf("%02.2f %02.2f %02.2f m", x, y, z);
     m_ui->positionLabel->setText(position);
     QString globalPosition;
-    globalPosition = globalPosition.sprintf("%02.2f %02.2f %02.2f m", lon, lat, alt);
+    QString latIndicator;
+    if (lat > 0)
+    {
+        latIndicator = "N";
+    }
+    else
+    {
+        latIndicator = "S";
+    }
+    QString lonIndicator;
+    if (lon > 0)
+    {
+        lonIndicator = "E";
+    }
+    else
+    {
+        lonIndicator = "W";
+    }
+    globalPosition = globalPosition.sprintf("%02.2f%s %02.2f%s %02.2f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt);
     m_ui->gpsLabel->setText(globalPosition);
 
+    // Altitude
+    if (groundDistance == 0 && alt != 0)
+    {
+        m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt));
+    }
+    else
+    {
+        m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance));
+    }
+
     // Speed
     QString speed;
     speed = speed.sprintf("%02.2f m/s", totalSpeed);
diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h
index bcd91c11b803006b9d72f1313c72435adda23106..928423e35d2589e5583765583c353b50eba1c681 100644
--- a/src/ui/uas/UASView.h
+++ b/src/ui/uas/UASView.h
@@ -93,6 +93,7 @@ protected:
     float lat;
     float lon;
     float alt;
+    float groundDistance;
 
 
     void mouseDoubleClickEvent (QMouseEvent * event);