From bc8363d8a26678bf6cc95c11bda0a38c54b14aaa Mon Sep 17 00:00:00 2001
From: pixhawk <pixhawk@student.ethz.ch>
Date: Tue, 15 Jun 2010 08:36:50 +0200
Subject: [PATCH] pushed setpoint visualization forward

---
 src/QGC.h            | 10 ++++++++++
 src/uas/UAS.cc       | 10 +++++++++-
 src/ui/HSIDisplay.cc | 22 ++++++++++++++++------
 3 files changed, 35 insertions(+), 7 deletions(-)

diff --git a/src/QGC.h b/src/QGC.h
index c2e813ab0..f8ed9f123 100644
--- a/src/QGC.h
+++ b/src/QGC.h
@@ -1,11 +1,21 @@
 #ifndef QGC_H
 #define QGC_H
 
+#include <QDateTime>
 #include <QColor>
 
 namespace QGC
 {
     const QColor ColorCyan(55, 154, 195);
+
+    quint64 groundTimeUsecs()
+    {
+        QDateTime time = QDateTime::currentDateTime();
+        time = time.toUTC();
+        /* Return seconds and milliseconds, in milliseconds unit */
+        quint64 microseconds = time.toTime_t() * static_cast<quint64>(1000000);
+        return static_cast<quint64>(microseconds + (time.time().msec()*1000));
+    }
 }
 
 #endif // QGC_H
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 3d3ee4a54..deadfb708 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -39,6 +39,7 @@ This file is part of the PIXHAWK project
 #include "LinkInterface.h"
 #include "UASManager.h"
 #include "MG.h"
+#include "QGC.h"
 #include "GAudioOutput.h"
 #include "MAVLinkProtocol.h"
 #include <mavlink.h>
@@ -406,7 +407,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_position_controller_output_t out;
                 mavlink_msg_position_controller_output_decode(&message, &out);
                 quint64 time = MG::TIME::getGroundTimeNowUsecs();
-                emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time);
+                //emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time);
                 emit valueChanged(uasId, "pos control x", out.x, time/1000.0f);
                 emit valueChanged(uasId, "pos control y", out.y, time/1000.0f);
                 emit valueChanged(uasId, "pos control z", out.z, time/1000.0f);
@@ -452,6 +453,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 //                emit waypointReached(this, wp.id);
             }
             break;
+        case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT:
+            {
+                mavlink_local_position_setpoint_t p;
+                mavlink_msg_local_position_setpoint_decode(&message, &p);
+                emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs());
+            }
+            break;
         case MAVLINK_MSG_ID_STATUSTEXT:
             {
                 QByteArray b;
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index b9bfe7ec4..e8466ea63 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -239,18 +239,24 @@ void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
     }
     else if (event->MouseButtonPress)
     {
+        startX = event->globalX();
         if (event->button() == Qt::RightButton)
         {
-            startX = event->globalX();
+            // Start tracking mouse move
+            dragStarted = true;
         }
         else if (event->button() == Qt::LeftButton)
         {
 
         }
     }
+    else if (event->MouseButtonRelease)
+    {
+        dragStarted = false;
+    }
     else if (event->MouseMove)
     {
-
+        if (dragStarted) uiYawSet += (startX - event->globalX()) / this->frameSize().width();
     }
 }
 
@@ -332,10 +338,14 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir
 {
     Q_UNUSED(usec);
     Q_UNUSED(uasid);
-    Q_UNUSED(yawDesired);
-    posXSet = xDesired;
-    posYSet = yDesired;
-    posZSet = zDesired;
+    bodyXSetCoordinate = xDesired;
+    bodyYSetCoordinate = yDesired;
+    bodyZSetCoordinate = zDesired;
+    bodyYawSet = yawDesired;
+//    posXSet = xDesired;
+//    posYSet = yDesired;
+//    posZSet = zDesired;
+//    posYawSet = yawDesired;
 }
 
 void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec)
-- 
2.22.0