Implemented setpoint changes for HSI widget

images/style-mission.css

@@ -107,7 +107,7 @@ QSpinBox {
 QPushButton {
 	font-weight: bold;
 	min-height: 18px;
-	max-height: 32px;
+	max-height: 18px;
 	border: 2px solid #4A4A4F;
 	border-radius: 5px;
 	padding-left: 10px;
@@ -127,10 +127,10 @@ QToolButton {
 	font-weight: bold;
 	min-height: 16px;
 	min-width: 24px;
-	max-height: 32px;
-	border: 1px solid #EEEEEE;
+	max-height: 18px;
+	border: 2px solid #4A4A4F;
 	border-radius: 5px;
-	background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #303030, stop: 1 #202020);
+	background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #232228, stop: 1 #020208);
 }
 
 QToolButton:checked {

qgroundcontrol.pro

@@ -3,23 +3,23 @@
 # from http://github.com/pixhawk/qmapcontrol/
 # over bundled version in lib directory
 # Version from GIT repository is preferred
-#include ( "../qmapcontrol/QMapControl/QMapControl.pri" ) #{
+# include ( "../qmapcontrol/QMapControl/QMapControl.pri" ) #{
 # Include bundled version if necessary
 include(lib/QMapControl/QMapControl.pri)
-#message("Including bundled QMapControl version as FALLBACK. This is fine on Linux and MacOS, but not the best choice in Windows")
-
-
-QT       += network opengl svg xml phonon
 
+# message("Including bundled QMapControl version as FALLBACK. This is fine on Linux and MacOS, but not the best choice in Windows")
+QT += network \
+    opengl \
+    svg \
+    xml \
+    phonon
 TEMPLATE = app
 TARGET = qgroundcontrol
-
 BASEDIR = .
 BUILDDIR = build
 LANGUAGE = C++
-
-CONFIG += debug_and_release console
-
+CONFIG += debug_and_release \
+    console
 OBJECTS_DIR = $$BUILDDIR/obj
 MOC_DIR = $$BUILDDIR/moc
 UI_HEADERS_DIR = src/ui/generated
@@ -142,7 +142,8 @@ HEADERS += src/MG.h \
     src/ui/watchdog/WatchdogProcessView.h \
     src/ui/watchdog/WatchdogView.h \
     src/uas/UASWaypointManager.h \
-    src/ui/HSIDisplay.h
+    src/ui/HSIDisplay.h \
+    src/QGC.h
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \

src/QGC.h

+#ifndef QGC_H
+#define QGC_H
+
+#include <QColor>
+
+namespace QGC
+{
+    const QColor ColorCyan(55, 154, 195);
+}
+
+#endif // QGC_H

src/ui/DebugConsole.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>488</width>
-    <height>390</height>
+    <width>435</width>
+    <height>185</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -24,16 +24,15 @@
     <number>6</number>
    </property>
    <item row="0" column="0" colspan="2">
-    <layout class="QHBoxLayout" name="horizontalLayout_2" stretch="0,10,0,0,0,0">
-     <item>
-      <widget class="QLabel" name="label">
-       <property name="text">
-        <string>Link</string>
-       </property>
-      </widget>
-     </item>
+    <layout class="QHBoxLayout" name="horizontalLayout_2" stretch="10,0,0,0,0">
      <item>
       <widget class="QComboBox" name="linkComboBox">
+       <property name="maximumSize">
+        <size>
+         <width>130</width>
+         <height>16777215</height>
+        </size>
+       </property>
        <property name="toolTip">
         <string>Select the link to monitor</string>
        </property>
@@ -52,7 +51,7 @@
         <string>Ignore MAVLINK protocol messages in display</string>
        </property>
        <property name="text">
-        <string>Ignore MAVLINK</string>
+        <string>No MAVLINK</string>
        </property>
       </widget>
      </item>

src/ui/HDDisplay.cc

@@ -581,6 +581,16 @@ float HDDisplay::refToScreenY(float y)
     return (scalingFactor * y);
 }
 
+float HDDisplay::screenToRefX(float x)
+{
+    return x/scalingFactor;
+}
+
+float HDDisplay::screenToRefY(float y)
+{
+    return y/scalingFactor;
+}
+
 void HDDisplay::drawLine(float refX1, float refY1, float refX2, float refY2, float width, const QColor& color, QPainter* painter)
 {
     QPen pen(Qt::SolidLine);

src/ui/HDDisplay.h

@@ -70,6 +70,8 @@ protected:
     float refLineWidthToPen(float line);
     float refToScreenX(float x);
     float refToScreenY(float y);
+    float screenToRefX(float x);
+    float screenToRefY(float y);
     void rotatePolygonClockWiseRad(QPolygonF& p, float angle, QPointF origin);
     void drawPolygon(QPolygonF refPolygon, QPainter* painter);
     void drawLine(float refX1, float refY1, float refX2, float refY2, float width, const QColor& color, QPainter* painter);

src/ui/HSIDisplay.cc

@@ -35,6 +35,7 @@ This file is part of the PIXHAWK project
 #include "UASManager.h"
 #include "HSIDisplay.h"
 #include "MG.h"
+#include "QGC.h"
 
 #include <QDebug>
 
@@ -62,11 +63,31 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
         x(0),
         y(0),
         z(0),
-        //yaw(0),
-        localAvailable(0)
+        vx(0),
+        vy(0),
+        vz(0),
+        speed(0),
+        localAvailable(0),
+        roll(0),
+        pitch(0),
+        yaw(0.0f),
+        bodyXSetCoordinate(0.0f),
+        bodyYSetCoordinate(0.0f),
+        bodyZSetCoordinate(0.0f),
+        bodyYawSet(0.0f),
+        uiXSetCoordinate(0.0f),
+        uiYSetCoordinate(0.0f),
+        uiZSetCoordinate(0.0f),
+        uiYawSet(0.0f),
+        metricWidth(2.0f)
 {
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
     refreshTimer->setInterval(80);
+
+    // FIXME
+    float bottomMargin = 3.0f;
+    xCenterPos = vwidth/2.0f;
+    yCenterPos = vheight/2.0f - bottomMargin;
 }
 
 void HSIDisplay::paintEvent(QPaintEvent * event)
@@ -83,21 +104,12 @@ void HSIDisplay::paintDisplay()
 {
     // Center location of the HSI gauge items
 
-    float xCenterPos = vwidth/2.0f;
-    float yCenterPos = vheight/2.0f;
+    float bottomMargin = 3.0f;
 
     // Size of the ring instrument
     const float margin = 0.1f;  // 10% margin of total width on each side
-    float baseRadius = (vwidth - vwidth * 2.0f * margin) / 2.0f;
-
-//    quint64 refreshInterval = 100;
-//    quint64 currTime = MG::TIME::getGroundTimeNow();
-//    if (currTime - lastPaintTime < refreshInterval)
-//    {
-//        // FIXME Need to find the source of the spurious paint events
-//        //return;
-//    }
-//    lastPaintTime = currTime;
+    float baseRadius = (vheight - vheight * 2.0f * margin) / 2.0f - bottomMargin / 2.0f;
+
     // Draw instruments
     // TESTING THIS SHOULD BE MOVED INTO A QGRAPHICSVIEW
     // Update scaling factor
@@ -109,75 +121,133 @@ void HSIDisplay::paintDisplay()
     QPainter painter(this);
     painter.setRenderHint(QPainter::Antialiasing, true);
     painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
+
+    // Draw background
     painter.fillRect(QRect(0, 0, width(), height()), backgroundColor);
-    const QColor ringColor = QColor(200, 250, 200);
 
     // Draw base instrument
     // ----------------------
+    const QColor ringColor = QColor(200, 250, 200);
     const int ringCount = 2;
     for (int i = 0; i < ringCount; i++)
     {
-        float radius = (vwidth - vwidth * 2.0f * margin) / (2.0f * i+1) / 2.0f;
-        drawCircle(vwidth/2.0f, vheight/2.0f, radius, 0.1f, ringColor, &painter);
+        float radius = (vwidth - vwidth * 2.0f * margin) / (2.0f * i+1) / 2.0f - bottomMargin / 2.0f;
+        drawCircle(xCenterPos, yCenterPos, radius, 0.1f, ringColor, &painter);
     }
 
     // Draw center indicator
-    drawCircle(vwidth/2.0f, vheight/2.0f, 1.0f, 0.1f, ringColor, &painter);
-
-    // Draw orientation labels
-    paintText(tr("N"), ringColor, 3.5f, xCenterPos - 1.0f, yCenterPos - baseRadius - 5.5f, &painter);
-    paintText(tr("S"), ringColor, 3.5f, xCenterPos - 1.0f, yCenterPos + baseRadius + 1.5f, &painter);
-    paintText(tr("E"), ringColor, 3.5f, xCenterPos + baseRadius + 2.0f, yCenterPos - 1.75f, &painter);
-    paintText(tr("W"), ringColor, 3.5f, xCenterPos - baseRadius - 5.5f, yCenterPos - 1.75f, &painter);
+    QPolygonF p(3);
+    p.replace(0, QPointF(xCenterPos, yCenterPos-2.8484f));
+    p.replace(1, QPointF(xCenterPos-2.0f, yCenterPos+2.0f));
+    p.replace(2, QPointF(xCenterPos+2.0f, yCenterPos+2.0f));
+    drawPolygon(p, &painter);
 
     // ----------------------
 
+    // Draw satellites
+    drawGPS(painter);
+
     // Draw state indicator
 
     // Draw position
     QColor positionColor(20, 20, 200);
-    drawPositionSetpoint(xCenterPos, yCenterPos, baseRadius, positionColor, &painter);
+    drawPositionDirection(xCenterPos, yCenterPos, baseRadius, positionColor, &painter);
 
     // Draw attitude
     QColor attitudeColor(200, 20, 20);
-    drawAttitudeSetpoint(xCenterPos, yCenterPos, baseRadius, attitudeColor, &painter);
+    drawAttitudeDirection(xCenterPos, yCenterPos, baseRadius, attitudeColor, &painter);
 
 
+    // Draw position setpoints in body coordinates
 
-    // Draw satellites
-    drawGPS();
+    if (uiXSetCoordinate != 0 || uiYSetCoordinate != 0)
+    {
+        QColor spColor(150, 150, 150);
+        drawSetpointXY(uiXSetCoordinate, uiYSetCoordinate, uiYawSet, spColor, painter);
+    }
+
+    if (bodyXSetCoordinate != 0 || bodyYSetCoordinate != 0)
+    {
+        // Draw setpoint
+        drawSetpointXY(bodyXSetCoordinate, bodyYSetCoordinate, bodyYawSet, QGC::ColorCyan, painter);
+        // Draw travel direction line
+        QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
+        QPointF s = metricBodyToRefX(m);
+        drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::ColorCyan, &painter);
+    }
+
+    // Labels on outer part and bottom
 
-    if (localAvailable > 0)
+    //if (localAvailable > 0)
     {
+        // Position
         QString str;
         str.sprintf("%05.2f %05.2f %05.2f m", x, y, z);
-        paintText(str, ringColor, 4.5f, xCenterPos + baseRadius - 5.5f, yCenterPos + baseRadius - 20.75f, &painter);
+        paintText(str, ringColor, 3.0f, xCenterPos + baseRadius - 30.75f, vheight - 5.0f, &painter);
+
+        // Speed
+        str.sprintf("%05.2f m/s", speed);
+        paintText(str, ringColor, 3.0f, xCenterPos, vheight - 5.0f, &painter);
     }
 
+    // Draw orientation labels
+    // Translate and rotate coordinate frame
+    painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
+    painter.rotate(yaw);
+    paintText(tr("N"), ringColor, 3.5f, - 1.0f, - baseRadius - 5.5f, &painter);
+    paintText(tr("S"), ringColor, 3.5f, - 1.0f, + baseRadius + 1.5f, &painter);
+    paintText(tr("E"), ringColor, 3.5f, + baseRadius + 2.0f, - 1.75f, &painter);
+    paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter);
+
+    // FIXME Just for testing
+    bodyXSetCoordinate = 0.95 * bodyXSetCoordinate + 0.05 * uiXSetCoordinate;
+    bodyYSetCoordinate = 0.95 * bodyYSetCoordinate + 0.05 * uiYSetCoordinate;
+    bodyZSetCoordinate = 0.95 * bodyZSetCoordinate + 0.05 * uiZSetCoordinate;
+    bodyYawSet = 0.95 * bodyYawSet + 0.05 * uiYawSet;
+}
 
-    //drawSystemIndicator(10.0f-gaugeWidth/2.0f, 20.0f, 10.0f, 40.0f, 15.0f, &painter);
-    //drawGauge(15.0f, 15.0f, gaugeWidth/2.0f, 0, 1.0f, "thrust", values.value("thrust", 0.0f), gaugeColor, &painter, qMakePair(0.45f, 0.8f), qMakePair(0.8f, 1.0f), true);
-    //drawGauge(15.0f+gaugeWidth*1.7f, 15.0f, gaugeWidth/2.0f, 0, 10.0f, "altitude", values.value("altitude", 0.0f), gaugeColor, &painter, qMakePair(1.0f, 2.5f), qMakePair(0.0f, 0.5f), true);
+QPointF HSIDisplay::screenToMetricBody(QPointF ref)
+{
+    return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth);
+}
 
