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 e2ca3c1750c597d2ed1c4333fadd4ca042b143d3..c09cdee664a67ff5780f9c378dbc49be3e3b6367 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -206,6 +206,22 @@ void HSIDisplay::paintDisplay()
 //    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)
 {
     return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth - x, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth - y);
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 */
diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc
index 9512b77aa2408b9fdd7e0087263cb88e7a45b728..5b608f0289ec53d56c7c2eb34ef6ff152dc6c1c0 100644
--- a/src/ui/WaypointList.cc
+++ b/src/ui/WaypointList.cc
@@ -66,7 +66,7 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
     connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit()));
 
     // REQUEST WAYPOINTS
-    connect(m_ui->readButton, SIGNAL(clicked()), this, SIGNAL(requestWaypoints()));
+    connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read()));
 
     // SAVE/LOAD WAYPOINTS
     connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints()));
@@ -105,11 +105,6 @@ void WaypointList::setUAS(UASInterface* uas)
         connect(this, SIGNAL(clearWaypointList()),                          &uas->getWaypointManager(), SLOT(clearWaypointList()));
 
         connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &)));
-
-        // This slot is not implemented in UAS: connect(this, SIGNAL(removeWaypointId(int)), uas, SLOT(removeWaypoint(Waypoint*)));
-
-        //qDebug() << "Requesting waypoints";
-        //emit requestWaypoints();
     }
 }
 
@@ -158,29 +153,24 @@ void WaypointList::waypointReached(UASInterface* uas, quint16 waypointId)
     }*/
 }
 
-void WaypointList::transmit()
+void WaypointList::read()
 {
-    transmitDelay->start(1000);
-    m_ui->transmitButton->setEnabled(false);
-    emit clearWaypointList();
-
-    for(int i = 0; i < waypoints.size(); i++)
-    {
-        //Waypoint* wp = waypoints[i];
-        //emit waypointChanged(wp);
-        //if (wp->current)
-        //    emit currentWaypointChanged(wp->id);
-    }
-
-    emit sendWaypoints(waypoints);
-
     while(waypoints.size()>0)
     {
         removeWaypoint(waypoints[0]);
     }
+    
     emit requestWaypoints();
 }
 
+void WaypointList::transmit()
+{
+    transmitDelay->start(1000);
+    m_ui->transmitButton->setEnabled(false);
+
+    emit sendWaypoints(waypoints);
+}
+
 void WaypointList::add()
 {
     // Only add waypoints if UAS is present
@@ -287,11 +277,6 @@ void WaypointList::moveDown(Waypoint* wp)
     }
 }
 
-/*void WaypointList::removeWaypointAndName(Waypoint* wp)
-{
-    removeWaypoint(wp);
-}*/
-
 void WaypointList::removeWaypoint(Waypoint* wp)
 {
     // Delete from list
diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h
index 403e71e0ce2c7a7913f10020d92bc6dfcea7df0c..8aeef71a78cf351adb1c9e02db426b41e5764bbe 100644
--- a/src/ui/WaypointList.h
+++ b/src/ui/WaypointList.h
@@ -62,6 +62,7 @@ public slots:
     void saveWaypoints();
     void loadWaypoints();    
     void transmit();
+    void read();
     void add();
     void moveUp(Waypoint* wp);
     void moveDown(Waypoint* wp);