diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 547fb5f2dce9f4583f6b65355fee5b309674a2ca..06eddf9bb2f542b85afbcf6e6c580c45358278a5 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -1344,6 +1344,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         case MAVLINK_MSG_ID_RAW_IMU:
         case MAVLINK_MSG_ID_SCALED_IMU:
         case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
+        {
+            //mavlink_set_local_position_setpoint_t p;
+            //mavlink_msg_set_local_position_setpoint_decode(&message, &p);
+            //emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw);
+            mavlink_nav_controller_output_t p;
+            mavlink_msg_nav_controller_output_decode(&message,&p);
+            setDistToWaypoint(p.wp_dist);
+        }
+            break;
         case MAVLINK_MSG_ID_RAW_PRESSURE:
         case MAVLINK_MSG_ID_SCALED_PRESSURE:
         case MAVLINK_MSG_ID_OPTICAL_FLOW:
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 8b634d259f8770f02c54254f0c75cd21b0401a2d..01f5a70f42a49980d5395d5e50dfac66df43a876 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -112,6 +112,7 @@ public:
     Q_PROPERTY(double roll READ getRoll WRITE setRoll NOTIFY rollChanged)
     Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged)
     Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged)
+    Q_PROPERTY(double distToWaypoint READ getDistToWaypoint WRITE setDistToWaypoint NOTIFY distToWaypointChanged)
 
     void setLocalX(double val)
     {
@@ -194,6 +195,17 @@ public:
         return isGlobalPositionKnown;
     }
 
+    void setDistToWaypoint(double val)
+    {
+        distToWaypoint = val;
+        emit distToWaypointChanged(val,"distToWaypoint");
+    }
+
+    double getDistToWaypoint() const
+    {
+        return distToWaypoint;
+    }
+
     void setRoll(double val)
     {
         roll = val;
@@ -370,6 +382,7 @@ protected: //COMMENTS FOR TEST UNIT
     double speedX;              ///< True speed in X axis
     double speedY;              ///< True speed in Y axis
     double speedZ;              ///< True speed in Z axis
+    double distToWaypoint;       ///< Distance to next waypoint
     double roll;
     double pitch;
     double yaw;
@@ -792,6 +805,7 @@ signals:
     void pitchChanged(double val,QString name);
     void yawChanged(double val,QString name);
     void satelliteCountChanged(double val,QString name);
+    void distToWaypointChanged(double val,QString name);
 
 protected:
     /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */
diff --git a/src/ui/uas/UASQuickView.cpp b/src/ui/uas/UASQuickView.cpp
index ad51c38bc61e1336953551dfe830bf66f25049d3..03bbd23161ff550cd11b5ddee3e865eb0cc7b211 100644
--- a/src/ui/uas/UASQuickView.cpp
+++ b/src/ui/uas/UASQuickView.cpp
@@ -61,6 +61,18 @@ UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent)
         uasPropertyToLabelMap["satelliteCount"] = item;
     }
 
+    {
+        QAction *action = new QAction("distToWaypoint",this);
+        action->setCheckable(true);
+        action->setChecked(true);
+        connect(action,SIGNAL(toggled(bool)),this,SLOT(actionTriggered(bool)));
+        this->addAction(action);
+        UASQuickViewItem *item = new UASQuickViewItem(this);
+        item->setTitle("distToWaypoint");
+        ui.verticalLayout->addWidget(item);
+        uasPropertyToLabelMap["distToWaypoint"] = item;
+    }
+
     updateTimer = new QTimer(this);
     connect(updateTimer,SIGNAL(timeout()),this,SLOT(updateTimerTick()));
     updateTimer->start(1000);