diff --git a/src/QGC.h b/src/QGC.h
index f8ed9f123fd3b6e79574250a72115fc11752d9c9..5dfdd881add82475179d3035e8bd3d4fbd1922d0 100644
--- a/src/QGC.h
+++ b/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();
diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index 2049e55f76b232d05ced9b380495dc56ef34abe1..7892a5f4b2eb54d1c999dbdd737b11746e116356 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/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:
                 {
diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h
index 0b10e375390a225f4f8d7a247dd41a3754b7c96c..5ff98d0110f059c9795365c7e81882b3ec87ea1c 100644
--- a/src/comm/MAVLinkSimulationLink.h
+++ b/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;
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index e8466ea6374b3c23ac57eabb22e0e215f20992fc..458472460d22be604cbce301bcae46b428d8f554 100644
--- a/src/ui/HSIDisplay.cc
+++ b/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)
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index 4c7899ec6938d7612c6461cb9845f7d7494e8bc7..a62f65e372d9252be91196c7e21bf0de81bd3b1f 100644
--- a/src/ui/HSIDisplay.h
+++ b/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 */