diff --git a/src/QGC.h b/src/QGC.h
index b7e25c81d6ffedf22a090bb561d0349d0e0cc89a..a733477d2c9cd2db588ed8fedc14f5f4e6f746e4 100644
--- a/src/QGC.h
+++ b/src/QGC.h
@@ -9,6 +9,7 @@ namespace QGC
     const QColor colorCyan(55, 154, 195);
     const QColor colorRed(154, 20, 20);
     const QColor colorGreen(20, 200, 20);
+    const QColor colorYellow(195, 154, 55);
 
     /** @brief Get the current ground time in microseconds */
     quint64 groundTimeUsecs();
diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index 1f867fe796f1b235a11f473670991dcac05b5a02..6fce11a6f674f1c797c54550e0487ca8d72359a9 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -63,12 +63,13 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
     _isConnected = false;
 
     onboardParams = QMap<QString, float>();
-    onboardParams.insert("ROLL_K_P", 0.5f);
-    onboardParams.insert("PITCH_K_P", 0.5f);
-    onboardParams.insert("YAW_K_P", 0.5f);
-    onboardParams.insert("XY_K_P", 0.5f);
-    onboardParams.insert("ALT_K_P", 0.5f);
-    onboardParams.insert("SYSTEM_TYPE", 1);
+    onboardParams.insert("PID_ROLL_K_P", 0.5f);
+    onboardParams.insert("PID_PITCH_K_P", 0.5f);
+    onboardParams.insert("PID_YAW_K_P", 0.5f);
+    onboardParams.insert("PID_XY_K_P", 0.5f);
+    onboardParams.insert("PID_ALT_K_P", 0.5f);
+    onboardParams.insert("SYS_TYPE", 1);
+    onboardParams.insert("SYS_ID", systemId);
 
     // Comments on the variables can be found in the header file
 
@@ -448,6 +449,22 @@ void MAVLinkSimulationLink::mainloop()
         streampointer += bufferlength;
 
 
