Finished 2D HSI widget, now displays waypoints, satellites and critical system states

src/QGC.h

@@ -9,7 +9,8 @@ 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);
+    const QColor colorYellow(255, 255, 0);
+    const QColor colorDarkYellow(180, 180, 0);
 
     /** @brief Get the current ground time in microseconds */
     quint64 groundTimeUsecs();

src/ui/HSIDisplay.cc

@@ -37,6 +37,7 @@ This file is part of the PIXHAWK project
 #include "MG.h"
 #include "QGC.h"
 #include "Waypoint.h"
+#include "UASWaypointManager.h"
 
 #include <QDebug>
 
@@ -136,19 +137,6 @@ void HSIDisplay::paintDisplay()
     // Draw background
     painter.fillRect(QRect(0, 0, width(), height()), backgroundColor);
 
-    // Draw status flags
-    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"), visionFix, painter);
-    drawPositionLock(44, 5, tr("GPS"), gpsFix, painter);
-    drawPositionLock(66, 5, tr("IRU"), iruFix, painter);
-
-
     // Draw base instrument
     // ----------------------
     painter.setBrush(Qt::NoBrush);
@@ -164,6 +152,17 @@ void HSIDisplay::paintDisplay()
         drawCircle(xCenterPos, yCenterPos, radius, 0.1f, ringColor, &painter);
     }
 
+    // Draw orientation labels
+    // Translate and rotate coordinate frame
+    painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
+    painter.rotate((-yaw/(M_PI))*180.0f);
+    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);
+    painter.rotate((yaw/(M_PI))*180.0f);
+    painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
+
     // Draw center indicator
     QPolygonF p(3);
     p.replace(0, QPointF(xCenterPos, yCenterPos-2.8484f));
@@ -208,9 +207,6 @@ void HSIDisplay::paintDisplay()
         drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::colorCyan, &painter);
     }
 
-    // Draw waypoints
-    drawWaypoints(painter);
-
     // Labels on outer part and bottom
 
     if (localAvailable > 0)
@@ -225,20 +221,20 @@ void HSIDisplay::paintDisplay()
         paintText(str, ringColor, 3.0f, 10.0f, vheight - 5.0f, &painter);
     }
 
-    // Draw orientation labels
-    // Translate and rotate coordinate frame
-    painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
-    painter.rotate((-yaw/(M_PI))*180.0f);
-    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);
+    // Draw waypoints
+    drawWaypoints(painter);
+
+    // Draw status flags
+    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);
 
-    //    //  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;
+    // Draw position lock indicators
+    drawPositionLock(2,  5, tr("POS"), positionFix, painter);
+    drawPositionLock(22, 5, tr("VIS"), visionFix, painter);
+    drawPositionLock(44, 5, tr("GPS"), gpsFix, painter);
+    drawPositionLock(66, 5, tr("IRU"), iruFix, painter);
 }
 
 void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, QPainter& painter)
@@ -251,7 +247,7 @@ void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, QP
     }
     else
     {
-        painter.setBrush(QGC::colorYellow);
+        painter.setBrush(QGC::colorDarkYellow);
     }
     painter.setPen(Qt::NoPen);
     painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), refToScreenX(7.0f), refToScreenY(4.0f)));
@@ -320,7 +316,8 @@ QPointF HSIDisplay::metricWorldToBody(QPointF world)
 {
     // First translate to body-centered coordinates
     // Rotate around -yaw
-    QPointF result(cos(yaw) * (x - world.x()) + -sin(yaw) * (x - world.x()), sin(yaw) * (y - world.y()) + cos(yaw) * (y - world.y()));
+    float angle = yaw + M_PI;
+    QPointF result(cos(angle) * (x - world.x()) - sin(angle) * (y - world.y()), sin(angle) * (x - world.x()) + cos(angle) * (y - world.y()));
     return result;
 }
 
@@ -328,7 +325,7 @@ QPointF HSIDisplay::metricBodyToWorld(QPointF body)
 {
     // First rotate into world coordinates
     // then translate to world position
-    QPointF result((cos(yaw) * body.x()) + (sin(yaw) * body.x()) + x, (-sin(yaw) * body.y()) + (cos(yaw) * body.y()) + y);
+    QPointF result((cos(yaw) * body.x()) + (sin(yaw) * body.y()) + x, (-sin(yaw) * body.x()) + (cos(yaw) * body.y()) + y);
     return result;
 }
 
@@ -634,9 +631,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
 {
     if (uas)
     {
-        QVector<Waypoint*> list = QVector<Waypoint*>();
-        list.append(new Waypoint(0, x, y, z, yaw, false, false, 0.5f, 2000));
-        list.append(new Waypoint(0, x+0.1, y+0.1, z, yaw, true, true, 0.5f, 2000));
+        QVector<Waypoint*>& list = uas->getWaypointManager().getWaypointList();
 
         QColor color;
         painter.setBrush(Qt::NoBrush);
@@ -651,43 +646,50 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
             // Scale from metric to screen reference coordinates
             QPointF p = metricBodyToRef(in);
 
+            // Setup pen
+            QPen pen(color);
+            painter.setBrush(Qt::NoBrush);
+
+            // DRAW WAYPOINT
+            //drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
+            float waypointSize = vwidth / 20.0f * 2.0f;
+            QPolygonF poly(4);
+            // Top point
+            poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
+            // Right point
+            poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
+            // Bottom point
+            poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
+            poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
+
             // Select color based on if this is the current waypoint
             if (list.at(i)->getCurrent())
             {
-                color = uas->getColor().lighter().lighter();
+                color = QGC::colorCyan;//uas->getColor();
+                pen.setWidthF(refLineWidthToPen(0.8f));
             }
             else
             {
                 color = uas->getColor();
+                pen.setWidthF(refLineWidthToPen(0.4f));
             }
 
-            // Setup pen
-            QPen pen(color);
-            pen.setWidthF(refLineWidthToPen(0.4f));
+            pen.setColor(color);
             painter.setPen(pen);
-            painter.setBrush(Qt::NoBrush);
+            float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
+            drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
+            drawPolygon(poly, &painter);
 
+            // DRAW CONNECTING LINE
             // Draw line from last waypoint to this one
             if (!lastWaypoint.isNull())
             {
+                pen.setWidthF(refLineWidthToPen(0.4f));
+                painter.setPen(pen);
+                color = uas->getColor();
                 drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f), color, &painter);
             }
             lastWaypoint = p;
-
-            //drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
-            float waypointSize = vwidth / 20.0f * 2.0f;
-            QPolygonF poly(4);
-            // Top point
-            poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
-            // Right point
-            poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
-            // Bottom point
-            poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
-            poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
-            drawPolygon(poly, &painter);
-            float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
-            drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
-            painter.setBrush(color);
         }
     }
 }

src/ui/QGCParamWidget.cc

@@ -269,6 +269,7 @@ void QGCParamWidget::requestParameterList()
 {
     // Clear view and request param list
     clear();
+    parameters.clear();
     mav->requestParameters();
 }
 

src/ui/uas/UASInfoWidget.cc

@@ -125,7 +125,7 @@ void UASInfoWidget::updateCPULoad(UASInterface* uas, double load)
 {
     if (activeUAS == uas)
     {
-        this->load = this->load * 0.8f + load * 0.2f;
+        this->load = load;
     }
 }