-    // Left spacing from border / other gauges, measured from left edge to center
-    //    float leftSpacing = gaugeWidth * spacing;
-    //    float xCoord = leftSpacing + gaugeWidth/2.0f;
-    //
-    //    float topSpacing = leftSpacing;
-    //    float yCoord = topSpacing + gaugeWidth/2.0f;
-    //
-    //    for (int i = 0; i < acceptList->size(); ++i)
-    //    {
-    //        QString value = acceptList->at(i);
-    //        drawGauge(xCoord, yCoord, gaugeWidth/2.0f, minValues.value(value, -1.0f), maxValues.value(value, 1.0f), value, values.value(value, minValues.value(value, 0.0f)), gaugeColor, &painter, goodRanges.value(value, qMakePair(0.0f, 0.5f)), critRanges.value(value, qMakePair(0.7f, 1.0f)), true);
-    //        xCoord += gaugeWidth + leftSpacing;
-    //        // Move one row down if necessary
-    //        if (xCoord + gaugeWidth > vwidth)
-    //        {
-    //            yCoord += topSpacing + gaugeWidth;
-    //            xCoord = leftSpacing + gaugeWidth/2.0f;
-    //        }
-    //    }
+QPointF HSIDisplay::refToMetricBody(QPointF &ref)
+{
+    return QPointF(-((ref.y() - yCenterPos)/ vwidth) * metricWidth, ((ref.x() - xCenterPos) / vwidth) * metricWidth);
+}
+
+/**
+ * @see refToScreenX()
+ */
+QPointF HSIDisplay::metricBodyToRefX(QPointF &metric)
+{
+    return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos);
+}
+
+void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
+{
+    static bool dragStarted;
+    static float startX;
+
+    if (event->MouseButtonDblClick)
+    {
+        //setBodySetpointCoordinateXY(-refToMetric(screenToRefY(event->y()) - yCenterPos), refToMetric(screenToRefX(event->x()) - xCenterPos));
+
+        QPointF p = screenToMetricBody(event->posF());
+        setBodySetpointCoordinateXY(p.x(), p.y());
+        qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos;
+    }
+    else if (event->MouseButtonPress)
+    {
+        if (event->button() == Qt::RightButton)
+        {
+
+        }
+        else if (event->button() == Qt::LeftButton)
+        {
+
+        }
+    }
 }
 
 /**
@@ -186,19 +256,23 @@ void HSIDisplay::paintDisplay()
  */
 void HSIDisplay::setActiveUAS(UASInterface* uas)
 {
-    HDDisplay::setActiveUAS(uas);
-    //qDebug() << "ATTEMPTING TO SET UAS";
     if (this->uas != NULL && this->uas != uas)
     {
         // Disconnect any previously connected active MAV
         //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
     }
 
+
+    HDDisplay::setActiveUAS(uas);
+    //qDebug() << "ATTEMPTING TO SET UAS";
+
+
     connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
     connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
     connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
+    connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
 
     // Now connect the new UAS
 
@@ -210,6 +284,36 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
     //}
 }
 