+        // Send controller states
+        uint8_t attControl = 1;
+        uint8_t posXYControl = 1;
+        uint8_t posZControl = 0;
+        uint8_t posYawControl = 1;
+
+        uint8_t gpsLock = 2;
+        uint8_t visLock = 3;
+        uint8_t posLock = qMax(gpsLock, visLock);
+
+        messageSize = mavlink_msg_control_status_pack(systemId, componentId, &msg, posLock, visLock, gpsLock, attControl, posXYControl, posZControl, posYawControl);
+        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
+        memcpy(stream+streampointer, buffer, bufferlength);
+        streampointer += bufferlength;
+
+
         /*
         // HEARTBEAT VEHICLE 2
 
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index c758e24fbe47a70725bbbacff437e35b431a6492..cccbca4eff9ebb126ee620b744288b769ef8b7a4 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -89,15 +89,15 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
         gpsFix(0),
         visionFix(0),
         laserFix(0),
-        mavInitialized(false)
+        mavInitialized(false),
+        bottomMargin(3.0f),
+        topMargin(3.0f)
 {
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
     refreshTimer->setInterval(60);
 
-    // FIXME
-    float bottomMargin = 3.0f;
     xCenterPos = vwidth/2.0f;
-    yCenterPos = vheight/2.0f - bottomMargin;
+    yCenterPos = vheight/2.0f + topMargin - bottomMargin;
 }
 
 void HSIDisplay::paintEvent(QPaintEvent * event)
@@ -118,7 +118,7 @@ void HSIDisplay::paintDisplay()
 
     // Size of the ring instrument
     const float margin = 0.1f;  // 10% margin of total width on each side
-    float baseRadius = (vheight - vheight * 2.0f * margin) / 2.0f - bottomMargin / 2.0f;
+    float baseRadius = (vheight - topMargin - bottomMargin) / 2.0f - bottomMargin / 2.0f;
 
     // Draw instruments
     // TESTING THIS SHOULD BE MOVED INTO A QGRAPHICSVIEW
@@ -139,11 +139,12 @@ void HSIDisplay::paintDisplay()
     drawStatusFlag(2,  1, tr("ATT"), attControlEnabled, painter);
     drawStatusFlag(22, 1, tr("PXY"), xyControlEnabled,  painter);
     drawStatusFlag(44, 1, tr("PZ"),  zControlEnabled,   painter);
+    drawStatusFlag(66, 1, tr("YAW"), yawControlEnabled, painter);
 
     // Draw position lock indicators
     drawPositionLock(2,  5, tr("POS"), positionFix, painter);
-    drawPositionLock(22, 5, tr("VIS"), positionFix, painter);
-    drawPositionLock(44, 5, tr("GPS"), positionFix, painter);
+    drawPositionLock(22, 5, tr("VIS"), visionFix, painter);
+    drawPositionLock(44, 5, tr("GPS"), gpsFix, painter);
 
 
     // Draw base instrument
@@ -157,7 +158,7 @@ void HSIDisplay::paintDisplay()
     const int ringCount = 2;
     for (int i = 0; i < ringCount; i++)
     {
-        float radius = (vwidth - vwidth * 2.0f * margin) / (2.0f * i+1) / 2.0f - bottomMargin / 2.0f;
+        float radius = (vwidth - topMargin - bottomMargin) / (2.0f * i+1) / 2.0f - bottomMargin / 2.0f;
         drawCircle(xCenterPos, yCenterPos, radius, 0.1f, ringColor, &painter);
     }
 
@@ -241,11 +242,11 @@ void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, QP
     QColor statusColor(250, 250, 250);
     if(status)
     {
-        painter.setBrush(QGC::colorRed);
+        painter.setBrush(QGC::colorGreen);
     }
     else
     {
-        painter.setBrush(QGC::colorRed);
+        painter.setBrush(QGC::colorYellow);
     }
     painter.setPen(Qt::NoPen);
     painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), refToScreenX(7.0f), refToScreenY(4.0f)));
@@ -292,25 +293,21 @@ void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, Q
 
 void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock)
 {
-    Q_UNUSED(uas);
     positionLock = lock;
 }
 
-void HSIDisplay::updateAttitudeControllerEnabled(UASInterface* uas, bool enabled)
+void HSIDisplay::updateAttitudeControllerEnabled(bool enabled)
 {
-    Q_UNUSED(uas);
     attControlEnabled = enabled;
 }
 
-void HSIDisplay::updatePositionXYControllerEnabled(UASInterface* uas, bool enabled)
+void HSIDisplay::updatePositionXYControllerEnabled(bool enabled)
 {
-    Q_UNUSED(uas);
     xyControlEnabled = enabled;
 }
 
-void HSIDisplay::updatePositionZControllerEnabled(UASInterface* uas, bool enabled)
+void HSIDisplay::updatePositionZControllerEnabled(bool enabled)
 {
-    Q_UNUSED(uas);
     zControlEnabled = enabled;
 }
 
@@ -417,10 +414,10 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
     connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
 
-    connect(uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(UASInterface*,bool)));
-    connect(uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(UASInterface*,bool)));
-    connect(uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(UASInterface*,bool)));
-    connect(uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(UASInterface*,bool)));
+    connect(uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool)));
+    connect(uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(bool)));
+    connect(uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(bool)));
+    connect(uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(bool)));
 
     connect(uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int)));
     connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int)));
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index 28f7654402d57b1be734bc3280d2a1f2dcd8b58e..1f037a392325b18b63d31f632b47a236fc116810 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -59,9 +59,9 @@ public slots:
     void updateGlobalPosition(UASInterface*, double lat, double lon, double alt, quint64 usec);
     void updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time);
     void updatePositionLock(UASInterface* uas, bool lock);
-    void updateAttitudeControllerEnabled(UASInterface* uas, bool enabled);
-    void updatePositionXYControllerEnabled(UASInterface* uas, bool enabled);
-    void updatePositionZControllerEnabled(UASInterface* uas, bool enabled);
+    void updateAttitudeControllerEnabled(bool enabled);
+    void updatePositionXYControllerEnabled(bool enabled);
+    void updatePositionZControllerEnabled(bool enabled);
     /** @brief Heading control enabled/disabled */
     void updatePositionYawControllerEnabled(bool enabled);
 
@@ -221,6 +221,8 @@ protected:
     int visionFix;
     int laserFix;
     bool mavInitialized;      ///< The MAV is initialized once the setpoint has been received
+    float bottomMargin;       ///< Margin on the bottom of the page, in virtual coordinates
+    float topMargin;          ///< Margin on top of the page, in virtual coordinates
 
 private:
 };