diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index 7ede06b9d801221d313f1be1d12e8fc8ed7eaef3..34c7963dcf23955367dd40bbb6392fb1da0999a3 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -70,7 +70,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
         localAvailable(0),
         roll(0),
         pitch(0),
-        yaw(0.0f),
+        yaw(1.0f), // FIXME Should be 0
         bodyXSetCoordinate(0.0f),
         bodyYSetCoordinate(0.0f),
         bodyZSetCoordinate(0.0f),
@@ -83,7 +83,8 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
         positionLock(false),
         attControlEnabled(false),
         xyControlEnabled(false),
-        zControlEnabled(false)
+        zControlEnabled(false),
+        mavInitialized(false)
 {
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
     refreshTimer->setInterval(60);
@@ -272,7 +273,7 @@ QPointF HSIDisplay::metricWorldToBody(QPointF world)
 {
     // First translate to body-centered coordinates
     // Rotate around -yaw
-    QPointF result(cos(yaw) * (world.x() - x) + -sin(yaw) * (world.x() - x), sin(yaw) * (world.y() - y) + cos(yaw) * (world.y() - y));
+    QPointF result(cos(yaw) * (x - world.x()) + -sin(yaw) * (x - world.x()), sin(yaw) * (y - world.y()) + cos(yaw) * (y - world.y()));
     return result;
 }
 
@@ -400,7 +401,9 @@ void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
     uiXSetCoordinate = sp.x();
     uiYSetCoordinate = sp.y();
 
-    if (uas)
+    qDebug() << "Attempting to set new setpoint at x: " << x << "metric y:" << y;
+
+    if (uas && mavInitialized)
     {
         uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
         qDebug() << "Setting new setpoint at x: " << x << "metric y:" << y;
@@ -445,6 +448,9 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir
     bodyYSetCoordinate = yDesired;
     bodyZSetCoordinate = zDesired;
     bodyYawSet = yawDesired;
+    mavInitialized = true;
+
+    qDebug() << "Received setpoint at x: " << x << "metric y:" << y;
 //    posXSet = xDesired;
 //    posYSet = yDesired;
 //    posZSet = zDesired;
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index 20b7ba3ab8d5e268e0ce81317279b6b13e1c5f32..18e7a89523b10b0eb42ec49052901aa274a3d05e 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -195,6 +195,7 @@ protected:
     bool attControlEnabled;
     bool xyControlEnabled;
     bool zControlEnabled;
+    bool mavInitialized;      ///< The MAV is initialized once the setpoint has been received
 
 private:
 };
diff --git a/src/ui/watchdog/WatchdogProcessView.cc b/src/ui/watchdog/WatchdogProcessView.cc
index 37d22f407ca00290f6879683de7f430eb6a1570f..1a65ac642bde646e45327866c88118c54cf81118 100644
--- a/src/ui/watchdog/WatchdogProcessView.cc
+++ b/src/ui/watchdog/WatchdogProcessView.cc
@@ -1,7 +1,7 @@
 #include "WatchdogProcessView.h"
 #include "ui_WatchdogProcessView.h"
 
-WatchdogProcessView::WatchdogProcessView(QWidget *parent) :
+WatchdogProcessView::WatchdogProcessView(int processid, QWidget *parent) :
     QWidget(parent),
     m_ui(new Ui::WatchdogProcessView)
 {