+void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time)
+{
+    Q_UNUSED(uas);
+    Q_UNUSED(time);
+    this->vx = vx;
+    this->vy = vy;
+    this->vz = vz;
+    this->speed = sqrt(pow(vx, 2.0f) + pow(vy, 2.0f) + pow(vz, 2.0f));
+}
+
+void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
+{
+    // Set coordinates and send them out to MAV
+
+    qDebug() << "Setting new setpoint at x: " << x << "metric y:" << y;
+    uiXSetCoordinate = x;
+    uiYSetCoordinate = y;
+}
+
+void HSIDisplay::setBodySetpointCoordinateZ(double z)
+{
+    // Set coordinates and send them out to MAV
+    uiZSetCoordinate = z;
+}
+
+void HSIDisplay::sendBodySetPointCoordinates()
+{
+    // Send the coordinates to the MAV
+}
+
 void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec)
 {
     Q_UNUSED(uas);
@@ -287,14 +391,36 @@ QColor HSIDisplay::getColorForSNR(float snr)
     return color;
 }
 
-void HSIDisplay::drawGPS()
+void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color, QPainter &painter)
 {
-    float xCenter = vwidth/2.0f;
-    float yCenter = vwidth/2.0f;
-    QPainter painter(this);
-    painter.setRenderHint(QPainter::Antialiasing, true);
-    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
+    float radius = vwidth / 20.0f;
+    QPen pen(color);
+    pen.setWidthF(refLineWidthToPen(0.4f));
+    pen.setColor(color);
+    painter.setPen(pen);
+    painter.setBrush(Qt::NoBrush);
+    QPointF in(x, y);
+    QPointF p = metricBodyToRefX(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);
+    painter.setBrush(color);
+    drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter);
+}
+
+void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter)
+{
+    QPen pen(color);
+    pen.setWidthF(refLineWidthToPen(0.1f));
+    pen.setColor(color);
+    painter.setPen(pen);
+    painter.drawRect(QRectF(topLeft, bottomRight));
+}
 
