diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index 77cc0d1210d77697c1f2f969bd71f94cafcbb517..db29cc0258fa93ff469cfbe65840effd46e02188 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -409,7 +409,7 @@ void MAVLinkSimulationLink::mainloop()
 //        streampointer += bufferlength;
 
         // GLOBAL POSITION
-        mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, xSpeed, ySpeed, zSpeed);
+        mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, xSpeed, ySpeed, zSpeed, yaw);
         bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
         //add data into datastream
         memcpy(stream+streampointer,buffer, bufferlength);
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 3efaebda2d3fa44e6a5e2d3743925c3e3139f70e..0b405f608b74cac21ad4263951b6bab1b783cc62 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -75,7 +75,7 @@ paramsOnceRequested(false),
 airframe(0),
 attitudeKnown(false),
 paramManager(NULL),
-attitudeStamped(true),
+attitudeStamped(false),
 lastAttitude(0)
 {
     color = UASInterface::getNextColor();
@@ -682,7 +682,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
                 if (pos.fix_type > 0) {
                     emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time);
-                    emit valueChanged(uasId, "gps speed", "m/s", pos.v, time);
+                    emit valueChanged(uasId, "gps speed", "m/s", pos.vel, time);
                     latitude = pos.lat/(double)1E7;
                     longitude = pos.lon/(double)1E7;
                     altitude = pos.alt/1000.0;
@@ -696,12 +696,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     }
                     emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time);
                     // Smaller than threshold and not NaN
-                    if (pos.v < 1000000 && pos.v == pos.v) {
-                        emit valueChanged(uasId, "speed", "m/s", pos.v, time);
+
+                    float vel = pos.vel/100.0f;
+
+                    if (vel < 1000000 && !isnan(vel) && !isinf(vel)) {
+                        emit valueChanged(uasId, "speed", "m/s", vel, time);
                         //qDebug() << "GOT GPS RAW";
                         // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
                     } else {
-                        emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v));
+                        emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
                     }
                 }
             }
@@ -854,6 +857,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 {
                     waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
                 }
+                else
+                {
+                    qDebug() << "Got waypoint message, but was not for me";
+                }
             }
             break;
 
@@ -866,6 +873,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 {
                     waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
                 }
+                else
+                {
+                    qDebug() << "Got waypoint message, but was not for me";
+                }
             }
             break;
 
@@ -888,6 +899,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 {
                     waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
                 }
+                else
+                {
+                    qDebug() << "Got waypoint message, but was not for me";
+                }
             }
             break;
 
diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc
index 5572cded1e8a573f75cf06d98e1f91e8d08f835d..340470c2f0a2d9828dfe04be8a5df39085ada172 100644
--- a/src/uas/UASWaypointManager.cc
+++ b/src/uas/UASWaypointManager.cc
@@ -32,7 +32,6 @@ This file is part of the QGROUNDCONTROL project
 #include "UASWaypointManager.h"
 #include "UAS.h"
 #include "mavlink_types.h"
-//#include "MainWindow.h"
 
 #define PROTOCOL_TIMEOUT_MS 2000    ///< maximum time to wait for pending messages until timeout
 #define PROTOCOL_DELAY_MS 40        ///< minimum delay between sent messages