From f929b6cb2750246e35372d4ef01d251806fec6f0 Mon Sep 17 00:00:00 2001 From: lm Date: Thu, 10 Jun 2010 09:28:38 +0200 Subject: [PATCH] Fixed compile warnings, moved MAVLink ids --- lib/QMapControl/src/fixedimageoverlay.cpp | 2 + lib/QMapControl/src/googlesatmapadapter.cpp | 10 +- qgroundcontrol.pro | 6 +- src/ui/HSIDisplay.cc | 208 ++++++++++++++------ src/ui/HSIDisplay.h | 37 +++- 5 files changed, 200 insertions(+), 63 deletions(-) diff --git a/lib/QMapControl/src/fixedimageoverlay.cpp b/lib/QMapControl/src/fixedimageoverlay.cpp index 05bbc9a6aa..a0e6cb70d2 100644 --- a/lib/QMapControl/src/fixedimageoverlay.cpp +++ b/lib/QMapControl/src/fixedimageoverlay.cpp @@ -46,6 +46,8 @@ namespace qmapcontrol void FixedImageOverlay::draw(QPainter* painter, const MapAdapter* mapadapter, const QRect &viewport, const QPoint offset) { + Q_UNUSED(viewport) + Q_UNUSED(offset) if (!visible) return; diff --git a/lib/QMapControl/src/googlesatmapadapter.cpp b/lib/QMapControl/src/googlesatmapadapter.cpp index b7576ef9e3..55ab1084e4 100644 --- a/lib/QMapControl/src/googlesatmapadapter.cpp +++ b/lib/QMapControl/src/googlesatmapadapter.cpp @@ -64,10 +64,12 @@ namespace qmapcontrol //double lon = ((point.x()/tilesize*numberOfTiles)*360)-180; //double lat = (((point.y()/tilesize*numberOfTiles)*180)-90)*-1; - qreal lon = (point.x()*(360./(numberOfTiles*mytilesize)))-180.; - //double lat = -(point.y()*(180./(numberOfTiles*tilesize)))+90; - //qreal lat = getMercatorLatitude(point.y()*-1*(2*M_PI/(numberOfTiles*tilesize)) + M_PI); - qreal lat = lat *180./M_PI; + qreal lon = (point.x()*(360.0/(numberOfTiles*mytilesize)))-180.0; + // qreal lat = -(point.y()*(180.0/(numberOfTiles*mytilesize)))+90.0; + // FIXME Looks buggy + + qreal lat = getMercatorLatitude(point.y()*-1*(2*M_PI/(numberOfTiles*mytilesize)) + M_PI); + //qreal lat = lat *180./M_PI; return QPointF(lon, lat); } diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 5d47625c18..d71b44f8fd 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -3,10 +3,10 @@ # 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") +#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 diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index d5e8358e4a..73b98b750f 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -41,7 +41,28 @@ This file is part of the PIXHAWK project HSIDisplay::HSIDisplay(QWidget *parent) : HDDisplay(NULL, parent), gpsSatellites(), - satellitesUsed(0) + satellitesUsed(0), + attXSet(0), + attYSet(0), + attYawSet(0), + altitudeSet(1.0), + posXSet(0), + posYSet(0), + posZSet(0), + attXSaturation(0.33), + attYSaturation(0.33), + attYawSaturation(0.33), + posXSaturation(1.0), + posYSaturation(1.0), + altitudeSaturation(1.0), + lat(0), + lon(0), + alt(0), + globalAvailable(0), + x(0), + y(0), + z(0), + localAvailable(0) { connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); } @@ -51,13 +72,22 @@ void HSIDisplay::paintEvent(QPaintEvent * event) Q_UNUSED(event); //paintGL(); static quint64 interval = 0; - qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; + //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; interval = MG::TIME::getGroundTimeNow(); paintDisplay(); } void HSIDisplay::paintDisplay() { + // Center location of the HSI gauge items + + float xCenterPos = vwidth/2.0f; + float yCenterPos = vheight/2.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) @@ -78,14 +108,11 @@ void HSIDisplay::paintDisplay() painter.setRenderHint(QPainter::Antialiasing, true); painter.setRenderHint(QPainter::HighQualityAntialiasing, true); painter.fillRect(QRect(0, 0, width(), height()), backgroundColor); - const int columns = 3; - const float spacing = 0.4f; // 40% of width - const float gaugeWidth = vwidth / (((float)columns) + (((float)columns+1) * spacing + spacing * 0.1f)); const QColor ringColor = QColor(200, 250, 200); + // Draw base instrument + // ---------------------- const int ringCount = 2; - const float margin = 0.1f; // 10% margin of total width on each side - for (int i = 0; i < ringCount; i++) { float radius = (vwidth - vwidth * 2.0f * margin) / (2.0f * i+1) / 2.0f; @@ -95,37 +122,60 @@ void HSIDisplay::paintDisplay() // Draw center indicator drawCircle(vwidth/2.0f, vheight/2.0f, 1.0f, 0.1f, ringColor, &painter); - drawGPS(); + // 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); + + // ---------------------- // Draw state indicator // Draw position + QColor positionColor(20, 20, 200); + drawPositionSetpoint(xCenterPos, yCenterPos, baseRadius, positionColor, &painter); + + // Draw attitude + QColor attitudeColor(200, 20, 20); + drawPositionSetpoint(xCenterPos, yCenterPos, baseRadius, attitudeColor, &painter); + // Draw satellites + drawGPS(); + + if (localAvailable > 0) + { + 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); + } + + //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); // 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; -// } -// } + // 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; + // } + // } } /** @@ -134,7 +184,7 @@ void HSIDisplay::paintDisplay() */ void HSIDisplay::setActiveUAS(UASInterface* uas) { - HDDisplay::setActiveUAS(uas); + HDDisplay::setActiveUAS(uas); //qDebug() << "ATTEMPTING TO SET UAS"; if (this->uas != NULL && this->uas != uas) { @@ -143,7 +193,9 @@ void HSIDisplay::setActiveUAS(UASInterface* 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()) + 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))); // Now connect the new UAS @@ -155,24 +207,39 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) //} } -void HSIDisplay::updateAttitudeSetpoints(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec) +void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec) { - + Q_UNUSED(uas); + Q_UNUSED(usec); + attXSet = pitchDesired; + attYSet = rollDesired; + attYawSet = yawDesired; + altitudeSet = thrustDesired; } -void HSIDisplay::updatePositionSetpoints(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec) +void HSIDisplay::updatePositionSetpoints(int uasid, double xDesired, double yDesired, double zDesired, quint64 usec) { - + Q_UNUSED(usec); + Q_UNUSED(uasid); + posXSet = xDesired; + posYSet = yDesired; + posZSet = zDesired; } void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec) { - + this->x = x; + this->y = y; + this->z = z; + localAvailable = usec; } void HSIDisplay::updateGlobalPosition(UASInterface*, double lat, double lon, double alt, quint64 usec) { - + this->lat = lat; + this->lon = lon; + this->alt = alt; + globalAvailable = usec; } void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float azimuth, float snr, bool used) @@ -286,33 +353,45 @@ void HSIDisplay::drawGPS() void HSIDisplay::drawObjects() { - + } -void HSIDisplay::drawBaseLines(float xRef, float yRef, float radius, float yaw, const QColor& color, QPainter* painter, bool solid) +void HSIDisplay::drawPositionSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) { - // Draw the circle - QPen circlePen(Qt::SolidLine); - if (!solid) circlePen.setStyle(Qt::DotLine); - circlePen.setWidth(refLineWidthToPen(0.5f)); - circlePen.setColor(defaultColor); - painter->setBrush(Qt::NoBrush); - painter->setPen(circlePen); - drawCircle(xRef, yRef, radius, 200.0f, color, painter); - //drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.0f, color, painter); + // Draw the needle + const float maxWidth = radius / 10.0f; + const float minWidth = maxWidth * 0.3f; -// // Draw the value -// QString label; -// label.sprintf("%05.1f", value); -// paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter); + float angle = asin(posXSet) + acos(posYSet); + + QPolygonF p(6); + p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f)); + p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f)); + p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f)); + p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.5f)); + p.replace(4, QPointF(xRef, yRef-radius * 0.46f)); + p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f)); + + rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef)); + + QBrush indexBrush; + indexBrush.setColor(color); + indexBrush.setStyle(Qt::SolidPattern); + painter->setPen(Qt::SolidLine); + painter->setPen(color); + painter->setBrush(indexBrush); + drawPolygon(p, painter); +} + +void HSIDisplay::drawAttitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) +{ // Draw the needle - // Scale the rotation so that the gauge does one revolution - // per max. change - const float rangeScale = (2.0f * M_PI); const float maxWidth = radius / 10.0f; const float minWidth = maxWidth * 0.3f; + float angle = asin(attXSet) + acos(attYSet); + QPolygonF p(6); p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f)); @@ -322,13 +401,32 @@ void HSIDisplay::drawBaseLines(float xRef, float yRef, float radius, float yaw, p.replace(4, QPointF(xRef, yRef-radius * 0.46f)); p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f)); - rotatePolygonClockWiseRad(p, yaw*rangeScale, QPointF(xRef, yRef)); + rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef)); QBrush indexBrush; - indexBrush.setColor(defaultColor); + indexBrush.setColor(color); indexBrush.setStyle(Qt::SolidPattern); painter->setPen(Qt::SolidLine); - painter->setPen(defaultColor); + painter->setPen(color); painter->setBrush(indexBrush); drawPolygon(p, painter); + + // TODO Draw Yaw indicator +} + +void HSIDisplay::drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) +{ + // Draw the circle + QPen circlePen(Qt::SolidLine); + circlePen.setWidth(refLineWidthToPen(0.5f)); + circlePen.setColor(color); + painter->setBrush(Qt::NoBrush); + painter->setPen(circlePen); + drawCircle(xRef, yRef, radius, 200.0f, color, painter); + //drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.0f, color, painter); + + // // Draw the value + // QString label; + // label.sprintf("%05.1f", value); + // paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter); } diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 321b62bc38..cd077a486a 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -51,13 +51,19 @@ public: public slots: void setActiveUAS(UASInterface* uas); void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used); + void updateAttitudeSetpoints(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); + void updatePositionSetpoints(int uasid, double xDesired, double yDesired, double zDesired, 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 paintEvent(QPaintEvent * event); protected slots: void paintDisplay(); void drawGPS(); void drawObjects(); - void drawBaseLines(float xRef, float yRef, float radius, float yaw, const QColor& color, QPainter* painter, bool solid); + 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 drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter); protected: @@ -103,6 +109,35 @@ protected: QMap gpsSatellites; unsigned int satellitesUsed; + // Current controller values + float attXSet; + float attYSet; + float attYawSet; + float altitudeSet; + + float posXSet; + float posYSet; + float posZSet; + + // Controller saturation values + float attXSaturation; + float attYSaturation; + float attYawSaturation; + + float posXSaturation; + float posYSaturation; + float altitudeSaturation; + + // Position + float lat; + float lon; + float alt; + quint64 globalAvailable; ///< Last global position update time + float x; + float y; + float z; + quint64 localAvailable; ///< Last local position update time + private: }; -- GitLab