+void HSIDisplay::drawGPS(QPainter &painter)
+{
+    float xCenter = xCenterPos;
+    float yCenter = xCenterPos;
     // Max satellite circle radius
 
     const float margin = 0.15f;  // 20% margin of total width on each side
@@ -349,18 +475,20 @@ void HSIDisplay::drawGPS()
             float xPos = xCenter + (sin(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
             float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
 
+            // Draw circle for satellite, filled for used satellites
             drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter);
+            // Draw satellite PRN
             paintText(QString::number(sat->id), QColor(255, 255, 255), 2.9f, xPos+1.7f, yPos+2.0f, &painter);
         }
     }
 }
 
-void HSIDisplay::drawObjects()
+void HSIDisplay::drawObjects(QPainter &painter)
 {
-
+    Q_UNUSED(painter);
 }
 
-void HSIDisplay::drawPositionSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
+void HSIDisplay::drawPositionDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
 {
     // Draw the needle
     const float maxWidth = radius / 10.0f;
@@ -392,10 +520,10 @@ void HSIDisplay::drawPositionSetpoint(float xRef, float yRef, float radius, cons
     painter->setBrush(indexBrush);
     drawPolygon(p, painter);
 
-    qDebug() << "DRAWING POS SETPOINT X:" << posXSet << "Y:" << posYSet << angle;
+    //qDebug() << "DRAWING POS SETPOINT X:" << posXSet << "Y:" << posYSet << angle;
 }
 
-void HSIDisplay::drawAttitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
+void HSIDisplay::drawAttitudeDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
 {
     // Draw the needle
     const float maxWidth = radius / 10.0f;
@@ -427,7 +555,7 @@ void HSIDisplay::drawAttitudeSetpoint(float xRef, float yRef, float radius, cons
 
     // TODO Draw Yaw indicator
 
-    qDebug() << "DRAWING ATT SETPOINT X:" << attXSet << "Y:" << attYSet << angle;
+    //qDebug() << "DRAWING ATT SETPOINT X:" << attXSet << "Y:" << attYSet << angle;
 }
 
 void HSIDisplay::drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
@@ -446,3 +574,13 @@ void HSIDisplay::drawAltitudeSetpoint(float xRef, float yRef, float radius, cons
     //    label.sprintf("%05.1f", value);
     //    paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter);
 }
+
+void HSIDisplay::updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat)
+{
+
+}
+
+void HSIDisplay::pressKey(int key)
+{
+
+}

src/ui/HSIDisplay.h

@@ -37,6 +37,7 @@ This file is part of the PIXHAWK project
 #include <QTimer>
 #include <QMap>
 #include <QPair>
+#include <QMouseEvent>
 #include <cmath>
 
 #include "HDDisplay.h"
@@ -55,19 +56,38 @@ public slots:
     void updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
     void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec);
     void updateGlobalPosition(UASInterface*, double lat, double lon, double alt, quint64 usec);
