diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 76c7c7a801f4af7c4dec110d033cb1ee53a3999a..be1dba7d2a72845fd633e2e0bd50d904afa9ff6b 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -72,7 +72,8 @@ pitch(0.0),
 yaw(0.0),
 statusTimeout(new QTimer(this)),
 paramsOnceRequested(false),
-airframe(0)
+airframe(0),
+attitudeKnown(false)
 {
     color = UASInterface::getNextColor();
     setBattery(LIPOLY, 3);
@@ -448,6 +449,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     compass -= 360.0f;
                 }
 
+                attitudeKnown = true;
+
                 emit valueChanged(uasId, "roll deg", "deg", (roll/M_PI)*180.0, time);
                 emit valueChanged(uasId, "pitch deg", "deg", (pitch/M_PI)*180.0, time);
                 emit valueChanged(uasId, "heading deg", "deg", compass, time);
@@ -472,6 +475,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "climbrate", "m/s", hud.climb, time);
                 emit valueChanged(uasId, "throttle", "%", hud.throttle, time);
                 emit thrustChanged(this, hud.throttle/100.0);
+
+                if (!attitudeKnown)
+                {
+                    yaw = QGC::limitAngleToPMPId((((double)hud.heading-180.0)/360.0)*M_PI);
+                    emit attitudeChanged(this, roll, pitch, yaw, time);
+                }
+
                 emit altitudeChanged(uasId, hud.alt);
                 //yaw = (hud.heading-180.0f/360.0f)*M_PI;
                 //emit attitudeChanged(this, roll, pitch, yaw, getUnixTime());
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 6a1ad5fa010f8d455fb93a41daacac9426c6d92d..008cbd6c5b97e32aec7f14dafc9b01bf0d87b9fd 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -175,6 +175,8 @@ protected: //COMMENTS FOR TEST UNIT
     QMap<int, QMap<QString, float>* > parameters; ///< All parameters
     bool paramsOnceRequested;   ///< If the parameter list has been read at least once
     int airframe;               ///< The airframe type
+    bool attitudeKnown;         ///< True if attitude was received, false else
+
 public:
     /** @brief Set the current battery type */
     void setBattery(BatteryType type, int cells);