Added coordinate transform

src/QGC.h

@@ -8,7 +8,7 @@ namespace QGC
 {
     const QColor ColorCyan(55, 154, 195);
 
-    quint64 groundTimeUsecs()
+    static quint64 groundTimeUsecs()
     {
         QDateTime time = QDateTime::currentDateTime();
         time = time.toUTC();

src/comm/MAVLinkSimulationLink.cc

@@ -42,6 +42,7 @@ This file is part of the PIXHAWK project
 #include "MAVLinkSimulationLink.h"
 // MAVLINK includes
 #include <mavlink.h>
+#include "QGC.h"
 
 /**
  * Create a simulated link. This link is connected to an input and output file.
@@ -97,6 +98,11 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
     // Open packet log
     mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName());
     mavlinkLogFile->open(QIODevice::ReadOnly);
+
+    x = 0;
+    y = 0;
+    z = 0;
+    yaw = 0;
 }
 
 MAVLinkSimulationLink::~MAVLinkSimulationLink()
@@ -369,6 +375,11 @@ void MAVLinkSimulationLink::mainloop()
     if (rate10hzCounter == 1000 / rate / 9)
     {
         rate10hzCounter = 1;
+
+        // Move X Position
+        x += sin(QGC::groundTimeUsecs());
+        y += sin(QGC::groundTimeUsecs());
+        z += sin(QGC::groundTimeUsecs());
     }
 
     // 1 HZ TASKS
@@ -538,6 +549,25 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                     // Set mode indepent of mode.target
                     status.mode = mode.mode;
                 }
+                break;
+            case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
+                {
+                    mavlink_local_position_setpoint_set_t set;
+                    mavlink_msg_local_position_setpoint_set_decode(&msg, &set);
+                    spX = set.x;
+                    spY = set.y;
+                    spZ = set.z;
+                    spYaw = set.yaw;
+
+                    // Send back new setpoint
+                    mavlink_message_t ret;
+                    mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ, spYaw);
+                    bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
+                    //add data into datastream
+                    memcpy(stream+streampointer,buffer, bufferlength);
+                    streampointer += bufferlength;
+                }
+                break;
                 // EXECUTE OPERATOR ACTIONS
             case MAVLINK_MSG_ID_ACTION:
                 {

src/comm/MAVLinkSimulationLink.h

@@ -93,6 +93,8 @@ protected:
 
     // UAS properties
     float roll, pitch, yaw;
+    float x, y, z;
+    float spX, spY, spZ, spYaw;
     int battery;
 
     QTimer* timer;

src/ui/HSIDisplay.cc

@@ -199,11 +199,27 @@ void HSIDisplay::paintDisplay()
     paintText(tr("E"), ringColor, 3.5f, + baseRadius + 2.0f, - 1.75f, &painter);
     paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter);
 
-    // FIXME Just for testing
-    bodyXSetCoordinate = 0.95 * bodyXSetCoordinate + 0.05 * uiXSetCoordinate;
-    bodyYSetCoordinate = 0.95 * bodyYSetCoordinate + 0.05 * uiYSetCoordinate;
-    bodyZSetCoordinate = 0.95 * bodyZSetCoordinate + 0.05 * uiZSetCoordinate;
-    bodyYawSet = 0.95 * bodyYawSet + 0.05 * uiYawSet;
+//    // FIXME Just for testing
+//    bodyXSetCoordinate = 0.95 * bodyXSetCoordinate + 0.05 * uiXSetCoordinate;
+//    bodyYSetCoordinate = 0.95 * bodyYSetCoordinate + 0.05 * uiYSetCoordinate;
+//    bodyZSetCoordinate = 0.95 * bodyZSetCoordinate + 0.05 * uiZSetCoordinate;
+//    bodyYawSet = 0.95 * bodyYawSet + 0.05 * uiYawSet;
+}
+
+QPointF HSIDisplay::metricWorldToBody(QPointF world)
+{
+    // First translate to body-centered coordinates
+    // Rotate around -yaw
+    QPointF result(sin(-yaw) * (world.x() - x), cos(-yaw) * (world.y() - y));
+    return result;
+}
+
+QPointF HSIDisplay::metricBodyToWorld(QPointF body)
+{
+    // First rotate into world coordinates
+    // then translate to world position
+    QPointF result((sin(yaw) * body.x()) + x, (cos(yaw) * body.y()) + y);
+    return result;
 }
 
 QPointF HSIDisplay::screenToMetricBody(QPointF ref)

src/ui/HSIDisplay.h

@@ -81,7 +81,12 @@ protected slots:
     void mouseDoubleClickEvent(QMouseEvent* event);
 
 protected:
+    /** @brief Get color from GPS signal-to-noise colormap */
     static QColor getColorForSNR(float snr);
+    /** @brief Metric world coordinates to metric body coordinates */
+    QPointF metricWorldToBody(QPointF world);
+    /** @brief Metric body coordinates to metric world coordinates */
+    QPointF metricBodyToWorld(QPointF body);
     /** @brief Screen coordinates of widget to metric coordinates in body frame */
     QPointF screenToMetricBody(QPointF ref);
     /** @brief Reference coordinates to metric coordinates */