+    void updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time);
     void paintEvent(QPaintEvent * event);
+    /** @brief Update state from joystick */
+    void updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat);
+    void pressKey(int key);
 
 protected slots:
     void paintDisplay();
-    void drawGPS();
-    void drawObjects();
-    void drawPositionSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter);
-    void drawAttitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter);
+    void drawGPS(QPainter &painter);
+    void drawObjects(QPainter &painter);
+    void drawPositionDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter);
+    void drawAttitudeDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter);
     void drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter);
+    void setBodySetpointCoordinateXY(double x, double y);
+    void setBodySetpointCoordinateZ(double z);
+    /** @brief Send the current ui setpoint coordinates as new setpoint to the MAV */
+    void sendBodySetPointCoordinates();
+    /** @brief Draw one setpoint */
+    void drawSetpointXY(float x, float y, float yaw, const QColor &color, QPainter &painter);
+    /** @brief Draw the limiting safety area */
+    void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight,  const QColor &color, QPainter &painter);
 
+    void mouseDoubleClickEvent(QMouseEvent* event);
 
 protected:
     static QColor getColorForSNR(float snr);
+    /** @brief Screen coordinates of widget to metric coordinates in body frame */
+    QPointF screenToMetricBody(QPointF ref);
+    /** @brief Reference coordinates to metric coordinates */
+    QPointF refToMetricBody(QPointF &ref);
+    /** @brief Metric coordinates to reference coordinates */
+    QPointF metricBodyToRefX(QPointF &metric);
 
     /**
      * @brief Private data container class to be used within the HSI widget
@@ -136,7 +156,27 @@ protected:
     float x;
     float y;
     float z;
+    float vx;
+    float vy;
+    float vz;
+    float speed;
     quint64 localAvailable;    ///< Last local position update time
+    float roll;
+    float pitch;
+    float yaw;
+    float bodyXSetCoordinate;  ///< X Setpoint coordinate active on the MAV
+    float bodyYSetCoordinate;  ///< Y Setpoint coordinate active on the MAV
+    float bodyZSetCoordinate;  ///< Z Setpoint coordinate active on the MAV
+    float bodyYawSet;          ///< Yaw setpoint coordinate active on the MAV
+    float uiXSetCoordinate;    ///< X Setpoint coordinate wanted by the UI
+    float uiYSetCoordinate;    ///< Y Setpoint coordinate wanted by the UI
+    float uiZSetCoordinate;    ///< Z Setpoint coordinate wanted by the UI
+    float uiYawSet;            ///< Yaw Setpoint wanted by the UI
+    float metricWidth;         ///< Width the instrument represents in meters (the width of the ground shown by the widget)
+
+    //
+    float xCenterPos;
+    float yCenterPos;
 
 private:
 };

src/ui/MainWindow.cc

@@ -490,10 +490,11 @@ void MainWindow::loadEngineerView()
     container3->setWidget(info);
     addDockWidget(Qt::LeftDockWidgetArea, container3);
 
-    // WAYPOINT LIST
-    QDockWidget* container5 = new QDockWidget(tr("Waypoint List"), this);
-    container5->setWidget(waypoints);
-    addDockWidget(Qt::BottomDockWidgetArea, container5);
+    // HORIZONTAL SITUATION INDICATOR
+    QDockWidget* container6 = new QDockWidget(tr("Horizontal Situation Indicator"), this);
+    container6->setWidget(hsi);
+    hsi->start();
+    addDockWidget(Qt::BottomDockWidgetArea, container6);
 
     // DEBUG CONSOLE
     QDockWidget* container7 = new QDockWidget(tr("Communication Console"), this);

src/ui/UASControl.ui

@@ -6,21 +6,27 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>387</width>
-    <height>212</height>
+    <width>380</width>
+    <height>190</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout" rowstretch="0,0,0,0,0,20,0" columnstretch="0,0,0,0,20">
-   <property name="margin">
-    <number>20</number>
+  <layout class="QGridLayout" name="gridLayout">
+   <property name="leftMargin">
+    <number>6</number>
+   </property>
+   <property name="topMargin">
+    <number>12</number>
    </property>
-   <property name="spacing">
+   <property name="rightMargin">
     <number>6</number>
    </property>
-   <item row="0" column="0" colspan="4">
+   <property name="bottomMargin">
+    <number>6</number>
+   </property>
+   <item row="0" column="1" colspan="4">
     <widget class="QLabel" name="controlStatusLabel">
      <property name="text">
       <string>UNCONNECTED</string>
@@ -30,27 +36,14 @@
      </property>
     </widget>
    </item>
-   <item row="0" column="4" rowspan="7">
-    <spacer name="horizontalSpacer">
-     <property name="orientation">
-      <enum>Qt::Horizontal</enum>
-     </property>
-     <property name="sizeHint" stdset="0">
-      <size>
-       <width>0</width>
-       <height>172</height>
-      </size>
-     </property>
-    </spacer>
-   </item>
-   <item row="1" column="0" colspan="4">
+   <item row="1" column="1" colspan="4">
     <widget class="QPushButton" name="controlButton">
      <property name="text">
       <string>Activate Engine</string>
      </property>
     </widget>
    </item>
-   <item row="2" column="0" colspan="4">
+   <item row="2" column="1" colspan="4">
     <spacer name="verticalSpacer_2">
      <property name="orientation">
       <enum>Qt::Vertical</enum>
@@ -60,13 +53,26 @@
      </property>
      <property name="sizeHint" stdset="0">
       <size>
-       <width>218</width>
-       <height>6</height>
+       <width>20</width>
+       <height>0</height>
       </size>
      </property>
     </spacer>
    </item>
-   <item row="3" column="0">
+   <item row="0" column="0" rowspan="6">
+    <spacer name="horizontalSpacer">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>31</width>
+       <height>159</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
+   <item row="3" column="1">
     <widget class="QPushButton" name="liftoffButton">
      <property name="text">
       <string>Liftoff</string>
@@ -77,56 +83,56 @@
      </property>
     </widget>
    </item>
-   <item row="4" column="0" colspan="3">
-    <widget class="QComboBox" name="modeComboBox"/>
-   </item>
-   <item row="3" column="3">
-    <widget class="QPushButton" name="shutdownButton">
+   <item row="3" column="2" colspan="2">
+    <widget class="QPushButton" name="landButton">
      <property name="text">
-      <string>Halt</string>
+      <string>Land</string>
      </property>
      <property name="icon">
       <iconset resource="../../mavground.qrc">
-       <normaloff>:/images/actions/system-log-out.svg</normaloff>:/images/actions/system-log-out.svg</iconset>
+       <normaloff>:/images/control/land.svg</normaloff>:/images/control/land.svg</iconset>
      </property>
     </widget>
    </item>
-   <item row="4" column="3">
-    <widget class="QPushButton" name="setModeButton">
+   <item row="3" column="4">
+    <widget class="QPushButton" name="shutdownButton">
      <property name="text">
-      <string>Set Mode</string>
+      <string>Halt</string>
      </property>
      <property name="icon">
       <iconset resource="../../mavground.qrc">
-       <normaloff>:/images/devices/network-wireless.svg</normaloff>:/images/devices/network-wireless.svg</iconset>
+       <normaloff>:/images/actions/system-log-out.svg</normaloff>:/images/actions/system-log-out.svg</iconset>
      </property>
     </widget>
    </item>
-   <item row="6" column="0" colspan="4">
-    <spacer name="verticalSpacer">
+   <item row="0" column="5" rowspan="6">
+    <spacer name="horizontalSpacer_2">
      <property name="orientation">
-      <enum>Qt::Vertical</enum>
+      <enum>Qt::Horizontal</enum>
      </property>
      <property name="sizeHint" stdset="0">
       <size>
-       <width>20</width>
-       <height>0</height>
+       <width>30</width>
+       <height>159</height>
       </size>
      </property>
     </spacer>
    </item>
-   <item row="3" column="1" colspan="2">
-    <widget class="QPushButton" name="landButton">
+   <item row="4" column="1" colspan="2">
+    <widget class="QComboBox" name="modeComboBox"/>
+   </item>
+   <item row="4" column="3" colspan="2">
+    <widget class="QPushButton" name="setModeButton">
      <property name="text">
-      <string>Land</string>
+      <string>Set Mode</string>
      </property>
      <property name="icon">
       <iconset resource="../../mavground.qrc">
-       <normaloff>:/images/control/land.svg</normaloff>:/images/control/land.svg</iconset>
+       <normaloff>:/images/devices/network-wireless.svg</normaloff>:/images/devices/network-wireless.svg</iconset>
      </property>
     </widget>
    </item>
-   <item row="5" column="0" colspan="4">
+   <item row="5" column="1" colspan="4">
     <widget class="QLabel" name="lastActionLabel">
      <property name="text">
       <string>No actions executed so far</string>
@@ -136,6 +142,19 @@
      </property>
     </widget>
    </item>
+   <item row="6" column="0" colspan="6">
+    <spacer name="verticalSpacer">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>20</width>
+       <height>0</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
   </layout>
  </widget>
  <resources>

src/ui/UASInfo.ui

@@ -17,6 +17,9 @@
    <string notr="true"/>
   </property>
   <layout class="QGridLayout" name="gridLayout">
+   <property name="margin">
+    <number>6</number>
+   </property>
    <item row="0" column="0">
     <widget class="QLabel" name="label">
      <property name="text">
@@ -109,19 +112,6 @@
      </property>
     </widget>
    </item>
-   <item row="0" column="6" rowspan="6">
-    <spacer name="horizontalSpacer">
-     <property name="orientation">
-      <enum>Qt::Horizontal</enum>
-     </property>
-     <property name="sizeHint" stdset="0">
-      <size>
-       <width>40</width>
-       <height>108</height>
-      </size>
-     </property>
-    </spacer>
-   </item>
    <item row="1" column="0">
     <widget class="QLabel" name="label_2">
      <property name="text">
@@ -398,7 +388,7 @@
      <property name="sizeHint" stdset="0">
       <size>
        <width>0</width>
-       <height>197</height>
+       <height>0</height>
       </size>
      </property>
     </spacer>

src/ui/UASList.ui

@@ -10,6 +10,12 @@
     <height>300</height>
    </rect>
   </property>
+  <property name="minimumSize">
+   <size>
+    <width>500</width>
+    <height>0</height>
+   </size>
+  </property>
   <property name="windowTitle">
    <string>Form</string>
   </property>

src/ui/UASView.ui

@@ -26,7 +26,7 @@
    <string>Form</string>
   </property>
   <property name="styleSheet">
-   <string>QWidget#colorIcon {}
+   <string notr="true">QWidget#colorIcon {}
 
 QWidget {
 background-color: none;
@@ -95,19 +95,6 @@ QGroupBox#heartbeatIcon {
 	background-color: red;
 }
 
-QToolButton {
-	font-weight: bold;
-	font-size: 12px;
-	border: 1px solid #999999;
-	border-radius: 5px;
-	min-width:44px;
-	max-width: 44px;
-	min-height: 44px;
-	max-height: 44px;
-	padding: 0px;
-	background-color: none;
-}
-
 QToolButton#typeButton {
 	font-weight: bold;
 	font-size: 12px;
@@ -126,7 +113,7 @@ QPushButton {
 	font-size: 12px;
 	border: 1px solid #999999;
 	border-radius: 10px;
-	min-width:12px;
+	min-width: 20px;
 	max-width: 40px;
 	min-height: 16px;
 	max-height: 16px;
@@ -203,14 +190,14 @@ QProgressBar::chunk#thrustBar {
    <item>
     <widget class="QGroupBox" name="uasViewFrame">
      <property name="sizePolicy">
-      <sizepolicy hsizetype="Maximum" vsizetype="Fixed">
+      <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
        <horstretch>0</horstretch>
        <verstretch>0</verstretch>
       </sizepolicy>
      </property>
      <property name="minimumSize">
       <size>
-       <width>350</width>
+       <width>0</width>
        <height>0</height>
       </size>
      </property>
@@ -558,7 +545,7 @@ QProgressBar::chunk#thrustBar {
          <widget class="QPushButton" name="liftoffButton">
           <property name="minimumSize">
            <size>
-            <width>18</width>
+            <width>26</width>
             <height>22</height>
            </size>
           </property>
@@ -575,7 +562,7 @@ QProgressBar::chunk#thrustBar {
          <widget class="QPushButton" name="haltButton">
           <property name="minimumSize">
            <size>
-            <width>18</width>
+            <width>26</width>
             <height>22</height>
            </size>
           </property>
@@ -592,7 +579,7 @@ QProgressBar::chunk#thrustBar {
          <widget class="QPushButton" name="continueButton">
           <property name="minimumSize">
            <size>
-            <width>18</width>
+            <width>26</width>
             <height>22</height>
            </size>
           </property>
@@ -609,7 +596,7 @@ QProgressBar::chunk#thrustBar {
          <widget class="QPushButton" name="landButton">
           <property name="minimumSize">
            <size>
-            <width>18</width>
+            <width>26</width>
             <height>22</height>
            </size>
           </property>
@@ -637,7 +624,7 @@ QProgressBar::chunk#thrustBar {
          <widget class="QPushButton" name="abortButton">
           <property name="minimumSize">
            <size>
-            <width>18</width>
+            <width>26</width>
             <height>22</height>
            </size>
           </property>
@@ -654,7 +641,7 @@ QProgressBar::chunk#thrustBar {
          <widget class="QPushButton" name="killButton">
           <property name="minimumSize">
            <size>
-            <width>18</width>
+            <width>26</width>
             <height>22</height>
            </size>
           </property>

src/ui/XMLCommProtocolWidget.cc

@@ -47,17 +47,18 @@ void XMLCommProtocolWidget::selectXMLFile()
     {
         m_ui->fileNameLabel->setText(fileNames.first());
         QFile file(fileNames.first());
-        // Store filename for next time
-        settings.setValue(mavlinkXML, fileNames.first());
+
         if (file.open(QIODevice::ReadOnly | QIODevice::Text))
         {
             const QString instanceText(QString::fromUtf8(file.readAll()));
             setXML(instanceText);
+            // Store filename for next time
+            settings.setValue(mavlinkXML, QFileInfo(file).absoluteFilePath());
         }
         else
         {
             QMessageBox msgBox;
-            msgBox.setText("Could not write XML file. Permission denied");
+            msgBox.setText("Could not read XML file. Permission denied");
             msgBox.exec();
         }
     }
@@ -114,10 +115,25 @@ void XMLCommProtocolWidget::selectOutputDirectory()
 
 void XMLCommProtocolWidget::generate()
 {
+    // Check if input file is present
+    if (!QFileInfo(m_ui->fileNameLabel->text().trimmed()).isFile())
+    {
+        QMessageBox::critical(this, tr("Please select an XML input file first"), tr("You have to select an input XML file before generating C files."), QMessageBox::Ok);
+        return;
+    }
+
+    // Check if output dir is selected
+    if (!QFileInfo(m_ui->outputDirNameLabel->text().trimmed()).isDir())
+    {
+        QMessageBox::critical(this, tr("Please select output directory first"), tr("You have to select an output directory before generating C files."), QMessageBox::Ok);
+        return;
+    }
+
     // First save file
     save();
     // Clean log
     m_ui->compileLog->clear();
+
     MAVLinkXMLParser* parser = new MAVLinkXMLParser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed());
     connect(parser, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString)));
     bool result = parser->generate();