diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index c9924110ba54113a8897e24fddf265c1e7aefb81..215a4a61366399a8ed3e2e53727f281d17dd4e53 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -49,6 +49,7 @@ This file is part of the PIXHAWK project
 #include "configuration.h"
 #include "LinkManager.h"
 #include <mavlink.h>
+#include "QGC.h"
 
 /**
  * The default constructor will create a new MAVLink object sending heartbeats at
@@ -129,7 +130,9 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
             if (m_loggingEnabled)
             {
                 uint8_t buf[MAVLINK_MAX_PACKET_LEN];
-                mavlink_msg_to_send_buffer(buf, &message);
+                quint64 time = MG::TIME::getGroundTimeNowUsecs();
+                memcpy(buf, (void*)&time, sizeof(quint64));
+                mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message);
                 m_logfile->write((const char*) buf);
                 qDebug() << "WROTE LOGFILE";
             }
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index b31357838f8361a95c2ea73f8544f0e24afd2740..e74501028ebe4ebfa7632e4f5dc4bda891474bc6 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -406,11 +406,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_position_controller_output_t out;
                 mavlink_msg_position_controller_output_decode(&message, &out);
-                quint64 time = MG::TIME::getGroundTimeNowUsecs();
+                quint64 time = MG::TIME::getGroundTimeNow();
                 //emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time);
-                emit valueChanged(uasId, "pos control x", out.x, time/1000.0f);
-                emit valueChanged(uasId, "pos control y", out.y, time/1000.0f);
-                emit valueChanged(uasId, "pos control z", out.z, time/1000.0f);
+                emit valueChanged(uasId, "pos control x", out.x, time);
+                emit valueChanged(uasId, "pos control y", out.y, time);
+                emit valueChanged(uasId, "pos control z", out.z, time);
             }
             break;
         case MAVLINK_MSG_ID_WAYPOINT_COUNT:
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index 9db24bd6c147b7698e12feb02015ce7ba5dc6ae9..ff6adf1872b03772a72e98b4c3e27b61980f755e 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -210,7 +210,7 @@ void HSIDisplay::paintDisplay()
         // Transform from body to world coordinates
         m = metricWorldToBody(m);
         // Scale from metric body to screen reference units
-        QPointF s = metricBodyToRefX(m);
+        QPointF s = metricBodyToRef(m);
         drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::ColorCyan, &painter);
     }
 
@@ -297,11 +297,19 @@ QPointF HSIDisplay::refToMetricBody(QPointF &ref)
 /**
  * @see refToScreenX()
  */
-QPointF HSIDisplay::metricBodyToRefX(QPointF &metric)
+QPointF HSIDisplay::metricBodyToRef(QPointF &metric)
 {
     return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos);
 }
 
+QPointF HSIDisplay::metricBodyToScreen(QPointF metric)
+{
+    QPointF ref = metricBodyToRef(metric);
+    ref.setX(refToScreenX(ref.x()));
+    ref.setY(refToScreenY(ref.y()));
+    return ref;
+}
+
 void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
 {
     static bool dragStarted;
@@ -508,7 +516,7 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color
     // Transform from body to world coordinates
     in = metricWorldToBody(in);
     // Scale from metric to screen reference coordinates
-    QPointF p = metricBodyToRefX(in);
+    QPointF p = metricBodyToRef(in);
     drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
     radius *= 0.8;
     drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
@@ -522,7 +530,7 @@ void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRig
     pen.setWidthF(refLineWidthToPen(0.1f));
     pen.setColor(color);
     painter.setPen(pen);
-    painter.drawRect(QRectF(topLeft, bottomRight));
+    painter.drawRect(QRectF(metricBodyToScreen(metricWorldToBody(topLeft)), metricBodyToScreen(metricWorldToBody(bottomRight))));
 }
 
 void HSIDisplay::drawGPS(QPainter &painter)
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index 50de3f5c99a021e7667ba6fa2d368975ae75356e..20b7ba3ab8d5e268e0ce81317279b6b13e1c5f32 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -98,7 +98,9 @@ protected:
     /** @brief Reference coordinates to metric coordinates */
     QPointF refToMetricBody(QPointF &ref);
     /** @brief Metric coordinates to reference coordinates */
-    QPointF metricBodyToRefX(QPointF &metric);
+    QPointF metricBodyToRef(QPointF &metric);
+    /** @brief Metric body coordinates to screen coordinates */
+    QPointF metricBodyToScreen(QPointF metric);
 
     /**
      * @brief Private data container class to be used within the HSI widget