From 26bca59daaa5898c0d32376a3d789f3a14a0d4f6 Mon Sep 17 00:00:00 2001 From: lm Date: Sat, 3 Jul 2010 16:19:57 +0200 Subject: [PATCH] Finished 2D HSI widget, now displays waypoints, satellites and critical system states --- src/QGC.h | 3 +- src/ui/HSIDisplay.cc | 112 ++++++++++++++++++------------------ src/ui/QGCParamWidget.cc | 1 + src/ui/uas/UASInfoWidget.cc | 2 +- 4 files changed, 61 insertions(+), 57 deletions(-) diff --git a/src/QGC.h b/src/QGC.h index a733477d2..1ea849a23 100644 --- a/src/QGC.h +++ b/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(); diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 645d2832d..b4c8db92d 100644 --- a/src/ui/HSIDisplay.cc +++ b/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 @@ -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 list = QVector(); - 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& 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); } } } diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index 3e5908a16..590b90ab3 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -269,6 +269,7 @@ void QGCParamWidget::requestParameterList() { // Clear view and request param list clear(); + parameters.clear(); mav->requestParameters(); } diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc index 0b926de03..c835c5f64 100644 --- a/src/ui/uas/UASInfoWidget.cc +++ b/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; } } -- 2.22.0