diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc
index f538df61bfc73374721b5c646bab287f828d9414..9b1fa9238435f538027b1cc25900ef162cdfad67 100644
--- a/src/uas/PxQuadMAV.cc
+++ b/src/uas/PxQuadMAV.cc
@@ -104,13 +104,6 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 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;
             }
             break;
     case MAVLINK_MSG_ID_AUX_STATUS:
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 8fbf860836afc94cb4a70cf14b387f70ce460353..3d3ee4a54baf3a0714119e1a4853503cd90db404 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -100,7 +100,7 @@ void UAS::updateState()
     }
     else
     {
-        if (mode > MAV_MODE_LOCKED && positionLock)
+        if (mode > (uint8_t)MAV_MODE_LOCKED && positionLock)
         {
             GAudioOutput::instance()->notifyNegative();
         }
@@ -301,6 +301,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "vy", pos.vy, time);
                 emit valueChanged(uasId, "vz", pos.vz, time);
                 emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
+                emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
+                // Set internal state
+                if (!positionLock)
+                {
+                    // If position was not locked before, notify positive
+                    GAudioOutput::instance()->notifyPositive();
+                }
+                positionLock = true;
             }
             break;
         case MAVLINK_MSG_ID_GLOBAL_POSITION:
@@ -317,6 +325,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "g-vy", pos.vy, time);
                 emit valueChanged(uasId, "g-vz", pos.vz, time);
                 emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
+                emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
+                // Set internal state
+                if (!positionLock)
+                {
+                    // If position was not locked before, notify positive
+                    GAudioOutput::instance()->notifyPositive();
+                }
+                positionLock = true;
             }
             break;
         case MAVLINK_MSG_ID_GPS_RAW:
@@ -464,6 +480,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
     }
 }
 
+void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw)
+{
+    mavlink_message_t msg;
+
+}
+
 quint64 UAS::getUnixTime(quint64 time)
 {
     if (time == 0)
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 5a644b8b13cbb570d2ed14e7e959119859b3648c..88cb4027b1709a413eb1784bc58d01d270c14b9a 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -216,6 +216,9 @@ public slots:
     /** @brief Update the system state */
     void updateState();
 
+    /** @brief Set local position setpoint */
+    void setLocalPositionSetpoint(float x, float y, float z, float yaw);
+
 signals:
 
     /** @brief The main/battery voltage has changed/was updated */