Added a few new transformations

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";
             }

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:

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)

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