From 0d1656d899eba9495dcdad9afca081429fcc9c18 Mon Sep 17 00:00:00 2001
From: Michael Carpenter <malcom2073@gmail.com>
Date: Tue, 7 May 2013 12:27:03 -0400
Subject: [PATCH] Added support for MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT for
 Distance to waypoint property

---
 src/uas/UAS.cc              |  9 +++++++++
 src/uas/UAS.h               | 14 ++++++++++++++
 src/ui/uas/UASQuickView.cpp | 12 ++++++++++++
 3 files changed, 35 insertions(+)

diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 547fb5f2dc..06eddf9bb2 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 8b634d259f..01f5a70f42 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 ad51c38bc6..03bbd23161 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);
-- 
GitLab