diff --git a/QGCApplication.pro b/QGCApplication.pro index ea05c690ceb0fef37dc96cdbbc752173558f5366..accfadd2a4782e8972d022c6f93ea08e3f8407ab 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -161,7 +161,6 @@ INCLUDEPATH += \ FORMS += \ src/QGCQmlWidgetHolder.ui \ - src/ui/HDDisplay.ui \ src/ui/Linechart.ui \ src/ui/LogReplayLinkConfigurationWidget.ui \ src/ui/MainWindow.ui \ @@ -270,9 +269,6 @@ HEADERS += \ src/uas/UASInterface.h \ src/uas/UASMessageHandler.h \ src/uas/UASWaypointManager.h \ - src/ui/HDDisplay.h \ - src/ui/HSIDisplay.h \ - src/ui/HUD.h \ src/ui/linechart/ChartPlot.h \ src/ui/linechart/IncrementalPlot.h \ src/ui/linechart/LinechartPlot.h \ @@ -312,7 +308,6 @@ HEADERS += \ src/ui/QGCMAVLinkInspector.h \ src/ui/QGCMAVLinkLogPlayer.h \ src/ui/QGCPluginHost.h \ - src/ui/QGCRGBDView.h \ src/ui/QGCTabbedInfoView.h \ src/ui/QGCTCPLinkConfiguration.h \ src/ui/QGCUASFileView.h \ @@ -402,9 +397,6 @@ SOURCES += \ src/uas/UAS.cc \ src/uas/UASMessageHandler.cc \ src/uas/UASWaypointManager.cc \ - src/ui/HDDisplay.cc \ - src/ui/HSIDisplay.cc \ - src/ui/HUD.cc \ src/ui/linechart/ChartPlot.cc \ src/ui/linechart/IncrementalPlot.cc \ src/ui/linechart/LinechartPlot.cc \ @@ -444,7 +436,6 @@ SOURCES += \ src/ui/QGCMAVLinkInspector.cc \ src/ui/QGCMAVLinkLogPlayer.cc \ src/ui/QGCPluginHost.cc \ - src/ui/QGCRGBDView.cc \ src/ui/QGCTabbedInfoView.cpp \ src/ui/QGCTCPLinkConfiguration.cc \ src/ui/QGCUASFileView.cc \ diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc deleted file mode 100644 index a9ba7f281ef7f7fa30248e966866e00958096530..0000000000000000000000000000000000000000 --- a/src/ui/HDDisplay.cc +++ /dev/null @@ -1,1113 +0,0 @@ -/*===================================================================== -======================================================================*/ - -/** - * @file - * @brief Implementation of Head Down Display (HDD) - * - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "HDDisplay.h" -#include "ui_HDDisplay.h" -#include "MG.h" -#include "QGC.h" -#include "QGCApplication.h" -#include "MultiVehicleManager.h" -#include "UAS.h" - -HDDisplay::HDDisplay(const QStringList &plotList, QString title, QWidget *parent) : - QGraphicsView(parent), - uas(NULL), - xCenterOffset(0.0f), - yCenterOffset(0.0f), - vwidth(80.0f), - vheight(80.0f), - backgroundColor(QColor(0, 0, 0)), - defaultColor(QColor(70, 200, 70)), - setPointColor(QColor(200, 20, 200)), - warningColor(Qt::yellow), - criticalColor(Qt::red), - infoColor(QColor(20, 200, 20)), - fuelColor(criticalColor), - warningBlinkRate(5), - refreshTimer(new QTimer(this)), - hardwareAcceleration(true), - strongStrokeWidth(1.5f), - normalStrokeWidth(1.0f), - fineStrokeWidth(0.5f), - acceptList(new QStringList()), - acceptUnitList(new QStringList()), - lastPaintTime(0), - columns(3), - valuesChanged(true), - m_ui(NULL) -{ - setWindowTitle(title); - //m_ui->setupUi(this); - - setAutoFillBackground(true); - - // Add all items in accept list to gauge - for(int i = 0; i < plotList.length(); ++i) - addGauge(plotList.at(i)); - - restoreState(); - // Set preferred size - setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - - createActions(); - - // setBackgroundBrush(QBrush(backgroundColor)); - // setDragMode(QGraphicsView::ScrollHandDrag); - // setCacheMode(QGraphicsView::CacheBackground); - // // FIXME Handle full update with care - ressource intensive - // setViewportUpdateMode(QGraphicsView::FullViewportUpdate); - // - // setRenderHints(QPainter::Antialiasing | QPainter::SmoothPixmapTransform); - // - // //Set-up the scene - // QGraphicsScene* Scene = new QGraphicsScene(this); - // setScene(Scene); - // - // //Populate the scene - // for(int x = 0; x < 1000; x = x + 25) { - // for(int y = 0; y < 1000; y = y + 25) { - // - // if(x % 100 == 0 && y % 100 == 0) { - // Scene->addRect(x, y, 2, 2); - // - // QString pointString; - // QTextStream stream(&pointString); - // stream << "(" << x << "," << y << ")"; - // QGraphicsTextItem* item = Scene->addText(pointString); - // item->setPos(x, y); - // } else { - // Scene->addRect(x, y, 1, 1); - // } - // } - // } - // - // //Set-up the view - // setSceneRect(0, 0, 1000, 1000); - // setCenter(QPointF(500.0, 500.0)); //A modified version of centerOn(), handles special cases - // setCursor(Qt::OpenHandCursor); - - - // Set minimum size - this->setMinimumHeight(125); - this->setMinimumWidth(100); - - scalingFactor = this->width()/vwidth; - - // Refresh timer - refreshTimer->setInterval(180); // - connect(refreshTimer, SIGNAL(timeout()), this, SLOT(triggerUpdate())); - //connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintGL())); - - fontDatabase = QFontDatabase(); - const QString fontFileName = ":/res/fonts/vera.ttf"; ///< Font file is part of the QRC file and compiled into the app - const QString fontFamilyName = "Bitstream Vera Sans"; - if(!QFile::exists(fontFileName)) qDebug() << "ERROR! font file: " << fontFileName << " DOES NOT EXIST!"; - - fontDatabase.addApplicationFont(fontFileName); - font = fontDatabase.font(fontFamilyName, "Roman", qMax(5, (int)(10*scalingFactor*1.2f+0.5f))); - if (font.family() != fontFamilyName) qDebug() << "ERROR! Font not loaded: " << fontFamilyName; - - // Connect with UAS - connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &HDDisplay::_activeVehicleChanged); - _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); -} - -HDDisplay::~HDDisplay() -{ - saveState(); - if(this->refreshTimer) - { - delete this->refreshTimer; - } - if(this->acceptList) - { - delete this->acceptList; - } - if(this->acceptUnitList) - { - delete this->acceptUnitList; - } - if(this->m_ui) - { - delete m_ui; - } -} - -QSize HDDisplay::sizeHint() const -{ - return QSize(400, 400.0f*(vwidth/vheight)*1.2f); -} - -void HDDisplay::enableGLRendering(bool enable) -{ - Q_UNUSED(enable); -} - -void HDDisplay::triggerUpdate() -{ - // Only repaint the regions necessary - update(this->geometry()); -} - -//void HDDisplay::updateValue(UASInterface* uas, const QString& name, const QString& unit, double value, quint64 msec) -//{ -// // UAS is not needed -// Q_UNUSED(uas); - -// if (!isnan(value) && !isinf(value)) -// { -// // Update mean -// const float oldMean = valuesMean.value(name, 0.0f); -// const int meanCount = valuesCount.value(name, 0); -// double mean = (oldMean * meanCount + value) / (meanCount + 1); -// if (isnan(mean) || isinf(mean)) mean = 0.0; -// valuesMean.insert(name, mean); -// valuesCount.insert(name, meanCount + 1); -// // Two-value sliding average -// double dot = (valuesDot.value(name) + (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f))/2.0f; -// if (isnan(dot) || isinf(dot)) -// { -// dot = 0.0; -// } -// valuesDot.insert(name, dot); -// values.insert(name, value); -// lastUpdate.insert(name, msec); -// //} - -// //qDebug() << __FILE__ << __LINE__ << "VALUE:" << value << "MEAN:" << mean << "DOT:" << dot << "COUNT:" << meanCount; -// } -//} - -void HDDisplay::paintEvent(QPaintEvent * event) -{ - Q_UNUSED(event); - //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; - renderOverlay(); -} - -void HDDisplay::contextMenuEvent (QContextMenuEvent* event) -{ - QMenu menu(this); - menu.addAction(addGaugeAction); - menu.addActions(getItemRemoveActions()); - menu.addSeparator(); - menu.addAction(setColumnsAction); - // Title change would ruin settings - // this can only be allowed once - // HDDisplays are instantiated - // by a factory method based on - // QSettings - //menu.addAction(setTitleAction); - menu.exec(event->globalPos()); -} - -void HDDisplay::saveState() -{ - QSettings settings; - - QString instruments; - // Restore instrument settings - for (int i = 0; i < acceptList->count(); i++) { - QString key = acceptList->at(i); - instruments += "|" + QString::number(minValues.value(key, -1.0))+","+key+","+acceptUnitList->at(i)+","+QString::number(maxValues.value(key, +1.0))+","+customNames.value(key, "")+","+((symmetric.value(key, false)) ? "s" : ""); - } - - // qDebug() << "Saving" << instruments; - - settings.setValue(windowTitle()+"_gauges", instruments); -} - -void HDDisplay::restoreState() -{ - QSettings settings; - - acceptList->clear(); - - QStringList instruments = settings.value(windowTitle()+"_gauges").toString().split('|'); - for (int i = 0; i < instruments.count(); i++) { - addGauge(instruments.at(i)); - } -} - -QList HDDisplay::getItemRemoveActions() -{ - QList actions; - for(int i = 0; i < acceptList->length(); ++i) { - QString gauge = acceptList->at(i); - QAction* remove = new QAction(tr("Remove %1 gauge").arg(gauge), this); - remove->setStatusTip(tr("Removes the %1 gauge from the view.").arg(gauge)); - remove->setData(gauge); - connect(remove, SIGNAL(triggered()), this, SLOT(removeItemByAction())); - actions.append(remove); - } - return actions; -} - -void HDDisplay::removeItemByAction() -{ - QAction* trigger = qobject_cast(QObject::sender()); - if (trigger) { - QString item = trigger->data().toString(); - int index = acceptList->indexOf(item); - acceptList->removeAt(index); - minValues.remove(item); - maxValues.remove(item); - symmetric.remove(item); - adjustGaugeAspectRatio(); - } -} - -void HDDisplay::addGauge() -{ - QStringList items; - for (int i = 0; i < values.count(); ++i) { - QString key = values.keys().at(i); - QString label = key; - QStringList keySplit = key.split("."); - if (keySplit.size() > 1) - { - keySplit.removeFirst(); - label = keySplit.join("."); - } - QString unit = units.value(key); - if (unit.contains("deg") || unit.contains("rad")) { - items.append(QString("%1,%2,%3,%4,%5,s").arg("-180").arg(key).arg(unit).arg("+180").arg(label)); - } else { - items.append(QString("%1,%2,%3,%4,%5").arg("0").arg(key).arg(unit).arg("+100").arg(label)); - } - } - bool ok; - QString item = QInputDialog::getItem(this, tr("Add Gauge Instrument"), - tr("Format: min, data name, unit, max, label [,s]"), items, 0, true, &ok); - if (ok && !item.isEmpty()) { - addGauge(item); - } -} - -void HDDisplay::addGauge(const QString& gauge) -{ - if (gauge.length() > 0) { - QStringList parts = gauge.split(','); - if (parts.count() > 2) { - double val; - bool ok; - bool success = true; - - QString key = parts.at(1); - QString unit = parts.at(2); - - if (!acceptList->contains(key)) { - // Convert min to double number - val = parts.first().toDouble(&ok); - success &= ok; - if (ok) minValues.insert(key, val); - // Convert max to double number - val = parts.at(3).toDouble(&ok); - success &= ok; - if (ok) maxValues.insert(key, val); - // Convert name - if (parts.length() >= 5) - { - if (parts.at(4).length() > 0) - { - customNames.insert(key, parts.at(4)); - } - } - // Convert symmetric flag - if (parts.length() >= 6) - { - if (parts.at(5).contains("s")) - { - symmetric.insert(key, true); - } - } - if (success) { - // Add value to acceptlist - acceptList->append(key); - acceptUnitList->append(unit); - } - } - } else if (parts.count() > 1) { - if (!acceptList->contains(gauge)) { - acceptList->append(parts.at(0)); - acceptUnitList->append(parts.at(1)); - } - } - } - adjustGaugeAspectRatio(); -} - -void HDDisplay::createActions() -{ - addGaugeAction = new QAction(tr("New &Gauge"), this); - addGaugeAction->setStatusTip(tr("Add a new gauge to the view by adding its name from the linechart")); - connect(addGaugeAction, SIGNAL(triggered()), this, SLOT(addGauge())); - - setTitleAction = new QAction(tr("Set Widget Title"), this); - setTitleAction->setStatusTip(tr("Set the title caption of this tool widget")); - connect(setTitleAction, SIGNAL(triggered()), this, SLOT(setTitle())); - - setColumnsAction = new QAction(tr("Set Number of Instrument Columns"), this); - setColumnsAction->setStatusTip(tr("Set number of columns to draw")); - connect(setColumnsAction, SIGNAL(triggered()), this, SLOT(setColumns())); -} - - -void HDDisplay::setColumns() -{ - bool ok; - int i = QInputDialog::getInt(this, tr("Number of Instrument Columns"), - tr("Columns:"), columns, 1, 15, 1, &ok); - if (ok) { - columns = i; - } -} - -void HDDisplay::setColumns(int cols) -{ - columns = cols; - adjustGaugeAspectRatio(); -} - -void HDDisplay::adjustGaugeAspectRatio() -{ - // Adjust vheight dynamically according to the number of rows - float vColWidth = vwidth / columns; - int vRows = ceil(acceptList->length()/(float)columns); - // Assuming square instruments, vheight is column width*row count - vheight = vColWidth * vRows; -} - -void HDDisplay::setTitle() -{ - QDockWidget* parent = dynamic_cast(this->parentWidget()); - if (parent) { - bool ok; - QString text = QInputDialog::getText(this, tr("New title"), - tr("Widget title:"), QLineEdit::Normal, - parent->windowTitle(), &ok); - if (ok && !text.isEmpty()) - parent->setWindowTitle(text); - this->setWindowTitle(text); - } -} - -void HDDisplay::renderOverlay() -{ - if (!valuesChanged || !isVisible()) return; - -#if (QGC_EVENTLOOP_DEBUG) - qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; -#endif - 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; - // Draw instruments - // TESTING THIS SHOULD BE MOVED INTO A QGRAPHICSVIEW - // Update scaling factor - // adjust scaling to fit both horizontally and vertically - scalingFactor = this->width()/vwidth; - - double scalingFactorH = this->height()/vheight; - if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH; - - QPainter painter(viewport()); - painter.setRenderHint(QPainter::Antialiasing, true); - painter.setRenderHint(QPainter::HighQualityAntialiasing, true); - //painter.fillRect(QRect(0, 0, width(), height()), backgroundColor); - const float spacing = 0.4f; // 40% of width - const float gaugeWidth = vwidth / (((float)columns) + (((float)columns+1) * spacing + spacing * 0.5f)); - QColor gaugeColor; - gaugeColor = qgcApp()->styleIsDark() ? gaugeColor = QColor(255, 255, 255) : gaugeColor = QColor(0, 0, 0); - //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); - QString label = customNames.value(value); - drawGauge(xCoord, yCoord, gaugeWidth/2.0f, minValues.value(value, -1.0f), maxValues.value(value, 1.0f), label, values.value(value, minValues.value(value, 0.0f)), gaugeColor, &painter, symmetric.value(value, false), 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*0.9f > vwidth) - { - yCoord += topSpacing + gaugeWidth; - xCoord = leftSpacing + gaugeWidth/2.0f; - } - } -} - -/** - * - * @param uas the UAS/MAV to monitor/display with the HUD - */ -void HDDisplay::_activeVehicleChanged(Vehicle* vehicle) -{ - // Disconnect any previously connected active UAS - if (this->uas != NULL) { - removeSource(this->uas); - this->uas = NULL; - } - - if (vehicle) { - this->uas = vehicle->uas(); - // Now connect the new UAS - addSource(uas); - } -} - -/** - * Rotate a polygon around a point - * - * @param p polygon to rotate - * @param origin the rotation center - * @param angle rotation angle, in radians - * @return p Polygon p rotated by angle around the origin point - */ -void HDDisplay::rotatePolygonClockWiseRad(QPolygonF& p, float angle, QPointF origin) -{ - // Standard 2x2 rotation matrix, counter-clockwise - // - // | cos(phi) sin(phi) | - // | -sin(phi) cos(phi) | - // - for (int i = 0; i < p.size(); i++) { - QPointF curr = p.at(i); - - const float x = curr.x(); - const float y = curr.y(); - - curr.setX(((cos(angle) * (x-origin.x())) + (-sin(angle) * (y-origin.y()))) + origin.x()); - curr.setY(((sin(angle) * (x-origin.x())) + (cos(angle) * (y-origin.y()))) + origin.y()); - p.replace(i, curr); - } -} - -void HDDisplay::drawPolygon(QPolygonF refPolygon, QPainter* painter) -{ - // Scale coordinates - QPolygonF draw(refPolygon.size()); - for (int i = 0; i < refPolygon.size(); i++) { - QPointF curr; - curr.setX(refToScreenX(refPolygon.at(i).x())); - curr.setY(refToScreenY(refPolygon.at(i).y())); - draw.replace(i, curr); - } - painter->drawPolygon(draw); -} - -void HDDisplay::drawChangeRateStrip(float xRef, float yRef, float height, float minRate, float maxRate, float value, QPainter* painter) -{ - QBrush brush(defaultColor, Qt::NoBrush); - painter->setBrush(brush); - QPen rectPen(Qt::SolidLine); - rectPen.setWidth(0); - rectPen.setColor(defaultColor); - painter->setPen(rectPen); - - float scaledValue = value; - - // Saturate value - if (value > maxRate) scaledValue = maxRate; - if (value < minRate) scaledValue = minRate; - - // x (Origin: xRef, yRef) - // - - // | - // | - // | - // = - // | - // -0.005 >| - // | - // - - - const float width = height / 8.0f; - const float lineWidth = 0.5f; - - // Indicator lines - // Top horizontal line - drawLine(xRef, yRef, xRef+width, yRef, lineWidth, defaultColor, painter); - // Vertical main line - drawLine(xRef+width/2.0f, yRef, xRef+width/2.0f, yRef+height, lineWidth, defaultColor, painter); - // Zero mark - drawLine(xRef, yRef+height/2.0f, xRef+width, yRef+height/2.0f, lineWidth, defaultColor, painter); - // Horizontal bottom line - drawLine(xRef, yRef+height, xRef+width, yRef+height, lineWidth, defaultColor, painter); - - // Text - QString label; - label.sprintf("< %06.2f", value); - paintText(label, defaultColor, 3.0f, xRef+width/2.0f, yRef+height-((scaledValue - minRate)/(maxRate-minRate))*height - 1.6f, painter); -} - -void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float max, QString name, float value, const QColor& color, QPainter* painter, bool symmetric, QPair goodRange, QPair criticalRange, bool solid) -{ - // Select color scheme based on light or dark theme. - QColor valueColor; - QColor backgroundColor; - if (qgcApp()->styleIsDark()) - { - valueColor = QGC::colorCyan; - backgroundColor = QColor(34, 34, 34); - } - else - { - valueColor = QColor(26, 75, 95); - backgroundColor = QColor(246, 246, 246); - } - - // Draw the circle - QPen circlePen(Qt::SolidLine); - - // Rotate the whole gauge with this angle (in radians) for the zero position - float zeroRotation; - if (symmetric) { - zeroRotation = 1.35f; - } else { - zeroRotation = 0.49f; - } - - // Scale the rotation so that the gauge does one revolution - // per max. change - float rangeScale; - if (symmetric) { - rangeScale = ((2.0f * M_PI) / (max - min)) * 0.57f; - } else { - rangeScale = ((2.0f * M_PI) / (max - min)) * 0.72f; - } - - const float scaledValue = (value-min)*rangeScale; - - float nameHeight = radius / 2.6f; - paintText(name.toUpper(), color, nameHeight*0.7f, xRef-radius, yRef-radius, painter); - - // Ensure some space - nameHeight *= 1.2f; - - if (!solid) { - circlePen.setStyle(Qt::DotLine); - } - circlePen.setWidth(refLineWidthToPen(radius/12.0f)); - circlePen.setColor(color); - - if (symmetric) { - circlePen.setStyle(Qt::DashLine); - } - painter->setBrush(Qt::NoBrush); - painter->setPen(circlePen); - drawCircle(xRef, yRef+nameHeight, radius, 0.0f, color, painter); - //drawCircle(xRef, yRef+nameHeight, radius, 0.0f, 170.0f, 1.0f, color, painter); - - QString label; - - // Show integer values without decimal places - if (intValues.contains(name)) { - label.sprintf("% 05d", (int)value); - } else { - label.sprintf("% 06.1f", value); - } - - - // Text - // height - const float textHeight = radius/2.1f; - const float textX = xRef-radius/3.0f; - const float textY = yRef+radius/2.0f; - - // Draw background rectangle - QBrush brush(backgroundColor, Qt::SolidPattern); - painter->setBrush(brush); - painter->setPen(Qt::NoPen); - - if (symmetric) { - painter->drawRect(refToScreenX(xRef-radius), refToScreenY(yRef+nameHeight+radius/4.0f), refToScreenX(radius+radius), refToScreenY((radius - radius/4.0f)*1.2f)); - } else { - painter->drawRect(refToScreenX(xRef-radius/2.5f), refToScreenY(yRef+nameHeight+radius/4.0f), refToScreenX(radius+radius/2.0f), refToScreenY((radius - radius/4.0f)*1.2f)); - } - - // Draw good value and crit. value markers - if (goodRange.first != goodRange.second) { - QRectF rectangle(refToScreenX(xRef-radius/2.0f), refToScreenY(yRef+nameHeight-radius/2.0f), refToScreenX(radius*2.0f), refToScreenX(radius*2.0f)); - painter->setPen(Qt::green); - //int start = ((goodRange.first*rangeScale+zeroRotation)/M_PI)*180.0f * 16.0f;// + 16.0f * 60.0f; - //int span = start - ((goodRange.second*rangeScale+zeroRotation)/M_PI)*180.0f * 16.0f; - //painter->drawArc(rectangle, start, span); - } - - if (criticalRange.first != criticalRange.second) { - QRectF rectangle(refToScreenX(xRef-radius/2.0f-3.0f), refToScreenY(yRef+nameHeight-radius/2.0f-3.0f), refToScreenX(radius*2.0f), refToScreenX(radius*2.0f)); - painter->setPen(Qt::yellow); - //int start = ((criticalRange.first*rangeScale+zeroRotation)/M_PI)*180.0f * 16.0f - 180.0f*16.0f;// + 16.0f * 60.0f; - //int span = start - ((criticalRange.second*rangeScale+zeroRotation)/M_PI)*180.0f * 16.0f + 180.0f*16.0f; - //painter->drawArc(rectangle, start, span); - } - - // Draw the value - //painter->setPen(textColor); - paintText(label, valueColor, textHeight, textX, textY+nameHeight, painter); - //paintText(label, color, ((radius - radius/3.0f)*1.1f), xRef-radius/2.5f, yRef+radius/3.0f, painter); - - // Draw the needle - - const float maxWidth = radius / 6.0f; - const float minWidth = maxWidth * 0.3f; - - QPolygonF p(6); - - p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef+nameHeight+radius * 0.05f)); - p.replace(1, QPointF(xRef-minWidth/2.0f, yRef+nameHeight+radius * 0.89f)); - p.replace(2, QPointF(xRef+minWidth/2.0f, yRef+nameHeight+radius * 0.89f)); - p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef+nameHeight+radius * 0.05f)); - p.replace(4, QPointF(xRef, yRef+nameHeight+radius * 0.0f)); - p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef+nameHeight+radius * 0.05f)); - - - rotatePolygonClockWiseRad(p, scaledValue+zeroRotation, QPointF(xRef, yRef+nameHeight)); - - QBrush indexBrush; - indexBrush.setColor(color); - indexBrush.setStyle(Qt::SolidPattern); - painter->setPen(Qt::NoPen); - painter->setBrush(indexBrush); - drawPolygon(p, painter); -} - - -void HDDisplay::drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter) -{ - if (values.size() > 0) { - QString selectedKey = values.begin().key(); - // | | | | | | - // | | | | | | - // x speed: 2.54 - - // One column per value - QMapIterator value(values); - - float x = xRef; - float y = yRef; - - const float vspacing = 1.0f; - float width = 1.5f; - float height = 1.5f; - const float hspacing = 0.6f; - - int i = 0; - while (value.hasNext() && i < maxNum && x < maxWidth && y < maxHeight) { - value.next(); - QBrush brush(Qt::SolidPattern); - - - if (value.value() < 0.01f && value.value() > -0.01f) { - brush.setColor(Qt::gray); - } else if (value.value() > 0.01f) { - brush.setColor(Qt::blue); - } else { - brush.setColor(Qt::yellow); - } - - painter->setBrush(brush); - painter->setPen(Qt::NoPen); - - // Draw current value colormap - painter->drawRect(refToScreenX(x), refToScreenY(y), refToScreenX(width), refToScreenY(height)); - - // Draw change rate colormap - painter->drawRect(refToScreenX(x), refToScreenY(y+height+hspacing), refToScreenX(width), refToScreenY(height)); - - // Draw mean value colormap - painter->drawRect(refToScreenX(x), refToScreenY(y+2.0f*(height+hspacing)), refToScreenX(width), refToScreenY(height)); - - // Add spacing - x += width+vspacing; - - // Iterate - i++; - } - - // Draw detail label - QString detail = "NO DATA AVAILABLE"; - - if (values.contains(selectedKey)) { - detail = values.find(selectedKey).key(); - detail.append(": "); - detail.append(QString::number(values.find(selectedKey).value())); - } - paintText(detail, QColor(255, 255, 255), 3.0f, xRef, yRef+3.0f*(height+hspacing)+1.0f, painter); - } -} - -void HDDisplay::drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid) -{ - // 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); - - QString label; - label.sprintf("%05.1f", value); - - // Draw the value - paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter); - - // Draw the needle - // Scale the rotation so that the gauge does one revolution - // per max. change - const float rangeScale = (2.0f * M_PI) / expectedMaxChange; - const float maxWidth = radius / 10.0f; - const float minWidth = maxWidth * 0.3f; - - 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, value*rangeScale, QPointF(xRef, yRef)); - - QBrush indexBrush; - indexBrush.setColor(defaultColor); - indexBrush.setStyle(Qt::SolidPattern); - painter->setPen(Qt::SolidLine); - painter->setPen(defaultColor); - painter->setBrush(indexBrush); - drawPolygon(p, painter); -} - -/** - * Paint text on top of the image and OpenGL drawings - * - * @param text chars to write - * @param color text color - * @param fontSize text size in mm - * @param refX position in reference units (mm of the real instrument). This is relative to the measurement unit position, NOT in pixels. - * @param refY position in reference units (mm of the real instrument). This is relative to the measurement unit position, NOT in pixels. - */ -void HDDisplay::paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter) -{ - QPen prevPen = painter->pen(); - float pPositionX = refToScreenX(refX) - (fontSize*scalingFactor*0.072f); - float pPositionY = refToScreenY(refY) - (fontSize*scalingFactor*0.212f); - - QFont font("Bitstream Vera Sans"); - // Enforce minimum font size of 5 pixels - int fSize = qMax(5, (int)(fontSize*scalingFactor*1.26f)); - font.setPixelSize(fSize); - - QFontMetrics metrics = QFontMetrics(font); - int border = qMax(4, metrics.leading()); - QRect rect = metrics.boundingRect(0, 0, width() - 2*border, int(height()*0.125), - Qt::AlignLeft | Qt::TextWordWrap, text); - painter->setPen(color); - painter->setFont(font); - painter->setRenderHint(QPainter::TextAntialiasing); - painter->drawText(pPositionX, pPositionY, - rect.width(), rect.height(), - Qt::AlignCenter | Qt::TextWordWrap, text); - painter->setPen(prevPen); -} - -float HDDisplay::refLineWidthToPen(float line) -{ - return line * 2.50f; -} - -// Connect a generic source -void HDDisplay::addSource(QObject* obj) -{ - connect(obj, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)), this, SLOT(updateValue(int,QString,QString,QVariant,quint64))); -} - -// Disconnect a generic source -void HDDisplay::removeSource(QObject* obj) -{ - disconnect(obj, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)), this, SLOT(updateValue(int,QString,QString,QVariant,quint64))); -} - -void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const QVariant &variant, const quint64 msec) -{ - Q_UNUSED(uasId); - Q_UNUSED(unit); - - QMetaType::Type type = static_cast< QMetaType::Type>(variant.type()); - if(type == QMetaType::QByteArray || type == QMetaType::QString) - return; - - bool ok; - double value = variant.toDouble(&ok); - if(!ok) - return; - - if(type == QMetaType::Int || type == QMetaType::UInt || type == QMetaType::Long || type == QMetaType::LongLong - || type == QMetaType::Short || type == QMetaType::Char || type == QMetaType::ULong || type == QMetaType::ULongLong - || type == QMetaType::UShort || type == QMetaType::UChar || type == QMetaType::Bool ) { - if (!intValues.contains(name)) - intValues.insert(name, true); - } - - // Update mean - const float oldMean = valuesMean.value(name, 0.0f); - const int meanCount = valuesCount.value(name, 0); - valuesMean.insert(name, (oldMean * meanCount + value) / (meanCount + 1)); - valuesCount.insert(name, meanCount + 1); - valuesDot.insert(name, (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f)); - if (values.value(name, 0.0) != value) valuesChanged = true; - values.insert(name, value); - units.insert(name, unit); - lastUpdate.insert(name, msec); -} - -/** - * @param y coordinate in pixels to be converted to reference mm units - * @return the screen coordinate relative to the QGLWindow origin - */ -float HDDisplay::refToScreenX(float x) -{ - return (scalingFactor * x); -} - -/** - * @param x coordinate in pixels to be converted to reference mm units - * @return the screen coordinate relative to the QGLWindow origin - */ -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); - pen.setWidth(refLineWidthToPen(width)); - pen.setColor(color); - painter->setPen(pen); - painter->drawLine(QPoint(refToScreenX(refX1), refToScreenY(refY1)), QPoint(refToScreenX(refX2), refToScreenY(refY2))); -} - -void HDDisplay::drawEllipse(float refX, float refY, float radiusX, float radiusY, float lineWidth, const QColor& color, QPainter* painter) -{ - QPen pen(painter->pen().style()); - pen.setWidth(refLineWidthToPen(lineWidth)); - pen.setColor(color); - painter->setPen(pen); - painter->drawEllipse(QPointF(refToScreenX(refX), refToScreenY(refY)), refToScreenX(radiusX), refToScreenY(radiusY)); -} - -void HDDisplay::drawCircle(float refX, float refY, float radius, float lineWidth, const QColor& color, QPainter* painter) -{ - drawEllipse(refX, refY, radius, radius, lineWidth, color, painter); -} - -void HDDisplay::changeEvent(QEvent *e) -{ - QWidget::changeEvent(e); - switch (e->type()) { - case QEvent::LanguageChange: - m_ui->retranslateUi(this); - break; - default: - break; - } -} - - -void HDDisplay::showEvent(QShowEvent* event) -{ - // React only to internal (pre-display) - // events - Q_UNUSED(event); - refreshTimer->start(updateInterval); -} - -void HDDisplay::hideEvent(QHideEvent* event) -{ - // React only to internal (pre-display) - // events - Q_UNUSED(event); - refreshTimer->stop(); - saveState(); -} - - -///** -// * Sets the current centerpoint. Also updates the scene's center point. -// * Unlike centerOn, which has no way of getting the floating point center -// * back, SetCenter() stores the center point. It also handles the special -// * sidebar case. This function will claim the centerPoint to sceneRec ie. -// * the centerPoint must be within the sceneRec. -// */ -////Set the current centerpoint in the -//void HDDisplay::setCenter(const QPointF& centerPoint) { -// //Get the rectangle of the visible area in scene coords -// QRectF visibleArea = mapToScene(rect()).boundingRect(); -// -// //Get the scene area -// QRectF sceneBounds = sceneRect(); -// -// double boundX = visibleArea.width() / 2.0; -// double boundY = visibleArea.height() / 2.0; -// double boundWidth = sceneBounds.width() - 2.0 * boundX; -// double boundHeight = sceneBounds.height() - 2.0 * boundY; -// -// //The max boundary that the centerPoint can be to -// QRectF bounds(boundX, boundY, boundWidth, boundHeight); -// -// if(bounds.contains(centerPoint)) { -// //We are within the bounds -// currentCenterPoint = centerPoint; -// } else { -// //We need to clamp or use the center of the screen -// if(visibleArea.contains(sceneBounds)) { -// //Use the center of scene ie. we can see the whole scene -// currentCenterPoint = sceneBounds.center(); -// } else { -// -// currentCenterPoint = centerPoint; -// -// //We need to clamp the center. The centerPoint is too large -// if(centerPoint.x() > bounds.x() + bounds.width()) { -// currentCenterPoint.setX(bounds.x() + bounds.width()); -// } else if(centerPoint.x() < bounds.x()) { -// currentCenterPoint.setX(bounds.x()); -// } -// -// if(centerPoint.y() > bounds.y() + bounds.height()) { -// currentCenterPoint.setY(bounds.y() + bounds.height()); -// } else if(centerPoint.y() < bounds.y()) { -// currentCenterPoint.setY(bounds.y()); -// } -// -// } -// } -// -// //Update the scrollbars -// centerOn(currentCenterPoint); -//} -// -///** -// * Handles when the mouse button is pressed -// */ -//void HDDisplay::mousePressEvent(QMouseEvent* event) { -// //For panning the view -// lastPanPoint = event->pos(); -// setCursor(Qt::ClosedHandCursor); -//} -// -///** -// * Handles when the mouse button is released -// */ -//void HDDisplay::mouseReleaseEvent(QMouseEvent* event) { -// setCursor(Qt::OpenHandCursor); -// lastPanPoint = QPoint(); -//} -// -///** -//*Handles the mouse move event -//*/ -//void HDDisplay::mouseMoveEvent(QMouseEvent* event) { -// if(!lastPanPoint.isNull()) { -// //Get how much we panned -// QPointF delta = mapToScene(lastPanPoint) - mapToScene(event->pos()); -// lastPanPoint = event->pos(); -// -// //Update the center ie. do the pan -// setCenter(getCenter() + delta); -// } -//} -// -///** -// * Zoom the view in and out. -// */ -//void HDDisplay::wheelEvent(QWheelEvent* event) { -// -// //Get the position of the mouse before scaling, in scene coords -// QPointF pointBeforeScale(mapToScene(event->pos())); -// -// //Get the original screen centerpoint -// QPointF screenCenter = getCenter(); //CurrentCenterPoint; //(visRect.center()); -// -// //Scale the view ie. do the zoom -// double scaleFactor = 1.15; //How fast we zoom -// if(event->delta() > 0) { -// //Zoom in -// scale(scaleFactor, scaleFactor); -// } else { -// //Zooming out -// scale(1.0 / scaleFactor, 1.0 / scaleFactor); -// } -// -// //Get the position after scaling, in scene coords -// QPointF pointAfterScale(mapToScene(event->pos())); -// -// //Get the offset of how the screen moved -// QPointF offset = pointBeforeScale - pointAfterScale; -// -// //Adjust to the new center for correct zooming -// QPointF newCenter = screenCenter + offset; -// setCenter(newCenter); -//} -// -///** -// * Need to update the center so there is no jolt in the -// * interaction after resizing the widget. -// */ -//void HDDisplay::resizeEvent(QResizeEvent* event) { -// //Get the rectangle of the visible area in scene coords -// QRectF visibleArea = mapToScene(rect()).boundingRect(); -// setCenter(visibleArea.center()); -// -// //Call the subclass resize so the scrollbars are updated correctly -// QGraphicsView::resizeEvent(event); -//} diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h deleted file mode 100644 index 8f646250d39560826c8aec675ec1fd9a9e2ed873..0000000000000000000000000000000000000000 --- a/src/ui/HDDisplay.h +++ /dev/null @@ -1,207 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Definition of Head Down Display (HDD) - * - * @author Lorenz Meier - * - */ - -#ifndef HDDISPLAY_H -#define HDDISPLAY_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "UASInterface.h" -#include "Vehicle.h" - -namespace Ui -{ -class HDDisplay; -} - -/** - * @brief Head Down Display Widget - * - * This widget is used for any head down display as base widget. It handles the basic widget setup - * each head down instrument has a virtual screen size in millimeters as base coordinate system - * this virtual screen size is then scaled to pixels on the screen. - * When the pixel per millimeter ratio is known, a 1:1 representation is possible on the screen - */ -class HDDisplay : public QGraphicsView -{ - Q_OBJECT -public: - HDDisplay(const QStringList& plotList, QString title="", QWidget *parent = 0); - ~HDDisplay(); - -public slots: - /** @brief Update the HDD with new data */ - void updateValue(const int uasId, const QString& name, const QString& unit, const QVariant &value, const quint64 msec); - - /** @brief Connects a source to the updateValue() signals */ - void addSource(QObject* obj); - /** @brief Disconnects a source to the updateValue() signals */ - void removeSource(QObject* obj); - - /** @brief Removes a plot item by the action data */ - void removeItemByAction(); - /** @brief Bring up the menu to add a gauge */ - void addGauge(); - /** @brief Add a gauge using this spec string */ - void addGauge(const QString& gauge); - /** @brief Set the title of this widget and any existing parent dock widget */ - void setTitle(); - /** @brief Set the number of colums via popup */ - void setColumns(); - /** @brief Set the number of colums */ - void setColumns(int cols); - /** @brief Save the current layout and state to disk */ - void saveState(); - /** @brief Restore the last layout and state from disk */ - void restoreState(); - -protected slots: - void enableGLRendering(bool enable); - //void render(QPainter* painter, const QRectF& target = QRectF(), const QRect& source = QRect(), Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio); - void renderOverlay(); - void triggerUpdate(); - /** @brief Adjust the size hint for the current gauge layout */ - void adjustGaugeAspectRatio(); - -protected: - QSize sizeHint() const; - void changeEvent(QEvent* e); - void paintEvent(QPaintEvent* event); - void showEvent(QShowEvent* event); - void hideEvent(QHideEvent* event); - void contextMenuEvent(QContextMenuEvent* event); - QList getItemRemoveActions(); - void createActions(); - 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); - void drawEllipse(float refX, float refY, float radiusX, float radiusY, float lineWidth, const QColor& color, QPainter* painter); - void drawCircle(float refX, float refY, float radius, float lineWidth, const QColor& color, QPainter* painter); - - void drawChangeRateStrip(float xRef, float yRef, float height, float minRate, float maxRate, float value, QPainter* painter); - void drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid=true); - void drawGauge(float xRef, float yRef, float radius, float min, float max, const QString name, float value, const QColor& color, QPainter* painter, bool symmetric, QPair goodRange, QPair criticalRange, bool solid=true); - void drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter); - void paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter); - -// //Holds the current centerpoint for the view, used for panning and zooming -// QPointF currentCenterPoint; -// -// //From panning the view -// QPoint lastPanPoint; -// -// //Set the current centerpoint in the -// void setCenter(const QPointF& centerPoint); -// QPointF getCenter() { return currentCenterPoint; } -// -// //Take over the interaction -// virtual void mousePressEvent(QMouseEvent* event); -// virtual void mouseReleaseEvent(QMouseEvent* event); -// virtual void mouseMoveEvent(QMouseEvent* event); -// virtual void wheelEvent(QWheelEvent* event); -// virtual void resizeEvent(QResizeEvent* event); - - UASInterface* uas; ///< The uas currently monitored - QMap values; ///< The variables this HUD displays - QMap units; ///< The units - QMap valuesDot; ///< First derivative of the variable - QMap valuesMean; ///< Mean since system startup for this variable - QMap valuesCount; ///< Number of values received so far - QMap lastUpdate; ///< The last update time for this variable - QMap minValues; ///< The minimum value this variable is assumed to have - QMap maxValues; ///< The maximum value this variable is assumed to have - QMap symmetric; ///< Draw the gauge / dial symmetric bool = yes - QMap intValues; ///< Is the gauge value an integer? - QMap customNames; ///< Custom names for the data names - QMap > goodRanges; ///< The range of good values - QMap > critRanges; ///< The range of critical values - double scalingFactor; ///< Factor used to scale all absolute values to screen coordinates - float xCenterOffset, yCenterOffset; ///< Offset from center of window in mm coordinates - float vwidth; ///< Virtual width of this window, 200 mm per default. This allows to hardcode positions and aspect ratios. This virtual image plane is then scaled to the window size. - float vheight; ///< Virtual height of this window, 150 mm per default - - int xCenter; ///< Center of the HUD instrument in pixel coordinates. Allows to off-center the whole instrument in its OpenGL window, e.g. to fit another instrument - int yCenter; ///< Center of the HUD instrument in pixel coordinates. Allows to off-center the whole instrument in its OpenGL window, e.g. to fit another instrument - - // HUD colors - QColor backgroundColor; ///< Background color - QColor defaultColor; ///< Color for most HUD elements, e.g. pitch lines, center cross, change rate gauges - QColor setPointColor; ///< Color for the current control set point, e.g. yaw desired - QColor warningColor; ///< Color for warning messages - QColor criticalColor; ///< Color for caution messages - QColor infoColor; ///< Color for normal/default messages - QColor fuelColor; ///< Current color for the fuel message, can be info, warning or critical color - - // Blink rates - int warningBlinkRate; ///< Blink rate of warning messages, will be rounded to the refresh rate - - QTimer* refreshTimer; ///< The main timer, controls the update rate - static const int updateInterval = 300; ///< Update interval in milliseconds - QPainter* hudPainter; - QFont font; ///< The HUD font, per default the free Bitstream Vera SANS, which is very close to actual HUD fonts - QFontDatabase fontDatabase;///< Font database, only used to load the TrueType font file (the HUD font is directly loaded from file rather than from the system) - bool hardwareAcceleration; ///< Enable hardware acceleration - - float strongStrokeWidth; ///< Strong line stroke width, used throughout the HUD - float normalStrokeWidth; ///< Normal line stroke width, used throughout the HUD - float fineStrokeWidth; ///< Fine line stroke width, used throughout the HUD - - QStringList* acceptList; ///< Variable names to plot - QStringList* acceptUnitList; ///< Unit names to plot - - quint64 lastPaintTime; ///< Last time this widget was refreshed - int columns; ///< Number of instrument columns - - QAction* addGaugeAction; ///< Action adding a gauge - QAction* setTitleAction; ///< Action setting the title - QAction* setColumnsAction; ///< Action setting the number of columns - bool valuesChanged; - -private slots: - void _activeVehicleChanged(Vehicle* vehicle); - -private: - Ui::HDDisplay *m_ui; -}; - -#endif // HDDISPLAY_H diff --git a/src/ui/HDDisplay.ui b/src/ui/HDDisplay.ui deleted file mode 100644 index 72273ec74e97193dd5fd982364ff030f216609fb..0000000000000000000000000000000000000000 --- a/src/ui/HDDisplay.ui +++ /dev/null @@ -1,27 +0,0 @@ - - - HDDisplay - - - - 0 - 0 - 400 - 300 - - - - Form - - - - 0 - - - - - - - - - diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc deleted file mode 100644 index cce397a39a962c34986a2e0e14d804e5165448ed..0000000000000000000000000000000000000000 --- a/src/ui/HSIDisplay.cc +++ /dev/null @@ -1,1621 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Implementation of Horizontal Situation Indicator class - * - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include -#include -#include - -#include "MultiVehicleManager.h" -#include "HomePositionManager.h" -#include "HSIDisplay.h" -#include "QGC.h" -#include "MissionItem.h" -#include "UASWaypointManager.h" -#include -#include "MAV2DIcon.h" -#include "QGCApplication.h" -#include "UAS.h" - -HSIDisplay::HSIDisplay(QWidget *parent) : - HDDisplay(QStringList(), "HSI", parent), - dragStarted(false), - leftDragStarted(false), - mouseHasMoved(false), - startX(0.0f), - startY(0.0f), - actionPending(false), - directSending(false), - gpsSatellites(), - satellitesUsed(0), - attXSet(0.0f), - attYSet(0.0f), - attYawSet(0.0f), - altitudeSet(1.0f), - posXSet(0.0f), - posYSet(0.0f), - posZSet(0.0f), - attXSaturation(0.2f), - attYSaturation(0.2f), - attYawSaturation(0.5f), - posXSaturation(0.05f), - posYSaturation(0.05f), - altitudeSaturation(1.0f), - lat(0.0f), - lon(0.0f), - alt(0.0f), - globalAvailable(0), - x(0.0f), - y(0.0f), - z(0.0f), - vx(0.0f), - vy(0.0f), - vz(0.0f), - speed(0.0f), - localAvailable(0), - roll(0.0f), - pitch(0.0f), - 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(4.0), - crosstrackError(std::numeric_limits::quiet_NaN()), - xCenterPos(0), - yCenterPos(0), - positionLock(false), - attControlEnabled(false), - xyControlEnabled(false), - zControlEnabled(false), - yawControlEnabled(false), - positionFix(0), - gpsFix(0), - visionFix(0), - laserFix(0), - iruFix(0), - mavInitialized(false), - topMargin(20.0f), - bottomMargin(14.0f), - attControlKnown(false), - xyControlKnown(false), - zControlKnown(false), - yawControlKnown(false), - positionFixKnown(false), - visionFixKnown(false), - gpsFixKnown(false), - iruFixKnown(false), - gyroKnown(false), - gyroON(false), - gyroOK(false), - accelKnown(false), - accelON(false), - accelOK(false), - magKnown(false), - magON(false), - magOK(false), - pressureKnown(false), - pressureON(false), - pressureOK(false), - diffPressureKnown(false), - diffPressureON(false), - diffPressureOK(false), - flowKnown(false), - flowON(false), - flowOK(false), - laserKnown(false), - laserON(false), - laserOK(false), - viconKnown(false), - viconON(false), - viconOK(false), - actuatorsKnown(false), - actuatorsON(false), - actuatorsOK(false), - setPointKnown(false), - positionSetPointKnown(false), - userSetPointSet(false), - userXYSetPointSet(false), - userZSetPointSet(false), - userYawSetPointSet(false) -{ - refreshTimer->setInterval(updateInterval); - - columns = 1; - this->setAutoFillBackground(true); - QPalette pal = palette(); - pal.setColor(backgroundRole(), QGC::colorBlack); - setPalette(pal); - - vwidth = 80.0f; - vheight = 100.0f; - - xCenterPos = vwidth/2.0f; - yCenterPos = vheight/2.0f + topMargin - bottomMargin; - - uas = NULL; - resetMAVState(); - - // Do first update - setMetricWidth(metricWidth); - // Set tooltip - setToolTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget.")); - setStatusTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget.")); - - // XXX this looks a potential recursive issue - //connect(&statusClearTimer, SIGNAL(timeout()), this, SLOT(clearStatusMessage())); - - connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &HSIDisplay::_activeVehicleChanged); - - _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); - - setFocusPolicy(Qt::StrongFocus); -} - -HSIDisplay::~HSIDisplay() -{ - -} - -void HSIDisplay::resetMAVState() -{ - mavInitialized = false; - attControlKnown = false; - attControlEnabled = false; - xyControlKnown = false; - xyControlEnabled = false; - zControlKnown = false; - zControlEnabled = false; - yawControlKnown = false; - yawControlEnabled = false; - - // Draw position lock indicators - positionFixKnown = false; - positionFix = 0; - visionFixKnown = false; - visionFix = 0; - gpsFixKnown = false; - gpsFix = 0; - iruFixKnown = false; - iruFix = 0; - - // Data - setPointKnown = false; - localAvailable = 0; - globalAvailable = 0; - - // Setpoints - positionSetPointKnown = false; - setPointKnown = false; -} - -void HSIDisplay::paintEvent(QPaintEvent * event) -{ - Q_UNUSED(event); - //paintGL(); - // static quint64 interval = 0; - // //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; - // interval = MG::TIME::getGroundTimeNow(); - renderOverlay(); -} - -void HSIDisplay::renderOverlay() -{ - if (!isVisible() || !uas) return; -#if (QGC_EVENTLOOP_DEBUG) - qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; -#endif - // Center location of the HSI gauge items - - //float bottomMargin = 3.0f; - - // Size of the ring instrument - //const float margin = 0.1f; // 10% margin of total width on each side - float baseRadius = (vheight - topMargin - bottomMargin) / 2.0f - (topMargin + bottomMargin) / 2.8f; - - // Draw instruments - // TESTING THIS SHOULD BE MOVED INTO A QGRAPHICSVIEW - // Update scaling factor - // adjust scaling to fit both horizontally and vertically - scalingFactor = this->width()/vwidth; - double scalingFactorH = this->height()/vheight; - if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH; - - QPainter painter(viewport()); - painter.setRenderHint(QPainter::Antialiasing, true); - painter.setRenderHint(QPainter::HighQualityAntialiasing, true); - - // Draw base instrument - // ---------------------- - painter.setBrush(Qt::NoBrush); - - // Set the color scheme depending on the light/dark theme employed. - QColor ringColor; - QColor positionColor; - QColor setpointColor; - QColor labelColor; - QColor valueColor; - QColor statusColor; - QColor waypointLineColor; - QColor attitudeColor; - if (qgcApp()->styleIsDark()) - { - ringColor = QColor(255, 255, 255); - positionColor = QColor(20, 20, 200); - setpointColor = QColor(150, 250, 150); - labelColor = QGC::colorCyan; - valueColor = QColor(255, 255, 255); - statusColor = QGC::colorOrange; - waypointLineColor = QGC::colorYellow; - attitudeColor = QColor(200, 20, 20); - } - else - { - ringColor = QGC::colorBlack; - positionColor = QColor(20, 20, 200); - setpointColor = QColor(150, 250, 150); - labelColor = QColor(26, 75, 95); - valueColor = QColor(40, 40, 40); - statusColor = QGC::colorOrange; - waypointLineColor = QGC::colorDarkYellow; - attitudeColor = QColor(200, 20, 20); - } - - QPen pen; - pen.setColor(ringColor); - pen.setWidth(refLineWidthToPen(1.0f)); - painter.setPen(pen); - const int ringCount = 2; - for (int i = 0; i < ringCount; i++) - { - float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.35f * i+1) / 2.0f - bottomMargin / 2.0f; - drawCircle(xCenterPos, yCenterPos, radius, 1.0f, ringColor, &painter); - paintText(tr("%1 m").arg(refToMetric(radius), 5, 'f', 1, ' '), valueColor, 1.6f, vwidth/2-4, vheight/2+radius+7, &painter); - } - - // Draw orientation labels - // Translate and rotate coordinate frame - painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor); - const float yawDeg = ((yaw/M_PI)*180.0f); - int yawRotate = static_cast(yawDeg) % 360; - painter.rotate(-yawRotate); - 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 + 3.0f, - 1.25f, &painter); - paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter); - painter.rotate(+yawRotate); - painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor); - - // Draw trail - // QPointF m(bodyXSetCoordinate, bodyYSetCoordinate); - // // Transform from body to world coordinates - // m = metricWorldToBody(m); - // // Scale from metric body to screen reference units - // QPointF s = metricBodyToRef(m); - // drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, uas->getColor(), &painter); - - - // Draw center indicator - // QPolygonF p(3); - // p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f)); - // p.replace(1, QPointF(xCenterPos-4.0f, yCenterPos+3.5f)); - // p.replace(2, QPointF(xCenterPos+4.0f, yCenterPos+3.5f)); - // drawPolygon(p, &painter); - - if (uas) - { - // Translate to center - painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor); - QColor uasColor = uas->getColor(); - MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f); - //MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f); - // Translate back - painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor); - } - - // ---------------------- - - // Draw satellites - drawGPS(painter); - - // Draw state indicator - - // Draw position - drawPositionDirection(xCenterPos, yCenterPos, baseRadius, positionColor, &painter); - - // Draw attitude - drawAttitudeDirection(xCenterPos, yCenterPos, baseRadius, attitudeColor, &painter); - - - // Draw position setpoints in body coordinates - - float xSpDiff = uiXSetCoordinate - bodyXSetCoordinate; - float ySpDiff = uiYSetCoordinate - bodyYSetCoordinate; - float zSpDiff = uiZSetCoordinate - bodyZSetCoordinate; - - float setPointDist = sqrt(xSpDiff*xSpDiff + ySpDiff*ySpDiff + zSpDiff*zSpDiff); - - float angleDiff = uiYawSet - bodyYawSet; - - float normAngleDiff = fabs(atan2(sin(angleDiff), cos(angleDiff))); - - // Labels on outer part and bottom - - // Draw waypoints - drawWaypoints(painter); - - // Draw status flags - drawStatusFlag(1, 1, tr("RAT"), rateControlEnabled, rateControlKnown, painter); - drawStatusFlag(17, 1, tr("ATT"), attControlEnabled, attControlKnown, painter); - drawStatusFlag(33, 1, tr("PXY"), xyControlEnabled, xyControlKnown, painter); - drawStatusFlag(49, 1, tr("PZ"), zControlEnabled, zControlKnown, painter); - drawStatusFlag(65, 1, tr("YAW"), yawControlEnabled, yawControlKnown, painter); - - // Draw position lock indicators - drawPositionLock(1, 6, tr("POS"), positionFix, positionFixKnown, painter); - drawPositionLock(17, 6, tr("GPS"), gpsFix, gpsFixKnown, painter); - drawStatusFlag(33, 6, tr("FLO"), flowON, flowKnown, flowOK, painter); - drawPositionLock(49, 6, tr("VIS"), visionFix, visionFixKnown, painter); - drawPositionLock(65, 6, tr("IRU"), iruFix, iruFixKnown, painter); - - drawStatusFlag(1, 11, tr("GYR"), gyroON, gyroKnown, gyroOK, painter); - drawStatusFlag(17, 11, tr("ACC"), accelON, accelKnown, accelOK, painter); - drawStatusFlag(33, 11, tr("MAG"), magON, magKnown, magOK, painter); - drawStatusFlag(49, 11, tr("BAR"), pressureON, pressureKnown, pressureOK, painter); - drawStatusFlag(65, 11, tr("PIT"), diffPressureON, diffPressureKnown, diffPressureOK, painter); - - drawStatusFlag(1, 16, tr("ACT"), actuatorsON, actuatorsKnown, actuatorsOK, painter); - drawStatusFlag(17, 16, tr("LAS"), laserON, laserKnown, laserOK, painter); - drawStatusFlag(33, 16, tr("VCN"), viconON, viconKnown, viconOK, painter); - - // Draw speed to top left - paintText(tr("SPEED"), labelColor, 2.2f, 2, topMargin+2, &painter); - paintText(tr("%1 m/s").arg(speed, 5, 'f', 2, '0'), valueColor, 2.2f, 12, topMargin+2, &painter); - - // Draw crosstrack error to top right - paintText(tr("XTRACK"), labelColor, 2.2f, 54, topMargin+2, &painter); - if (!isnan(crosstrackError)) { - paintText(tr("%1 m").arg(crosstrackError, 5, 'f', 2, '0'), valueColor, 2.2f, 67, topMargin+2, &painter); - } else { - paintText(tr("-- m"), valueColor, 2.2f, 67, topMargin+2, &painter); - } - - - // Draw position to bottom left - if (localAvailable > 0) - { - // Position - QString str; - float offset = (globalAvailable > 0) ? -3.0f : 0.0f; - str.sprintf("%05.2f %05.2f %05.2f m", x, y, z); - paintText(tr("POS"), labelColor, 2.6f, 2, vheight - offset - 4.0f, &painter); - paintText(str, valueColor, 2.6f, 10, vheight - offset - 4.0f, &painter); - } - - if (globalAvailable > 0) - { - // Position - QString str; - str.sprintf("lat: %05.2f lon: %06.2f alt: %06.2f", lat, lon, alt); - paintText(tr("GPS"), labelColor, 2.6f, 2, vheight- 4.0f, &painter); - paintText(str, valueColor, 2.6f, 10, vheight - 4.0f, &painter); - } - - // Draw status message - paintText(statusMessage, statusColor, 2.8f, 8, 15, &painter); - - // Draw setpoint over waypoints - if (positionSetPointKnown || setPointKnown) - { - // Draw setpoint - drawSetpointXYZYaw(bodyXSetCoordinate, bodyYSetCoordinate, bodyZSetCoordinate, bodyYawSet, setpointColor, painter); - // Draw travel direction line - QPointF m(bodyXSetCoordinate, bodyYSetCoordinate); - // Transform from body to world coordinates - m = metricWorldToBody(m); - // Scale from metric body to screen reference units - QPointF s = metricBodyToRef(m); - drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, setpointColor, &painter); - } - - if ((userSetPointSet || dragStarted) && ((normAngleDiff > 0.05f) || !(setPointDist < 0.08f && mavInitialized))) - { - drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, setpointColor, painter); - } -} - -void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter) -{ - drawStatusFlag(x, y, label, status, known, true, painter); -} - -void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, bool ok, QPainter& painter) -{ - QColor statusColor; - QColor labelColor; - if (qgcApp()->styleIsDark()) - { - statusColor = QColor(250, 250, 250); - labelColor = QGC::colorCyan; - } - else - { - statusColor = QColor(40, 40, 40); - labelColor = QColor(26, 75, 95); - } - - // Draw the label. - paintText(label, labelColor, 2.6f, x, y+0.8f, &painter); - - // Determine color of status rectangle. - if (!ok) { - painter.setBrush(QGC::colorDarkYellow); - } else { - if(status) { - painter.setBrush(QGC::colorGreen); - } else { - painter.setBrush(Qt::darkGray); - } - } - painter.setPen(Qt::NoPen); - - // Draw the status rectangle. - float indicatorWidth = refToScreenX(7.0f); - float indicatorHeight = refToScreenY(4.0f); - painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), indicatorWidth, indicatorHeight)); - paintText((status) ? tr("ON") : tr("OFF"), statusColor, 2.6f, x+7.9f, y+0.8f, &painter); - - // Cross out instrument if state unknown - if (!known) - { - QPen pen(Qt::yellow); - pen.setWidth(3); - painter.setPen(pen); - // Top left to bottom right - QPointF p1, p2, p3, p4; - p1.setX(refToScreenX(x)); - p1.setY(refToScreenX(y)); - p2.setX(p1.x()+indicatorWidth+refToScreenX(7.3f)); - p2.setY(p1.y()+indicatorHeight); - painter.drawLine(p1, p2); - // Bottom left to top right - p3.setX(refToScreenX(x)); - p3.setY(refToScreenX(y)+indicatorHeight); - p4.setX(p1.x()+indicatorWidth+refToScreenX(7.3f)); - p4.setY(p1.y()); - painter.drawLine(p3, p4); - } -} - -void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter) -{ - // Select color scheme based on light or dark window theme. - QColor labelColor; - QColor negStatusColor(200, 20, 20); - QColor intermediateStatusColor (Qt::yellow); - QColor posStatusColor(20, 200, 20); - QColor statusColor; - if (qgcApp()->styleIsDark()) - { - statusColor = QColor(250, 250, 250); - labelColor = QGC::colorCyan; - } - else - { - statusColor = QColor(40, 40, 40); - labelColor = QColor(26, 75, 95); - } - - // Draw the label. - paintText(label, labelColor, 2.6f, x, y+0.8f, &painter); - - // based on the status, choose both the coloring and lock text. - QString lockText; - switch (status) { - case 1: - painter.setBrush(intermediateStatusColor.dark(150)); - lockText = tr("LOC"); - break; - case 2: - painter.setBrush(intermediateStatusColor.dark(150)); - lockText = tr("2D"); - break; - case 3: - painter.setBrush(posStatusColor); - lockText = tr("3D"); - break; - default: - painter.setBrush(negStatusColor); - lockText = tr("NO"); - break; - } - - float indicatorWidth = refToScreenX(7.0f); - float indicatorHeight = refToScreenY(4.0f); - - painter.setPen(Qt::NoPen); - painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), refToScreenX(7.0f), refToScreenY(4.0f))); - paintText(lockText, statusColor, 2.6f, x+7.9f, y+0.8f, &painter); - - // Cross out instrument if state unknown - if (!known) { - QPen pen(Qt::yellow); - pen.setWidth(3); - painter.setPen(pen); - // Top left to bottom right - QPointF p1, p2, p3, p4; - p1.setX(refToScreenX(x)); - p1.setY(refToScreenX(y)); - p2.setX(p1.x()+indicatorWidth+refToScreenX(7.3f)); - p2.setY(p1.y()+indicatorHeight); - painter.drawLine(p1, p2); - // Bottom left to top right - p3.setX(refToScreenX(x)); - p3.setY(refToScreenX(y)+indicatorHeight); - p4.setX(p1.x()+indicatorWidth+refToScreenX(7.3f)); - p4.setY(p1.y()); - painter.drawLine(p3, p4); - } -} - -void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock) -{ - Q_UNUSED(uas); - bool changed = positionLock != lock; - positionLock = lock; - if (changed) - { - update(); - } -} - -void HSIDisplay::updateAttitudeControllerEnabled(bool enabled) -{ - bool changed = attControlEnabled != enabled; - attControlEnabled = enabled; - attControlKnown = true; - if (changed) - { - update(); - } -} - -void HSIDisplay::updatePositionXYControllerEnabled(bool enabled) -{ - bool changed = xyControlEnabled != enabled; - xyControlEnabled = enabled; - xyControlKnown = true; - if (changed) - { - update(); - } -} - -void HSIDisplay::updatePositionZControllerEnabled(bool enabled) -{ - bool changed = zControlEnabled != enabled; - zControlEnabled = enabled; - zControlKnown = true; - if (changed) - { - update(); - } -} - -void HSIDisplay::updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance) -{ - Q_UNUSED(quality); - Q_UNUSED(name); - Q_UNUSED(type); - Q_UNUSED(id); - Q_UNUSED(time); - // FIXME add multi-object support - QPainter painter(this); - QColor color(Qt::yellow); - 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(cos(bearing)-sin(bearing)*distance, sin(bearing)+cos(bearing)*distance); - // Scale from metric to screen reference coordinates - QPointF p = metricBodyToRef(in); - drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter); -} - -QPointF HSIDisplay::metricWorldToBody(QPointF world) -{ - // First translate to body-centered coordinates - // Rotate around -yaw - 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; -} - -QPointF HSIDisplay::metricBodyToWorld(QPointF body) -{ - // First rotate into world coordinates - // then translate to world position - QPointF result((cos(-yaw) * body.x()) + (sin(-yaw) * body.y()) + x, (-sin(-yaw) * body.x()) + (cos(-yaw) * body.y()) + y); - return result; -} - -QPointF HSIDisplay::screenToMetricBody(QPointF ref) -{ - return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth); -} - -QPointF HSIDisplay::refToMetricBody(QPointF &ref) -{ - return QPointF(-((ref.y() - yCenterPos)/ vwidth) * metricWidth - x, ((ref.x() - xCenterPos) / vwidth) * metricWidth - y); -} - -/** - * @see refToScreenX() - */ -QPointF HSIDisplay::metricBodyToRef(QPointF &metric) -{ - return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos); -} - -double HSIDisplay::metricToRef(double metric) -{ - return (metric / metricWidth) * vwidth; -} - -double HSIDisplay::refToMetric(double ref) -{ - return (ref/vwidth) * metricWidth; -} - -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) -{ - if (event->type() == QMouseEvent::MouseButtonDblClick) - { - QPointF p = screenToMetricBody(event->localPos()); - if (!directSending) - { - setBodySetpointCoordinateXY(p.x(), p.y()); - if (!userZSetPointSet) setBodySetpointCoordinateZ(0.0); - } - // qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos; - } -} - -void HSIDisplay::mouseReleaseEvent(QMouseEvent * event) -{ - // FIXME hardcode yaw to current value - //setBodySetpointCoordinateYaw(0); - if (mouseHasMoved) - { - if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton) - { - if (dragStarted) - { - if (!directSending) setBodySetpointCoordinateYaw(uiYawSet); - dragStarted = false; - } - } - if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::LeftButton) - { - if (leftDragStarted) - { - if (!directSending) setBodySetpointCoordinateZ(uiZSetCoordinate); - leftDragStarted = false; - } - } - } - mouseHasMoved = false; -} - -void HSIDisplay::mousePressEvent(QMouseEvent * event) -{ - if (event->type() == QMouseEvent::MouseButtonPress) - { - if (event->button() == Qt::RightButton) - { - startX = event->x(); - // Start tracking mouse move - dragStarted = true; - } - else if (event->button() == Qt::LeftButton) - { - startY = event->y(); - leftDragStarted = true; - } - } - mouseHasMoved = false; -} - -void HSIDisplay::mouseMoveEvent(QMouseEvent * event) -{ - if (event->type() == QMouseEvent::MouseMove) - { - if (dragStarted) uiYawSet -= 0.06f*(startX - event->x()) / this->frameSize().width(); - - if (leftDragStarted) - { - // uiZSetCoordinate -= 0.06f*(startY - event->y()) / this->frameSize().height(); - // setBodySetpointCoordinateZ(uiZSetCoordinate); - } - - if (leftDragStarted || dragStarted) mouseHasMoved = true; - } -} - -void HSIDisplay::keyPressEvent(QKeyEvent* event) -{ - QPointF bodySP = metricWorldToBody(QPointF(uiXSetCoordinate, uiYSetCoordinate)); - - if ((event->key() == Qt::Key_Enter || event->key() == Qt::Key_Return) && actionPending) - { - actionPending = false; - statusMessage = "SETPOINT SENT"; - statusClearTimer.start(); - sendBodySetPointCoordinates(); - } - else - { - // FIXME hardcode yaw to current value - setBodySetpointCoordinateYaw(0); - - - // Reset setpoints to current position / orientation - // if not initialized - if (!userYawSetPointSet) - { - setBodySetpointCoordinateYaw(0); - } - - if (!userZSetPointSet) - { - setBodySetpointCoordinateZ(0); - } - - if (!userXYSetPointSet) - { - setBodySetpointCoordinateXY(0, 0); - } - - if ((event->key() == Qt::Key_W)) - { - setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()+0.2, +1.5), bodySP.y()); - } - - if ((event->key() == Qt::Key_S)) - { - setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y()); - } - - if ((event->key() == Qt::Key_A)) - { - setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()-0.2, +1.5)); - } - - if ((event->key() == Qt::Key_D)) - { - setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()+0.2, +1.5)); - } - - if ((event->key() == Qt::Key_Up)) - { - setBodySetpointCoordinateZ(-0.2); - } - - if ((event->key() == Qt::Key_Down)) - { - setBodySetpointCoordinateZ(+0.2); - } - - if ((event->key() == Qt::Key_Left)) - { - setBodySetpointCoordinateYaw(-0.2); - } - - if ((event->key() == Qt::Key_Right)) - { - setBodySetpointCoordinateYaw(0.2); - } - } - - - // Overwrite any existing non-zero body setpoints - // for escape - if ((event->key() == Qt::Key_Escape) || (event->key() == Qt::Key_R)) - { - setBodySetpointCoordinateXY(0, 0); - setBodySetpointCoordinateZ(0); - setBodySetpointCoordinateYaw(0); - statusMessage = "CANCELLED, PRESS TO SEND"; - } - - if ((event->key() == Qt::Key_T)) - { - directSending = !directSending; - statusMessage = (directSending) ? "DIRECT SEND ON" : "DIRECT SEND OFF"; - statusClearTimer.start(); - } - - if (actionPending && (directSending || (event->key() == Qt::Key_Escape))) - { - actionPending = false; - statusMessage = "SETPOINT SENT"; - statusClearTimer.start(); - sendBodySetPointCoordinates(); - } - - HDDisplay::keyPressEvent(event); -} - -void HSIDisplay::contextMenuEvent (QContextMenuEvent* event) -{ - event->ignore(); -} - -void HSIDisplay::setMetricWidth(double width) -{ - if (width != metricWidth) { - metricWidth = width; - emit metricWidthChanged(metricWidth); - } -} - -/** - * - * @param uas the UAS/MAV to monitor/display with the HUD - */ -void HSIDisplay::_activeVehicleChanged(Vehicle* vehicle) -{ - if (this->uas != NULL) { - disconnect(this->uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool))); - disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64))); - disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); - disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float))); - disconnect(this->uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - - disconnect(this->uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool))); - disconnect(this->uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(bool))); - disconnect(this->uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(bool))); - disconnect(this->uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(bool))); - - disconnect(this->uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int))); - disconnect(this->uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int))); - disconnect(this->uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int))); - disconnect(this->uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int))); - disconnect(this->uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float))); - - disconnect(this->uas, SIGNAL(gyroStatusChanged(bool,bool,bool)), this, SLOT(updateGyroStatus(bool,bool,bool))); - disconnect(this->uas, SIGNAL(accelStatusChanged(bool,bool,bool)), this, SLOT(updateAccelStatus(bool,bool,bool))); - disconnect(this->uas, SIGNAL(magSensorStatusChanged(bool,bool,bool)), this, SLOT(updateMagSensorStatus(bool,bool,bool))); - disconnect(this->uas, SIGNAL(baroStatusChanged(bool,bool,bool)), this, SLOT(updateBaroStatus(bool,bool,bool))); - disconnect(this->uas, SIGNAL(airspeedStatusChanged(bool,bool,bool)), this, SLOT(updateAirspeedStatus(bool,bool,bool))); - disconnect(this->uas, SIGNAL(opticalFlowStatusChanged(bool,bool,bool)), this, SLOT(updateOpticalFlowStatus(bool,bool,bool))); - disconnect(this->uas, SIGNAL(laserStatusChanged(bool,bool,bool)), this, SLOT(updateLaserStatus(bool,bool,bool))); - disconnect(this->uas, SIGNAL(groundTruthSensorStatusChanged(bool,bool,bool)), this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool))); - disconnect(this->uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)), this, SLOT(updateActuatorStatus(bool,bool,bool))); - disconnect(this->uas, &UASInterface::navigationControllerErrorsChanged, - this, &HSIDisplay::UpdateNavErrors); - } - - this->uas = NULL; - - if (vehicle) - { - this->uas = vehicle->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,double,quint64)), - this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)), - this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64))); - connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), - this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); - connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), - this, SLOT(updateUserPositionSetpoints(int,float,float,float,float))); - connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), - this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), - this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeControlEnabled(bool)), - this, SLOT(updateAttitudeControllerEnabled(bool))); - connect(uas, SIGNAL(positionXYControlEnabled(bool)), - this, SLOT(updatePositionXYControllerEnabled(bool))); - connect(uas, SIGNAL(positionZControlEnabled(bool)), - this, SLOT(updatePositionZControllerEnabled(bool))); - connect(uas, SIGNAL(positionYawControlEnabled(bool)), - this, SLOT(updatePositionYawControllerEnabled(bool))); - - connect(uas, SIGNAL(localizationChanged(UASInterface*,int)), - this, SLOT(updateLocalization(UASInterface*,int))); - connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), - this, SLOT(updateVisionLocalization(UASInterface*,int))); - connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), - this, SLOT(updateGpsLocalization(UASInterface*,int))); - connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), - this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int))); - connect(uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), - this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float))); - - connect(uas, SIGNAL(gyroStatusChanged(bool,bool,bool)), - this, SLOT(updateGyroStatus(bool,bool,bool))); - connect(uas, SIGNAL(accelStatusChanged(bool,bool,bool)), - this, SLOT(updateAccelStatus(bool,bool,bool))); - connect(uas, SIGNAL(magSensorStatusChanged(bool,bool,bool)), - this, SLOT(updateMagSensorStatus(bool,bool,bool))); - connect(uas, SIGNAL(baroStatusChanged(bool,bool,bool)), - this, SLOT(updateBaroStatus(bool,bool,bool))); - connect(uas, SIGNAL(airspeedStatusChanged(bool,bool,bool)), - this, SLOT(updateAirspeedStatus(bool,bool,bool))); - connect(uas, SIGNAL(opticalFlowStatusChanged(bool,bool,bool)), - this, SLOT(updateOpticalFlowStatus(bool,bool,bool))); - connect(uas, SIGNAL(laserStatusChanged(bool,bool,bool)), - this, SLOT(updateLaserStatus(bool,bool,bool))); - connect(uas, SIGNAL(groundTruthSensorStatusChanged(bool,bool,bool)), - this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool))); - connect(uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)), - this, SLOT(updateActuatorStatus(bool,bool,bool))); - connect(uas, &UASInterface::navigationControllerErrorsChanged, - this, &HSIDisplay::UpdateNavErrors); - statusClearTimer.start(3000); - } - else - { - statusClearTimer.stop(); - } - - resetMAVState(); -} - -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.0) + pow(vy, 2.0) + pow(vz, 2.0)); -} - -void HSIDisplay::setBodySetpointCoordinateXY(double x, double y) -{ - if (uas) - { - userSetPointSet = true; - userXYSetPointSet = true; - // Set coordinates and send them out to MAV - - QPointF sp(x, y); - sp = metricBodyToWorld(sp); - uiXSetCoordinate = sp.x(); - uiYSetCoordinate = sp.y(); - - qDebug() << "Attempting to set new setpoint at x: " << x << "metric y:" << y; - - - //sendBodySetPointCoordinates(); - statusMessage = "POSITION SET, PRESS TO SEND"; - actionPending = true; - statusClearTimer.start(); - } -} - -void HSIDisplay::setBodySetpointCoordinateZ(double z) -{ - if (uas) - { - userSetPointSet = true; - userZSetPointSet = true; - // Set coordinates and send them out to MAV - uiZSetCoordinate = z+uas->getLocalZ(); - statusMessage = "Z SET, PRESS TO SEND"; - actionPending = true; - statusClearTimer.start(); - } - //sendBodySetPointCoordinates(); -} - -void HSIDisplay::setBodySetpointCoordinateYaw(double yaw) -{ - if (uas) - { - if (!userXYSetPointSet && setPointKnown) - { - uiXSetCoordinate = bodyXSetCoordinate; - uiYSetCoordinate = bodyYSetCoordinate; - } - else if (!userXYSetPointSet && mavInitialized) - { - QPointF coord = metricBodyToWorld(QPointF(0.0, 0.0)); - uiXSetCoordinate = coord.x(); - uiYSetCoordinate = coord.y(); - } - userSetPointSet = true; - userYawSetPointSet = true; - // Set coordinates and send them out to MAV - uiYawSet = atan2(sin(yaw+uas->getYaw()), cos(yaw+uas->getYaw())); - statusMessage = "YAW SET, PRESS TO SEND"; - statusClearTimer.start(); - actionPending = true; - } - //sendBodySetPointCoordinates(); -} - -void HSIDisplay::sendBodySetPointCoordinates() -{ - // Send the coordinates to the MAV - if (uas) - { - uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet); - } -} - -void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec) -{ - Q_UNUSED(uas); - Q_UNUSED(usec); - attXSet = pitchDesired; - attYSet = rollDesired; - attYawSet = yawDesired; - altitudeSet = thrustDesired; -} - -void HSIDisplay::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time) -{ - Q_UNUSED(uas); - Q_UNUSED(time); - this->roll = roll; - this->pitch = pitch; - this->yaw = yaw; -} - -void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired) -{ - Q_UNUSED(uasid); - uiXSetCoordinate = xDesired; - uiYSetCoordinate = yDesired; - uiZSetCoordinate = zDesired; - uiYawSet = yawDesired; - userXYSetPointSet = true; - userZSetPointSet = true; - userYawSetPointSet = true; - userSetPointSet = true; -} - -void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec) -{ - Q_UNUSED(uasid); - Q_UNUSED(usec); - bodyXSetCoordinate = xDesired; - bodyYSetCoordinate = yDesired; - bodyZSetCoordinate = zDesired; - bodyYawSet = yawDesired; - mavInitialized = true; - setPointKnown = true; - positionSetPointKnown = true; - - if (!userSetPointSet && !dragStarted) - { - uiXSetCoordinate = bodyXSetCoordinate; - uiYSetCoordinate = bodyYSetCoordinate; - // uiZSetCoordinate = bodyZSetCoordinate; - uiYawSet= bodyYawSet; - } -} - -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::UpdateNavErrors(UASInterface *uas, double altitudeError, double airspeedError, double crosstrackError) -{ - Q_UNUSED(altitudeError); - Q_UNUSED(airspeedError); - if (this->uas == uas) { - this->crosstrackError = crosstrackError; - } -} - -void HSIDisplay::updateGlobalPosition(UASInterface*, double lat, double lon, double altAMSL, double altWGS84, quint64 usec) -{ - Q_UNUSED(altAMSL); - this->lat = lat; - this->lon = lon; - this->alt = altWGS84; - globalAvailable = usec; -} - -void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float azimuth, float snr, bool used) -{ - Q_UNUSED(uasid); - // If slot is empty, insert object - if (satid != 0) { // Satellite PRNs currently range from 1-32, but are never zero - if (gpsSatellites.contains(satid)) { - gpsSatellites.value(satid)->update(satid, elevation, azimuth, snr, used); - } else { - gpsSatellites.insert(satid, new GPSSatellite(satid, elevation, azimuth, snr, used)); - } - } -} - -void HSIDisplay::updatePositionYawControllerEnabled(bool enabled) -{ - yawControlEnabled = enabled; - yawControlKnown = true; -} - -/** - * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization - */ -void HSIDisplay::updateLocalization(UASInterface* uas, int fix) -{ - Q_UNUSED(uas); - positionFix = fix; - positionFixKnown = true; - //qDebug() << "LOCALIZATION FIX CALLED"; -} -/** - * @param fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D localization, 3: 3D localization - */ -void HSIDisplay::updateGpsLocalization(UASInterface* uas, int fix) -{ - Q_UNUSED(uas); - gpsFix = fix; - gpsFixKnown = true; -} -/** - * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization - */ -void HSIDisplay::updateVisionLocalization(UASInterface* uas, int fix) -{ - Q_UNUSED(uas); - visionFix = fix; - visionFixKnown = true; - //qDebug() << "VISION FIX GOT CALLED"; -} - -/** - * @param fix 0: lost, 1-N: Localized with N ultrasound or infrared sensors - */ -void HSIDisplay::updateInfraredUltrasoundLocalization(UASInterface* uas, int fix) -{ - Q_UNUSED(uas); - iruFix = fix; - iruFixKnown = true; -} - -QColor HSIDisplay::getColorForSNR(float snr) -{ - QColor color; - if (snr > 0 && snr < 30) { - color = QColor(250, 10, 10); - } else if (snr >= 30 && snr < 35) { - color = QColor(230, 230, 10); - } else if (snr >= 35 && snr < 40) { - color = QColor(90, 200, 90); - } else if (snr >= 40) { - color = QColor(20, 200, 20); - } else { - color = QColor(180, 180, 180); - } - return color; -} - -void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter) -{ - if (uas) - { - float radius = vwidth / 18.0f; - QPen pen(color); - pen.setWidthF(refLineWidthToPen(1.6f)); - pen.setColor(color); - painter.setPen(pen); - painter.setBrush(Qt::NoBrush); - QPointF in(x, y); - // Transform from body to world coordinates - in = metricWorldToBody(in); - // Scale from metric to screen reference coordinates - QPointF p = metricBodyToRef(in); - - QPolygonF poly(4); - // Top point - poly.replace(0, QPointF(p.x()-radius, p.y()-radius)); - // Right point - poly.replace(1, QPointF(p.x()+radius, p.y()-radius)); - // Bottom point - poly.replace(2, QPointF(p.x()+radius, p.y()+radius)); - poly.replace(3, QPointF(p.x()-radius, p.y()+radius)); - - // Label - paintText(QString("z: %1").arg(z, 3, 'f', 1, QChar(' ')), color, 2.1f, p.x()-radius, p.y()-radius-3.5f, &painter); - - drawPolygon(poly, &painter); - - radius *= 0.8f; - drawLine(p.x(), p.y(), p.x()+sin(-yaw + uas->getYaw() + M_PI) * radius, p.y()+cos(-yaw + uas->getYaw() + M_PI) * radius, refLineWidthToPen(0.6f), color, &painter); - - // Draw center dot - painter.setBrush(color); - drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter); - } -} - -void HSIDisplay::drawWaypoint(QPainter& painter, const QColor& color, float width, const MissionItem *w, const QPointF& p) -{ - painter.setBrush(Qt::NoBrush); - - // Setup pen for foreground - QPen pen(color); - pen.setWidthF(width); - - // DRAW WAYPOINT - 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())); - - float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f)); - float acceptRadius = w->getAcceptanceRadius(); - double yawDiff = w->yawRadians()/180.0*M_PI-yaw; - - // Draw background - pen.setColor(Qt::black); - painter.setPen(pen); - drawLine(p.x(), p.y(), p.x()+sin(yawDiff) * radius, p.y()-cos(yawDiff) * radius, refLineWidthToPen(0.4f*3.0f), Qt::black, &painter); - drawPolygon(poly, &painter); - drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 3.0, Qt::black, &painter); - - // Draw foreground - pen.setColor(color); - painter.setPen(pen); - drawLine(p.x(), p.y(), p.x()+sin(yawDiff) * radius, p.y()-cos(yawDiff) * radius, refLineWidthToPen(0.4f), color, &painter); - drawPolygon(poly, &painter); - drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter); -} - -void HSIDisplay::drawWaypoints(QPainter& painter) -{ - if (uas) - { - // Grab all waypoints. - const QList& list = uas->getWaypointManager()->getWaypointEditableList(); - const int numWaypoints = list.size(); - - // Do not work on empty lists - if (list.size() == 0) - { - return; - } - - // Make sure any drawn shapes are not filled-in. - painter.setBrush(Qt::NoBrush); - - QPointF lastWaypoint; - for (int i = 0; i < numWaypoints; i++) - { - const MissionItem *w = list.at(i); - QPointF in; - // Use local coordinates as-is. - int frameRef = w->getFrame(); - if (frameRef == MAV_FRAME_LOCAL_NED) - { - in = QPointF(w->x(), w->y()); - } - else if (frameRef == MAV_FRAME_LOCAL_ENU) - { - in = QPointF(w->y(), w->x()); - } - // Convert global coordinates into the local ENU frame, then display them. - else if (frameRef == MAV_FRAME_GLOBAL || frameRef == MAV_FRAME_GLOBAL_RELATIVE_ALT) { - // Get the position of the GPS origin for the MAV. - - // Transform the lat/lon for this waypoint into the local frame - double e, n, u; - HomePositionManager::instance()->wgs84ToEnu(w->x(), w->y(),w->z(), &e, &n, &u); - in = QPointF(n, e); - } - // Otherwise we don't process this waypoint. - // FIXME: This code will probably fail if the last waypoint found is not a valid one. - else { - continue; - } - - // Transform from world to body coordinates - in = metricWorldToBody(in); - // Scale from metric to screen reference coordinates - QPointF p = metricBodyToRef(in); - - // Select color based on if this is the current waypoint. - if (w->isCurrentItem()) - { - drawWaypoint(painter, QGC::colorYellow, refLineWidthToPen(0.8f), w, p); - } - else - { - drawWaypoint(painter, QGC::colorCyan, refLineWidthToPen(0.4f), w, p); - } - - // Draw connecting line from last waypoint to this one. - if (!lastWaypoint.isNull()) - { - drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f*2.0f), Qt::black, &painter); - drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f), QGC::colorCyan, &painter); - } - lastWaypoint = p; - } - } -} - -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); - pen.setBrush(Qt::NoBrush); - painter.setPen(pen); - painter.drawRect(QRectF(metricBodyToScreen(metricWorldToBody(topLeft)), metricBodyToScreen(metricWorldToBody(bottomRight)))); -} - -void HSIDisplay::drawGPS(QPainter &painter) -{ - float xCenter = xCenterPos; - float yCenter = yCenterPos; - - const float yawDeg = ((yaw/M_PI)*180.0f); - int yawRotate = static_cast(yawDeg) % 360; - // XXX check rotation direction - - // Max satellite circle radius - const float margin = 0.15f; // 20% margin of total width on each side - float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f; - quint64 currTime = MG::TIME::getGroundTimeNowUsecs(); - - // Draw satellite labels - // QString label; - // label.sprintf("%05.1f", value); - // paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter); - - QMapIterator i(gpsSatellites); - while (i.hasNext()) { - i.next(); - GPSSatellite* sat = i.value(); - - // Check if update is not older than 10 seconds, else delete satellite - if (sat->lastUpdate + 10000000 < currTime) { - // Delete and go to next satellite - gpsSatellites.remove(i.key()); - if (i.hasNext()) { - i.next(); - sat = i.value(); - } else { - continue; - } - } - - if (sat) { - // Draw satellite - QBrush brush; - QColor color = getColorForSNR(sat->snr); - brush.setColor(color); - if (sat->used) { - brush.setStyle(Qt::SolidPattern); - } else { - brush.setStyle(Qt::NoBrush); - } - painter.setPen(Qt::SolidLine); - painter.setPen(color); - painter.setBrush(brush); - - float xPos = xCenter + (sin(((sat->azimuth/255.0f)*360.0f-yawRotate)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius; - float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f-yawRotate)/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(QPainter &painter) -{ - Q_UNUSED(painter); -} - -void HSIDisplay::drawPositionDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) -{ - if (xyControlKnown && xyControlEnabled) { - // Draw the needle - const float maxWidth = radius / 5.0f; - const float minWidth = maxWidth * 0.3f; - - float angle = atan2(posXSet, -posYSet); - angle -= (float)M_PI/2.0f; - - QPolygonF p(6); - - //radius *= ((posXSaturation + posYSaturation) - sqrt(pow(posXSet, 2), pow(posYSet, 2))) / (2*posXSaturation); - - radius *= sqrt(pow(posXSet, 2) + pow(posYSet, 2)) / sqrt(posXSaturation + posYSaturation); - - p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); - 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.4f)); - p.replace(4, QPointF(xRef, yRef-radius * 0.36f)); - p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); - - 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); - - //qDebug() << "DRAWING POS SETPOINT X:" << posXSet << "Y:" << posYSet << angle; - } -} - -void HSIDisplay::drawAttitudeDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) -{ - if (attControlKnown && attControlEnabled) { - // Draw the needle - const float maxWidth = radius / 10.0f; - const float minWidth = maxWidth * 0.3f; - - float angle = atan2(attYSet, -attXSet); - - radius *= sqrt(attXSet*attXSet + attYSet*attYSet) / sqrt(attXSaturation*attXSaturation + attYSaturation*attYSaturation); - - QPolygonF p(6); - - p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); - 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.4f)); - p.replace(4, QPointF(xRef, yRef-radius * 0.36f)); - p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); - - 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); - - // TODO Draw Yaw indicator - - //qDebug() << "DRAWING ATT SETPOINT X:" << attXSet << "Y:" << attYSet << angle; - } -} - -void HSIDisplay::drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) -{ - if (zControlKnown && zControlEnabled) { - // 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); - } -} - -void HSIDisplay::wheelEvent(QWheelEvent* event) -{ - double zoomScale = 0.005; // Scaling of zoom value - if(event->delta() > 0) { - // Reduce width -> Zoom in - metricWidth -= event->delta() * zoomScale; - } else { - // Increase width -> Zoom out - metricWidth -= event->delta() * zoomScale; - } - metricWidth = qBound(0.5, metricWidth, 9999.0); - emit metricWidthChanged(metricWidth); -} - -void HSIDisplay::showEvent(QShowEvent* event) -{ - // React only to internal (pre-display) - // events - Q_UNUSED(event) { - refreshTimer->start(updateInterval); - } -} - -void HSIDisplay::hideEvent(QHideEvent* event) -{ - // React only to internal (post-display) - // events - Q_UNUSED(event) { - refreshTimer->stop(); - } -} - -void HSIDisplay::updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat) -{ - Q_UNUSED(roll); - Q_UNUSED(pitch); - Q_UNUSED(yaw); - Q_UNUSED(thrust); - Q_UNUSED(xHat); - Q_UNUSED(yHat); -} - -void HSIDisplay::pressKey(int key) -{ - Q_UNUSED(key); -} - - diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h deleted file mode 100644 index c809c9e77107b0417a942e9f3e61fb01ea44e07f..0000000000000000000000000000000000000000 --- a/src/ui/HSIDisplay.h +++ /dev/null @@ -1,429 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Definition of of Horizontal Situation Indicator class - * - * @author Lorenz Meier - * - */ - -#ifndef HSIDISPLAY_H -#define HSIDISPLAY_H - -#include -#include -#include -#include -#include -#include -#include - -#include "HDDisplay.h" -#include "MG.h" -#include "Vehicle.h" - -class HSIDisplay : public HDDisplay -{ - Q_OBJECT -public: - HSIDisplay(QWidget *parent = 0); - ~HSIDisplay(); - -public slots: - /** @brief Set the width in meters this widget shows from top */ - void setMetricWidth(double width); - void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used); - void updateAttitudeSetpoints(UASInterface*, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec); - void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time); - void updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired); - 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 altAMSL, double altWGS84, quint64 usec); - void updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time); - void UpdateNavErrors(UASInterface *uas, double altitudeError, double airspeedError, double crosstrackError); - void updatePositionLock(UASInterface* uas, bool lock); - void updateAttitudeControllerEnabled(bool enabled); - void updatePositionXYControllerEnabled(bool enabled); - void updatePositionZControllerEnabled(bool enabled); - - /** @brief Optical flow status changed */ - void updateOpticalFlowStatus(bool supported, bool enabled, bool ok) { - if (supported && enabled && ok) { - visionFix = true; - } else { - visionFix = false; - } - } - - /** @brief Vision based localization status changed */ - void updateVisionLocalizationStatus(bool supported, bool enabled, bool ok) { - if (enabled && ok) { - visionFix = true; - } else { - visionFix = false; - } - visionFixKnown = supported; - } - /** @brief Infrared / Ultrasound status changed */ - void updateDistanceSensorStatus(bool supported, bool enabled, bool ok) { - if (enabled && ok) { - iruFix = true; - } else { - iruFix = false; - } - iruFixKnown = supported; - } - /** @brief Gyroscope status changed */ - void updateGyroStatus(bool supported, bool enabled, bool ok) { - gyroKnown = supported; - gyroON = enabled; - gyroOK = ok; - } - /** @brief Accelerometer status changed */ - void updateAccelStatus(bool supported, bool enabled, bool ok) { - accelKnown = supported; - accelON = enabled; - accelOK = ok; - } - /** @brief Magnetometer status changed */ - void updateMagSensorStatus(bool supported, bool enabled, bool ok) { - magKnown = supported; - magON = enabled; - magOK = ok; - } - /** @brief Barometer status changed */ - void updateBaroStatus(bool supported, bool enabled, bool ok) { - pressureKnown = supported; - pressureON = enabled; - pressureOK = ok; - } - /** @brief Differential pressure / airspeed status changed */ - void updateAirspeedStatus(bool supported, bool enabled, bool ok) { - diffPressureKnown = supported; - diffPressureON = enabled; - diffPressureOK = ok; - } - /** @brief Actuator status changed */ - void updateActuatorStatus(bool supported, bool enabled, bool ok) { - actuatorsKnown = supported; - actuatorsON = enabled; - actuatorsOK = ok; - } - /** @brief Laser scanner status changed */ - void updateLaserStatus(bool supported, bool enabled, bool ok) { - laserKnown = supported; - laserON = enabled; - laserOK = ok; - } - /** @brief Vicon / Leica Geotracker status changed */ - void updateGroundTruthSensorStatus(bool supported, bool enabled, bool ok) { - viconKnown = supported; - viconON = enabled; - viconOK = ok; - } - - void updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance); - /** @brief Heading control enabled/disabled */ - void updatePositionYawControllerEnabled(bool enabled); - - /** @brief Localization quality changed */ - void updateLocalization(UASInterface* uas, int localization); - /** @brief GPS localization quality changed */ - void updateGpsLocalization(UASInterface* uas, int localization); - /** @brief Vision localization quality changed */ - void updateVisionLocalization(UASInterface* uas, int localization); - - /** @brief Ultrasound/Infrared localization changed */ - void updateInfraredUltrasoundLocalization(UASInterface* uas, int fix); - - /** @brief Repaint the widget */ - 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); - /** @brief Reset the state of the view */ - void resetMAVState(); - /** @brief Clear the status message */ - void clearStatusMessage() - { - statusMessage = ""; - if (actionPending) statusMessage = "TIMED OUT, NO ACTION"; - statusClearTimer.start(); - userSetPointSet = false; - actionPending = false; - } - -signals: - void metricWidthChanged(double width); - -protected slots: - void renderOverlay(); - 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); - /** @brief Draw a status flag indicator */ - void drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter); - void drawStatusFlag(float x, float y, QString label, bool status, bool known, bool ok, QPainter& painter); - /** @brief Draw a position lock indicator */ - void drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter); - void setBodySetpointCoordinateXY(double x, double y); - void setBodySetpointCoordinateYaw(double yaw); - void setBodySetpointCoordinateZ(double z); - /** @brief Send the current ui setpoint coordinates as new setpoint to the MAV */ - void sendBodySetPointCoordinates(); - /** @brief Draw one setpoint */ - void drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter); - /** @brief Draw waypoints of this system */ - void drawWaypoints(QPainter& painter); - /** @brief Draw one waypoint */ - void drawWaypoint(QPainter& painter, const QColor& color, float width, const MissionItem *w, const QPointF& p); - /** @brief Draw the limiting safety area */ - void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter); - /** @brief Receive mouse clicks */ - void mouseDoubleClickEvent(QMouseEvent* event); - void mousePressEvent(QMouseEvent * event); - void mouseReleaseEvent(QMouseEvent * event); - void mouseMoveEvent(QMouseEvent * event); - /** @brief Receive mouse wheel events */ - void wheelEvent(QWheelEvent* event); - /** @brief Read out send keys */ - void keyPressEvent(QKeyEvent* event); - /** @brief Ignore context menu event */ - void contextMenuEvent (QContextMenuEvent* event); - /** @brief Set status message on screen */ - void setStatusMessage(const QString& message) - { - statusMessage = message; - statusClearTimer.start(); - } - -protected: - - void showEvent(QShowEvent* event); - void hideEvent(QHideEvent* event); - /** @brief Get color from GPS signal-to-noise colormap */ - static QColor getColorForSNR(float snr); - /** @brief Metric world coordinates to metric body coordinates */ - QPointF metricWorldToBody(QPointF world); - /** @brief Metric body coordinates to metric world coordinates */ - QPointF metricBodyToWorld(QPointF body); - /** @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 metricBodyToRef(QPointF &metric); - /** @brief Metric length to reference coordinates */ - double metricToRef(double metric); - /** @bried Reference coordinates to metric length */ - double refToMetric(double ref); - /** @brief Metric body coordinates to screen coordinates */ - QPointF metricBodyToScreen(QPointF metric); - - /** - * @brief Private data container class to be used within the HSI widget - */ - class GPSSatellite - { - public: - GPSSatellite(int id, float elevation, float azimuth, float snr, bool used) : - id(id), - elevation(elevation), - azimuth(azimuth), - snr(snr), - used(used), - lastUpdate(MG::TIME::getGroundTimeNowUsecs()) { - - } - - void update(int id, float elevation, float azimuth, float snr, bool used) { - this->id = id; - this->elevation = elevation; - this->azimuth = azimuth; - this->snr = snr; - this->used = used; - this->lastUpdate = MG::TIME::getGroundTimeNowUsecs(); - } - - int id; - float elevation; - float azimuth; - float snr; - bool used; - quint64 lastUpdate; - - friend class HSIDisplay; - }; - - QMap objectNames; - QMap objectTypes; - QMap objectQualities; - QMap objectBearings; - QMap objectDistances; - bool dragStarted; - bool leftDragStarted; - bool mouseHasMoved; - float startX; - float startY; - QTimer statusClearTimer; - QString statusMessage; - bool actionPending; - bool directSending; - - 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; - 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 - double metricWidth; ///< Width the instrument represents in meters (the width of the ground shown by the widget) - - // Navigation parameters - double crosstrackError; ///< The crosstrack error (m) reported by the UAS - - // - float xCenterPos; ///< X center of instrument in virtual coordinates - float yCenterPos; ///< Y center of instrument in virtual coordinates - - bool positionLock; - bool rateControlEnabled; ///< Rate control enabled - bool attControlEnabled; ///< Attitude control enabled - bool xyControlEnabled; ///< Horizontal control enabled - bool zControlEnabled; ///< Vertical control enabled - bool yawControlEnabled; ///< Yaw angle position control enabled - int positionFix; ///< Total dimensions the MAV is localizaed in - int gpsFix; ///< Localization dimensions based on GPS - int visionFix; ///< Localizaiton dimensions based on computer vision - int laserFix; ///< Localization dimensions based on laser - int iruFix; ///< Localization dimensions based on ultrasound - - bool mavInitialized; ///< The MAV is initialized once the setpoint has been received - float topMargin; ///< Margin on top of the page, in virtual coordinates - float bottomMargin; ///< Margin on the bottom of the page, in virtual coordinates - - bool rateControlKnown; ///< Rate control status known flag - bool attControlKnown; ///< Attitude control status known flag - bool xyControlKnown; ///< XY control status known flag - bool zControlKnown; ///< Z control status known flag - bool yawControlKnown; ///< Yaw control status known flag - - // Position lock indicators - bool positionFixKnown; ///< Position fix status known flag - bool visionFixKnown; ///< Vision fix status known flag - bool gpsFixKnown; ///< GPS fix status known flag - bool iruFixKnown; ///< Infrared/Ultrasound fix status known flag - - // System state indicators - bool gyroKnown; - bool gyroON; - bool gyroOK; - - bool accelKnown; - bool accelON; - bool accelOK; - - bool magKnown; - bool magON; - bool magOK; - - bool pressureKnown; - bool pressureON; - bool pressureOK; - - bool diffPressureKnown; - bool diffPressureON; - bool diffPressureOK; - - bool flowKnown; - bool flowON; - bool flowOK; - - bool laserKnown; - bool laserON; - bool laserOK; - - bool viconKnown; - bool viconON; - bool viconOK; - - bool actuatorsKnown; - bool actuatorsON; - bool actuatorsOK; - - // Data indicators - bool setPointKnown; ///< Controller setpoint known status flag - bool positionSetPointKnown; ///< Position setpoint known status flag - bool userSetPointSet; ///< User set X, Y and Z - bool userXYSetPointSet; ///< User set the X/Y position already - bool userZSetPointSet; ///< User set the Z position already - bool userYawSetPointSet; ///< User set the YAW position already - -private slots: - void _activeVehicleChanged(Vehicle* vehicle); -}; - -#endif // HSIDISPLAY_H diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc deleted file mode 100644 index 3eaefdc7da8bfb445a6cc2924f3012df3051f8b9..0000000000000000000000000000000000000000 --- a/src/ui/HUD.cc +++ /dev/null @@ -1,1405 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Head Up Display (HUD) - * - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "UAS.h" -#include "HUD.h" -#include "QGC.h" -#include "QGCApplication.h" -#include "QGCFileDialog.h" -#include "MultiVehicleManager.h" - -/** - * @warning The HUD widget will not start painting its content automatically - * to update the view, start the auto-update by calling HUD::start(). - * - * @param width - * @param height - * @param parent - */ -HUD::HUD(int width, int height, QWidget* parent) - : QLabel(parent), - image(NULL), - uas(NULL), - yawInt(0.0f), - mode(tr("UNKNOWN MODE")), - state(tr("UNKNOWN STATE")), - fuelStatus(tr("00.0V (00m:00s)")), - xCenterOffset(0.0f), - yCenterOffset(0.0f), - vwidth(200.0f), - vheight(150.0f), - vGaugeSpacing(65.0f), - vPitchPerDeg(6.0f), ///< 4 mm y translation per degree) - rawBuffer1(NULL), - rawBuffer2(NULL), - rawImage(NULL), - rawLastIndex(0), - rawExpectedBytes(0), - bytesPerLine(1), - imageStarted(false), - receivedDepth(8), - receivedChannels(1), - receivedWidth(640), - receivedHeight(480), - warningBlinkRate(5), - refreshTimer(new QTimer(this)), - noCamera(true), - hardwareAcceleration(true), - strongStrokeWidth(1.5f), - normalStrokeWidth(1.0f), - fineStrokeWidth(0.5f), - waypointName(""), - roll(0.0f), - pitch(0.0f), - yaw(0.0f), - rollLP(0.0f), - pitchLP(0.0f), - yawLP(0.0f), - yawDiff(0.0f), - xPos(0.0), - yPos(0.0), - zPos(0.0), - xSpeed(0.0), - ySpeed(0.0), - zSpeed(0.0), - lastSpeedUpdate(0), - totalSpeed(0.0), - totalAcc(0.0), - lat(0.0), - lon(0.0), - alt(0.0), - load(0.0f), - offlineDirectory(""), - nextOfflineImage(""), - HUDInstrumentsEnabled(false), - videoEnabled(true), - imageLoggingEnabled(false), - xImageFactor(1.0), - yImageFactor(1.0), - imageRequested(false) -{ - Q_UNUSED(width); - Q_UNUSED(height); - - // Set auto fill to false - setAutoFillBackground(false); - - // Set minimum size - setMinimumSize(80, 60); - // Set preferred size - setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - scalingFactor = this->width()/vwidth; - - // Set up the initial color theme. This can be updated by a styleChanged - // signal from MainWindow. - styleChanged(qgcApp()->styleIsDark()); - - // Refresh timer - refreshTimer->setInterval(updateInterval); - connect(refreshTimer, SIGNAL(timeout()), this, SLOT(repaint())); - - // Resize to correct size and fill with image - QWidget::resize(this->width(), this->height()); - - fontDatabase = QFontDatabase(); - const QString fontFileName = ":/res/fonts/vera.ttf"; ///< Font file is part of the QRC file and compiled into the app - const QString fontFamilyName = "Bitstream Vera Sans"; - if(!QFile::exists(fontFileName)) qDebug() << "ERROR! font file: " << fontFileName << " DOES NOT EXIST!"; - - fontDatabase.addApplicationFont(fontFileName); - font = fontDatabase.font(fontFamilyName, "Roman", qMax(5,(int)(10.0f*scalingFactor*1.2f+0.5f))); - QFont* fontPtr = &font; - if (!fontPtr) { - qDebug() << "ERROR! FONT NOT LOADED!"; - } else { - if (font.family() != fontFamilyName) qDebug() << "ERROR! WRONG FONT LOADED: " << fontFamilyName; - } - - // Connect the themeChanged signal from the MainWindow to this widget, so it - // can change it's styling accordingly. - connect(qgcApp(), &QGCApplication::styleChanged, this, &HUD::styleChanged); - - // Connect with UAS - connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &HUD::_activeVehicleChanged); - - createActions(); - - _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); -} - -HUD::~HUD() -{ - refreshTimer->stop(); -} - -QSize HUD::sizeHint() const -{ - return QSize(width(), (width()*3.0f)/4); -} - -void HUD::styleChanged(bool styleIsDark) -{ - // Generate a background image that's dependent on the current color scheme. - QImage fill = QImage(width(), height(), QImage::Format_Indexed8); - fill.fill(styleIsDark ? 0 : 255); - glImage = QGLWidget::convertToGLFormat(fill); - - // Now set the other default colors based on the current color scheme. - if (styleIsDark) - { - defaultColor = QColor(70, 200, 70); - setPointColor = QColor(200, 20, 200); - warningColor = Qt::yellow; - criticalColor = Qt::red; - infoColor = QColor(20, 200, 20); - fuelColor = criticalColor; - } - else - { - defaultColor = QColor(0x01, 0x47, 0x01); - setPointColor = QColor(0x82, 0x17, 0x82); - warningColor = Qt::darkYellow; - criticalColor = Qt::darkRed; - infoColor = QColor(0x07, 0x82, 0x07); - fuelColor = criticalColor; - } -} - -void HUD::showEvent(QShowEvent* event) -{ - // React only to internal (pre-display) - // events - QWidget::showEvent(event); - refreshTimer->start(updateInterval); - emit visibilityChanged(true); -} - -void HUD::hideEvent(QHideEvent* event) -{ - // React only to internal (pre-display) - // events - refreshTimer->stop(); - QWidget::hideEvent(event); - emit visibilityChanged(false); -} - -void HUD::contextMenuEvent (QContextMenuEvent* event) -{ - QMenu menu(this); - // Update actions - enableHUDAction->setChecked(HUDInstrumentsEnabled); - enableVideoAction->setChecked(videoEnabled); - - menu.addAction(enableHUDAction); - //menu.addAction(selectHUDColorAction); - menu.addAction(enableVideoAction); - menu.addAction(selectOfflineDirectoryAction); - menu.addAction(selectSaveDirectoryAction); - menu.exec(event->globalPos()); -} - -void HUD::createActions() -{ - enableHUDAction = new QAction(tr("Enable HUD"), this); - enableHUDAction->setStatusTip(tr("Show the HUD instruments in this window")); - enableHUDAction->setCheckable(true); - enableHUDAction->setChecked(HUDInstrumentsEnabled); - connect(enableHUDAction, SIGNAL(triggered(bool)), this, SLOT(enableHUDInstruments(bool))); - - enableVideoAction = new QAction(tr("Enable Video Live feed"), this); - enableVideoAction->setStatusTip(tr("Show the video live feed")); - enableVideoAction->setCheckable(true); - enableVideoAction->setChecked(videoEnabled); - connect(enableVideoAction, SIGNAL(triggered(bool)), this, SLOT(enableVideo(bool))); - - selectOfflineDirectoryAction = new QAction(tr("Load image log"), this); - selectOfflineDirectoryAction->setStatusTip(tr("Load previously logged images into simulation / replay")); - connect(selectOfflineDirectoryAction, SIGNAL(triggered()), this, SLOT(selectOfflineDirectory())); - - selectSaveDirectoryAction = new QAction(tr("Save images to directory"), this); - selectSaveDirectoryAction->setStatusTip(tr("Save images from image stream to a directory")); - selectSaveDirectoryAction->setCheckable(true); - connect(selectSaveDirectoryAction, SIGNAL(triggered(bool)), this, SLOT(saveImages(bool))); -} - -/** - * - * @param uas the UAS/MAV to monitor/display with the HUD - */ -void HUD::_activeVehicleChanged(Vehicle* vehicle) -{ - if (this->uas != NULL) { - // Disconnect any previously connected active MAV - disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*, double, double, double, quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64))); - disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int, double, double, double, quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64))); - disconnect(this->uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int))); - disconnect(this->uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); - disconnect(this->uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - disconnect(this->uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); - - disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); - - // Try to disconnect the image link - UAS* u = dynamic_cast(this->uas); - if (u) { - disconnect(u, SIGNAL(imageReady(UASInterface*)), this, SLOT(copyImage(UASInterface*))); - } - } - - this->uas = NULL; - - if (vehicle) { - this->uas = vehicle->uas(); - // Now connect the new UAS - // Setup communication - connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64))); - connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64))); - connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int))); - connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); - connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); - - 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,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64))); - connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); - - // Try to connect the image link - UAS* u = qobject_cast(uas); - if (u) { - connect(u, SIGNAL(imageReady(UASInterface*)), this, SLOT(copyImage(UASInterface*))); - } - } - -} - -//void HUD::updateAttitudeThrustSetPoint(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec) -//{ -//// updateValue(uas, "roll desired", rollDesired, msec); -//// updateValue(uas, "pitch desired", pitchDesired, msec); -//// updateValue(uas, "yaw desired", yawDesired, msec); -//// updateValue(uas, "thrust desired", thrustDesired, msec); -//} - -void HUD::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp) -{ - Q_UNUSED(uas); - Q_UNUSED(timestamp); - if (!isnan(roll) && !isinf(roll) && !isnan(pitch) && !isinf(pitch) && !isnan(yaw) && !isinf(yaw)) - { - this->roll = roll; - this->pitch = pitch*3.35f; // Constant here is the 'focal length' of the projection onto the plane - this->yaw = yaw; - } -} - -void HUD::updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp) -{ - Q_UNUSED(uas); - Q_UNUSED(timestamp); - if (!isnan(roll) && !isinf(roll) && !isnan(pitch) && !isinf(pitch) && !isnan(yaw) && !isinf(yaw)) - { - attitudes.insert(component, QVector3D(roll, pitch*3.35f, yaw)); // Constant here is the 'focal length' of the projection onto the plane - } -} - -void HUD::updateBattery(UASInterface* uas, double voltage, double current, double percent, int seconds) -{ - Q_UNUSED(uas); - Q_UNUSED(seconds); - Q_UNUSED(current); - fuelStatus = tr("BAT [%1% | %2V]").arg(percent, 2, 'f', 0, QChar('0')).arg(voltage, 4, 'f', 1, QChar('0')); - if (percent < 20.0f) { - fuelColor = warningColor; - } else if (percent < 10.0f) { - fuelColor = criticalColor; - } else { - fuelColor = infoColor; - } -} - -void HUD::receiveHeartbeat(UASInterface*) -{ -} - -void HUD::updateThrust(UASInterface* uas, double thrust) -{ - Q_UNUSED(uas); - Q_UNUSED(thrust); -// updateValue(uas, "thrust", thrust, MG::TIME::getGroundTimeNow()); -} - -void HUD::updateLocalPosition(UASInterface* uas,double x,double y,double z,quint64 timestamp) -{ - Q_UNUSED(uas); - Q_UNUSED(timestamp); - this->xPos = x; - this->yPos = y; - this->zPos = z; -} - -void HUD::updateGlobalPosition(UASInterface* uas,double lat, double lon, double altitudeAMSL, double altitudeWGS84, quint64 timestamp) -{ - Q_UNUSED(uas); - Q_UNUSED(altitudeAMSL); - Q_UNUSED(timestamp); - this->lat = lat; - this->lon = lon; - this->alt = altitudeWGS84; -} - -void HUD::updateSpeed(UASInterface* uas,double x,double y,double z,quint64 timestamp) -{ - Q_UNUSED(uas); - Q_UNUSED(timestamp); - this->xSpeed = x; - this->ySpeed = y; - this->zSpeed = z; - double newTotalSpeed = sqrt(xSpeed*xSpeed + ySpeed*ySpeed + zSpeed*zSpeed); - totalAcc = (newTotalSpeed - totalSpeed) / ((double)(lastSpeedUpdate - timestamp)/1000.0); - totalSpeed = newTotalSpeed; -} - -/** - * Updates the current system state, but only if the uas matches the currently monitored uas. - * - * @param uas the system the state message originates from - * @param state short state text, displayed in HUD - */ -void HUD::updateState(UASInterface* uas,QString state) -{ - // Only one UAS is connected at a time - Q_UNUSED(uas); - this->state = state; -} - -/** - * Updates the current system mode, but only if the uas matches the currently monitored uas. - * - * @param uas the system the state message originates from - * @param mode short mode text, displayed in HUD - */ -void HUD::updateMode(int id,QString mode, QString description) -{ - // Only one UAS is connected at a time - Q_UNUSED(id); - Q_UNUSED(description); - this->mode = mode; -} - -void HUD::updateLoad(UASInterface* uas, double load) -{ - Q_UNUSED(uas); - this->load = load; - //updateValue(uas, "load", load, MG::TIME::getGroundTimeNow()); -} - -/** - * @param y coordinate in pixels to be converted to reference mm units - * @return the screen coordinate relative to the QGLWindow origin - */ -float HUD::refToScreenX(float x) -{ - //qDebug() << "sX: " << (scalingFactor * x) << "Orig:" << x; - return (scalingFactor * x); -} -/** - * @param x coordinate in pixels to be converted to reference mm units - * @return the screen coordinate relative to the QGLWindow origin - */ -float HUD::refToScreenY(float y) -{ - //qDebug() << "sY: " << (scalingFactor * y); - return (scalingFactor * y); -} - -/** - * Paint text on top of the image and OpenGL drawings - * - * @param text chars to write - * @param color text color - * @param fontSize text size in mm - * @param refX position in reference units (mm of the real instrument). This is relative to the measurement unit position, NOT in pixels. - * @param refY position in reference units (mm of the real instrument). This is relative to the measurement unit position, NOT in pixels. - */ -void HUD::paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter) -{ - QPen prevPen = painter->pen(); - float pPositionX = refToScreenX(refX) - (fontSize*scalingFactor*0.072f); - float pPositionY = refToScreenY(refY) - (fontSize*scalingFactor*0.212f); - - QFont font("Bitstream Vera Sans"); - // Enforce minimum font size of 5 pixels - int fSize = qMax(5, (int)(fontSize*scalingFactor*1.26f)); - font.setPixelSize(fSize); - - QFontMetrics metrics = QFontMetrics(font); - int border = qMax(4, metrics.leading()); - QRect rect = metrics.boundingRect(0, 0, width() - 2*border, int(height()*0.125), - Qt::AlignLeft | Qt::TextWordWrap, text); - painter->setPen(color); - painter->setFont(font); - painter->setRenderHint(QPainter::TextAntialiasing); - painter->drawText(pPositionX, pPositionY, - rect.width(), rect.height(), - Qt::AlignCenter | Qt::TextWordWrap, text); - painter->setPen(prevPen); -} - -/** - * @param referencePositionX horizontal position in the reference mm-unit space - * @param referencePositionY horizontal position in the reference mm-unit space - * @param referenceWidth width in the reference mm-unit space - * @param referenceHeight width in the reference mm-unit space - */ -void HUD::setupGLView(float referencePositionX, float referencePositionY, float referenceWidth, float referenceHeight) -{ - Q_UNUSED(referencePositionX); - Q_UNUSED(referencePositionY); - Q_UNUSED(referenceWidth); - Q_UNUSED(referenceHeight); -#if 0 - // code ifdef'ed out but left in to silence warnings - int pixelWidth = (int)(referenceWidth * scalingFactor); - int pixelHeight = (int)(referenceHeight * scalingFactor); - // Translate and scale the GL view in the virtual reference coordinate units on the screen - int pixelPositionX = (int)((referencePositionX * scalingFactor) + xCenterOffset); - int pixelPositionY = this->height() - (referencePositionY * scalingFactor) + yCenterOffset - pixelHeight; -#endif -} - -void HUD::paintRollPitchStrips() -{ -} - - -void HUD::paintEvent(QPaintEvent *event) -{ - Q_UNUSED(event); - paintHUD(); -} - -void HUD::paintHUD() -{ - if (isVisible()) { - // static quint64 interval = 0; - // qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; - // interval = MG::TIME::getGroundTimeNow(); - -#if (QGC_EVENTLOOP_DEBUG) - qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; -#endif - - // Read out most important values to limit hash table lookups - // Low-pass roll, pitch and yaw - rollLP = roll;//rollLP * 0.2f + 0.8f * roll; - pitchLP = pitch;//pitchLP * 0.2f + 0.8f * pitch; - yawLP = (!isinf(yaw) && !isnan(yaw)) ? yaw : yawLP;//yawLP * 0.2f + 0.8f * yaw; - - // Translate for yaw - const float maxYawTrans = 60.0f; - - float newYawDiff = yawDiff; - if (isinf(newYawDiff)) newYawDiff = yawDiff; - if (newYawDiff > M_PI) newYawDiff = newYawDiff - M_PI; - - if (newYawDiff < -M_PI) newYawDiff = newYawDiff + M_PI; - - newYawDiff = yawDiff * 0.8 + newYawDiff * 0.2; - - yawDiff = newYawDiff; - - yawInt += newYawDiff; - - if (yawInt > M_PI) yawInt = (float)M_PI; - if (yawInt < -M_PI) yawInt = (float)-M_PI; - - float yawTrans = yawInt * (float)maxYawTrans; - yawInt *= 0.6f; - - if ((yawTrans < 5.0) && (yawTrans > -5.0)) yawTrans = 0; - - // Negate to correct direction - yawTrans = -yawTrans; - - yawTrans = 0; - - //qDebug() << "yaw translation" << yawTrans << "integral" << yawInt << "difference" << yawDiff << "yaw" << yaw; - - // Update scaling factor - // adjust scaling to fit both horizontally and vertically - scalingFactor = this->width()/vwidth; - double scalingFactorH = this->height()/vheight; - if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH; - - // And if either video or the data stream is enabled, draw the next frame. - if (videoEnabled) - { - xImageFactor = width() / (float)glImage.width(); - yImageFactor = height() / (float)glImage.height(); - } - - QPainter painter; - painter.begin(this); - painter.setRenderHint(QPainter::Antialiasing, true); - painter.setRenderHint(QPainter::HighQualityAntialiasing, true); - QPixmap pmap = QPixmap::fromImage(glImage).scaledToWidth(width()); - painter.drawPixmap(0, (height() - pmap.height()) / 2, pmap); - - // END OF OPENGL PAINTING - - if (HUDInstrumentsEnabled) - { - - //glEnable(GL_MULTISAMPLE); - - // QT PAINTING - //makeCurrent(); - - painter.translate((this->vwidth/2.0+xCenterOffset)*scalingFactor, (this->vheight/2.0+yCenterOffset)*scalingFactor); - - // COORDINATE FRAME IS NOW (0,0) at CENTER OF WIDGET - - // Draw all fixed indicators - // BATTERY - paintText(fuelStatus, fuelColor, 6.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 6, &painter); - // Waypoint - paintText(waypointName, defaultColor, 6.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter); - - QPen linePen(Qt::SolidLine); - linePen.setWidth(refLineWidthToPen(1.0f)); - linePen.setColor(defaultColor); - painter.setBrush(Qt::NoBrush); - painter.setPen(linePen); - - // YAW INDICATOR - // - // . - // . . - // ....... - // - const float yawIndicatorWidth = 12.0f; - const float yawIndicatorY = vheight/2.0f - 15.0f; - QPolygon yawIndicator(4); - yawIndicator.setPoint(0, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY))); - yawIndicator.setPoint(1, QPoint(refToScreenX(yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth))); - yawIndicator.setPoint(2, QPoint(refToScreenX(-yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth))); - yawIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY))); - painter.drawPolyline(yawIndicator); - painter.setPen(linePen); - - // CENTER - - // HEADING INDICATOR - // - // __ __ - // \/\/ - // - const float hIndicatorWidth = 20.0f; - const float hIndicatorY = -25.0f; - const float hIndicatorYLow = hIndicatorY + hIndicatorWidth / 6.0f; - const float hIndicatorSegmentWidth = hIndicatorWidth / 7.0f; - QPolygon hIndicator(7); - hIndicator.setPoint(0, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f), refToScreenY(hIndicatorY))); - hIndicator.setPoint(1, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f+hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY))); - hIndicator.setPoint(2, QPoint(refToScreenX(0.0f-hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow))); - hIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(hIndicatorY))); - hIndicator.setPoint(4, QPoint(refToScreenX(0.0f+hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow))); - hIndicator.setPoint(5, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f-hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY))); - hIndicator.setPoint(6, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f), refToScreenY(hIndicatorY))); - painter.drawPolyline(hIndicator); - - - // SETPOINT - const float centerWidth = 8.0f; - // TODO - //painter.drawEllipse(QPointF(refToScreenX(qMin(10.0f, values.value("roll desired", 0.0f) * 10.0f)), refToScreenY(qMin(10.0f, values.value("pitch desired", 0.0f) * 10.0f))), refToScreenX(centerWidth/2.0f), refToScreenX(centerWidth/2.0f)); - - const float centerCrossWidth = 20.0f; - // left - painter.drawLine(QPointF(refToScreenX(-centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(-centerCrossWidth / 2.0f), refToScreenY(0.0f))); - // right - painter.drawLine(QPointF(refToScreenX(centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(centerCrossWidth / 2.0f), refToScreenY(0.0f))); - // top - painter.drawLine(QPointF(refToScreenX(0.0f), refToScreenY(-centerWidth / 2.0f)), QPointF(refToScreenX(0.0f), refToScreenY(-centerCrossWidth / 2.0f))); - - - - // COMPASS - const float compassY = -vheight/2.0f + 6.0f; - QRectF compassRect(QPointF(refToScreenX(-12.0f), refToScreenY(compassY)), QSizeF(refToScreenX(24.0f), refToScreenY(12.0f))); - painter.setBrush(Qt::NoBrush); - painter.setPen(linePen); - painter.drawRoundedRect(compassRect, 3, 3); - QString yawAngle; - - // const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f; - - // YAW is in compass-human readable format, so 0 .. 360 deg. - float yawDeg = (yawLP / M_PI) * 180.0f; - if (yawDeg < 0) yawDeg += 360; - if (yawDeg > 360) yawDeg -= 360; - /* final safeguard for really stupid systems */ - int yawCompass = static_cast(yawDeg) % 360; - yawAngle.sprintf("%03d", yawCompass); - paintText(yawAngle, defaultColor,8.5f, -9.8f, compassY+ 1.7f, &painter); - - painter.setBrush(Qt::NoBrush); - painter.setPen(linePen); - - // CHANGE RATE STRIPS - drawChangeRateStrip(-95.0f, -60.0f, 40.0f, -10.0f, 10.0f, -zSpeed, &painter); - - // CHANGE RATE STRIPS - drawChangeRateStrip(95.0f, -60.0f, 40.0f, -10.0f, 10.0f, totalAcc, &painter,true); - - // GAUGES - - // Left altitude gauge - float gaugeAltitude; - - if (this->alt != 0) { - gaugeAltitude = alt; - } else { - gaugeAltitude = -zPos; - } - - painter.setBrush(Qt::NoBrush); - painter.setPen(linePen); - - drawChangeIndicatorGauge(-vGaugeSpacing, 35.0f, 15.0f, 10.0f, gaugeAltitude, defaultColor, &painter, false); - paintText("alt m", defaultColor, 5.5f, -73.0f, 50, &painter); - - // Right speed gauge - drawChangeIndicatorGauge(vGaugeSpacing, 35.0f, 15.0f, 10.0f, totalSpeed, defaultColor, &painter, false); - paintText("v m/s", defaultColor, 5.5f, 55.0f, 50, &painter); - - - // Waypoint name - if (waypointName != "") paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter); - - // MOVING PARTS - - - painter.translate(refToScreenX(yawTrans), 0); - - // Old single-component pitch drawing -// // Rotate view and draw all roll-dependent indicators -// painter.rotate((rollLP/M_PI)* -180.0f); - -// painter.translate(0, (-pitchLP/(float)M_PI)* -180.0f * refToScreenY(1.8f)); - -// //qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f); - -// // PITCH - -// paintPitchLines(pitchLP, &painter); - - QColor attColor = painter.pen().color(); - - // Draw multi-component attitude - foreach (QVector3D att, attitudes.values()) - { - attColor = attColor.darker(200); - painter.setPen(attColor); - // Rotate view and draw all roll-dependent indicators - painter.rotate((att.x()/M_PI)* -180.0f); - - painter.translate(0, (-att.y()/(float)M_PI)* -180.0f * refToScreenY(1.8f)); - - //qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f); - - // PITCH - - paintPitchLines(att.y(), &painter); - painter.translate(0, -(-att.y()/(float)M_PI)* -180.0f * refToScreenY(1.8f)); - painter.rotate(-(att.x()/M_PI)* -180.0f); - } - - - } - - painter.end(); - } - -} - - -/** - * @param pitch pitch angle in degrees (-180 to 180) - */ -void HUD::paintPitchLines(float pitch, QPainter* painter) -{ - QString label; - - const float yDeg = vPitchPerDeg; - const float lineDistance = 5.0f; ///< One pitch line every 10 degrees - const float posIncrement = yDeg * lineDistance; - float posY = posIncrement; - const float posLimit = sqrt(pow(vwidth, 2.0f) + pow(vheight, 2.0f))*3.0f; - - const float offsetAbs = pitch * yDeg; - - float offset = pitch; - if (offset < 0) offset = -offset; - int offsetCount = 0; - while (offset > lineDistance) { - offset -= lineDistance; - offsetCount++; - } - - int iPos = (int)(0.5f + lineDistance); ///< The first line - int iNeg = (int)(-0.5f - lineDistance); ///< The first line - - offset *= yDeg; - - - painter->setPen(defaultColor); - - posY = -offsetAbs + posIncrement; //+ 100;// + lineDistance; - - while (posY < posLimit) { - paintPitchLinePos(label.sprintf("%3d", iPos), 0.0f, -posY, painter); - posY += posIncrement; - iPos += (int)lineDistance; - } - - - - // HORIZON - // - // ------------ ------------ - // - const float pitchWidth = 30.0f; - const float pitchGap = pitchWidth / 2.5f; - const QColor horizonColor = defaultColor; - const float diagonal = sqrt(pow(vwidth, 2.0f) + pow(vheight, 2.0f)); - const float lineWidth = refLineWidthToPen(0.5f); - - // Left horizon - drawLine(0.0f-diagonal, offsetAbs, 0.0f-pitchGap/2.0f, offsetAbs, lineWidth, horizonColor, painter); - // Right horizon - drawLine(0.0f+pitchGap/2.0f, offsetAbs, 0.0f+diagonal, offsetAbs, lineWidth, horizonColor, painter); - - - - label.clear(); - - posY = offsetAbs + posIncrement; - - - while (posY < posLimit) { - paintPitchLineNeg(label.sprintf("%3d", iNeg), 0.0f, posY, painter); - posY += posIncrement; - iNeg -= (int)lineDistance; - } -} - -void HUD::paintPitchLinePos(QString text, float refPosX, float refPosY, QPainter* painter) -{ - //painter->setPen(QPen(QBrush, normalStrokeWidth)); - - const float pitchWidth = 30.0f; - const float pitchGap = pitchWidth / 2.5f; - const float pitchHeight = pitchWidth / 12.0f; - const float textSize = pitchHeight * 1.6f; - const float lineWidth = 1.5f; - - // Positive pitch indicator: - // - // _______ _______ - // |10 | - // - - // Left vertical line - drawLine(refPosX-pitchWidth/2.0f, refPosY, refPosX-pitchWidth/2.0f, refPosY+pitchHeight, lineWidth, defaultColor, painter); - // Left horizontal line - drawLine(refPosX-pitchWidth/2.0f, refPosY, refPosX-pitchGap/2.0f, refPosY, lineWidth, defaultColor, painter); - // Text left - paintText(text, defaultColor, textSize, refPosX-pitchWidth/2.0 + 0.75f, refPosY + pitchHeight - 1.3f, painter); - - // Right vertical line - drawLine(refPosX+pitchWidth/2.0f, refPosY, refPosX+pitchWidth/2.0f, refPosY+pitchHeight, lineWidth, defaultColor, painter); - // Right horizontal line - drawLine(refPosX+pitchWidth/2.0f, refPosY, refPosX+pitchGap/2.0f, refPosY, lineWidth, defaultColor, painter); -} - -void HUD::paintPitchLineNeg(QString text, float refPosX, float refPosY, QPainter* painter) -{ - const float pitchWidth = 30.0f; - const float pitchGap = pitchWidth / 2.5f; - const float pitchHeight = pitchWidth / 12.0f; - const float textSize = pitchHeight * 1.6f; - const float segmentWidth = ((pitchWidth - pitchGap)/2.0f) / 7.0f; ///< Four lines and three gaps -> 7 segments - - const float lineWidth = 1.5f; - - // Negative pitch indicator: - // - // -10 - // _ _ _ _| |_ _ _ _ - // - // - - // Left vertical line - drawLine(refPosX-pitchGap/2.0, refPosY, refPosX-pitchGap/2.0, refPosY-pitchHeight, lineWidth, defaultColor, painter); - // Left horizontal line with four segments - for (int i = 0; i < 7; i+=2) { - drawLine(refPosX-pitchWidth/2.0+(i*segmentWidth), refPosY, refPosX-pitchWidth/2.0+(i*segmentWidth)+segmentWidth, refPosY, lineWidth, defaultColor, painter); - } - // Text left - paintText(text, defaultColor, textSize, refPosX-pitchWidth/2.0f + 0.75f, refPosY + pitchHeight - 1.3f, painter); - - // Right vertical line - drawLine(refPosX+pitchGap/2.0, refPosY, refPosX+pitchGap/2.0, refPosY-pitchHeight, lineWidth, defaultColor, painter); - // Right horizontal line with four segments - for (int i = 0; i < 7; i+=2) { - drawLine(refPosX+pitchWidth/2.0f-(i*segmentWidth), refPosY, refPosX+pitchWidth/2.0f-(i*segmentWidth)-segmentWidth, refPosY, lineWidth, defaultColor, painter); - } -} - -void rotatePointClockWise(QPointF& p, float angle) -{ - // Standard 2x2 rotation matrix, counter-clockwise - // - // | cos(phi) sin(phi) | - // | -sin(phi) cos(phi) | - // - - //p.setX(cos(angle) * p.x() + sin(angle) * p.y()); - //p.setY(-sin(angle) * p.x() + cos(angle) * p.y()); - - - p.setX(cos(angle) * p.x() + sin(angle)* p.y()); - p.setY((-1.0f * sin(angle) * p.x()) + cos(angle) * p.y()); -} - -float HUD::refLineWidthToPen(float line) -{ - return line * 2.50f; -} - -/** - * Rotate a polygon around a point - * - * @param p polygon to rotate - * @param origin the rotation center - * @param angle rotation angle, in radians - * @return p Polygon p rotated by angle around the origin point - */ -void HUD::rotatePolygonClockWiseRad(QPolygonF& p, float angle, QPointF origin) -{ - // Standard 2x2 rotation matrix, counter-clockwise - // - // | cos(phi) sin(phi) | - // | -sin(phi) cos(phi) | - // - for (int i = 0; i < p.size(); i++) { - QPointF curr = p.at(i); - - const float x = curr.x(); - const float y = curr.y(); - - curr.setX(((cos(angle) * (x-origin.x())) + (-sin(angle) * (y-origin.y()))) + origin.x()); - curr.setY(((sin(angle) * (x-origin.x())) + (cos(angle) * (y-origin.y()))) + origin.y()); - p.replace(i, curr); - } -} - -void HUD::drawPolygon(QPolygonF refPolygon, QPainter* painter) -{ - // Scale coordinates - QPolygonF draw(refPolygon.size()); - for (int i = 0; i < refPolygon.size(); i++) { - QPointF curr; - curr.setX(refToScreenX(refPolygon.at(i).x())); - curr.setY(refToScreenY(refPolygon.at(i).y())); - draw.replace(i, curr); - } - painter->drawPolygon(draw); -} - -void HUD::drawChangeRateStrip(float xRef, float yRef, float height, float minRate, float maxRate, float value, QPainter* painter,bool reverse) -{ - float scaledValue = value; - - // Saturate value - if (value > maxRate) scaledValue = maxRate; - if (value < minRate) scaledValue = minRate; - - // x (Origin: xRef, yRef) - // - - // | - // | - // | - // = - // | - // -0.005 >| - // | - // - - - const float width = height / 8.0f; - const float lineWidth = 1.5f; - - // Indicator lines - // Top horizontal line - if (reverse) - { - drawLine(xRef, yRef, xRef-width, yRef, lineWidth, defaultColor, painter); - // Vertical main line - drawLine(xRef-width/2.0f, yRef, xRef-width/2.0f, yRef+height, lineWidth, defaultColor, painter); - // Zero mark - drawLine(xRef, yRef+height/2.0f, xRef-width, yRef+height/2.0f, lineWidth, defaultColor, painter); - // Horizontal bottom line - drawLine(xRef, yRef+height, xRef-width, yRef+height, lineWidth, defaultColor, painter); - - // Text - QString label; - label.sprintf("%+06.2f >", value); - - QFont font("Bitstream Vera Sans"); - // Enforce minimum font size of 5 pixels - //int fSize = qMax(5, (int)(6.0f*scalingFactor*1.26f)); - font.setPixelSize(6.0f * 1.26f); - - QFontMetrics metrics = QFontMetrics(font); - paintText(label, defaultColor, 6.0f, (xRef-width) - metrics.width(label), yRef+height-((scaledValue - minRate)/(maxRate-minRate))*height - 1.6f, painter); - } - else - { - drawLine(xRef, yRef, xRef+width, yRef, lineWidth, defaultColor, painter); - // Vertical main line - drawLine(xRef+width/2.0f, yRef, xRef+width/2.0f, yRef+height, lineWidth, defaultColor, painter); - // Zero mark - drawLine(xRef, yRef+height/2.0f, xRef+width, yRef+height/2.0f, lineWidth, defaultColor, painter); - // Horizontal bottom line - drawLine(xRef, yRef+height, xRef+width, yRef+height, lineWidth, defaultColor, painter); - - // Text - QString label; - label.sprintf("< %+06.2f", value); - paintText(label, defaultColor, 6.0f, xRef+width/2.0f, yRef+height-((scaledValue - minRate)/(maxRate-minRate))*height - 1.6f, painter); - } -} - -//void HUD::drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter) -//{ -// Q_UNUSED(maxWidth); -// Q_UNUSED(maxHeight); -// if (values.size() > 0) -// { -// QString selectedKey = values.begin().key(); -// // | | | | | | -// // | | | | | | -// // x speed: 2.54 - -// // One column per value -// QMapIterator value(values); - -// float x = xRef; -// float y = yRef; - -// const float vspacing = 1.0f; -// float width = 1.5f; -// float height = 1.5f; -// const float hspacing = 0.6f; - -// // TODO ensure that instrument stays smaller than maxWidth and maxHeight - - -// int i = 0; -// while (value.hasNext() && i < maxNum) -// { -// value.next(); -// QBrush brush(Qt::SolidPattern); - - -// if (value.value() < 0.01f && value.value() > -0.01f) -// { -// brush.setColor(Qt::gray); -// } -// else if (value.value() > 0.01f) -// { -// brush.setColor(Qt::blue); -// } -// else -// { -// brush.setColor(Qt::yellow); -// } - -// painter->setBrush(brush); -// painter->setPen(Qt::NoPen); - -// // Draw current value colormap -// painter->drawRect(refToScreenX(x), refToScreenY(y), refToScreenX(width), refToScreenY(height)); - -// // Draw change rate colormap -// painter->drawRect(refToScreenX(x), refToScreenY(y+height+hspacing), refToScreenX(width), refToScreenY(height)); - -// // Draw mean value colormap -// painter->drawRect(refToScreenX(x), refToScreenY(y+2.0f*(height+hspacing)), refToScreenX(width), refToScreenY(height)); - -// // Add spacing -// x += width+vspacing; - -// // Iterate -// i++; -// } - -// // Draw detail label -// QString detail = "NO DATA AVAILABLE"; - -// if (values.contains(selectedKey)) -// { -// detail = values.find(selectedKey).key(); -// detail.append(": "); -// detail.append(QString::number(values.find(selectedKey).value())); -// } -// paintText(detail, QColor(255, 255, 255), 3.0f, xRef, yRef+3.0f*(height+hspacing)+1.0f, painter); -// } -//} - -void HUD::drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid) -{ - // Draw the circle - QPen circlePen(Qt::SolidLine); - if (!solid) circlePen.setStyle(Qt::DotLine); - circlePen.setColor(defaultColor); - circlePen.setWidth(refLineWidthToPen(2.0f)); - painter->setBrush(Qt::NoBrush); - painter->setPen(circlePen); - drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.5f, color, painter); - - QString label; - label.sprintf("%05.1f", value); - - float textSize = radius / 2.5; - - // Draw the value - paintText(label, color, textSize, xRef-textSize*1.7f, yRef-textSize*0.4f, painter); - - // Draw the needle - // Scale the rotation so that the gauge does one revolution - // per max. change - const float rangeScale = (2.0f * M_PI) / expectedMaxChange; - const float maxWidth = radius / 10.0f; - const float minWidth = maxWidth * 0.3f; - - 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, value*rangeScale, QPointF(xRef, yRef)); - - QBrush indexBrush; - indexBrush.setColor(defaultColor); - indexBrush.setStyle(Qt::SolidPattern); - painter->setPen(Qt::SolidLine); - painter->setPen(defaultColor); - painter->setBrush(indexBrush); - drawPolygon(p, painter); -} - -void HUD::drawLine(float refX1, float refY1, float refX2, float refY2, float width, const QColor& color, QPainter* painter) -{ - QPen pen(Qt::SolidLine); - pen.setWidth(refLineWidthToPen(width)); - pen.setColor(color); - painter->setPen(pen); - painter->drawLine(QPoint(refToScreenX(refX1), refToScreenY(refY1)), QPoint(refToScreenX(refX2), refToScreenY(refY2))); -} - -void HUD::drawEllipse(float refX, float refY, float radiusX, float radiusY, float startDeg, float endDeg, float lineWidth, const QColor& color, QPainter* painter) -{ - Q_UNUSED(startDeg); - Q_UNUSED(endDeg); - QPen pen(painter->pen().style()); - pen.setWidth(refLineWidthToPen(lineWidth)); - pen.setColor(color); - painter->setPen(pen); - painter->drawEllipse(QPointF(refToScreenX(refX), refToScreenY(refY)), refToScreenX(radiusX), refToScreenY(radiusY)); -} - -void HUD::drawCircle(float refX, float refY, float radius, float startDeg, float endDeg, float lineWidth, const QColor& color, QPainter* painter) -{ - drawEllipse(refX, refY, radius, radius, startDeg, endDeg, lineWidth, color, painter); -} - -void HUD::selectWaypoint(int uasId, int id) -{ - Q_UNUSED(uasId); - waypointName = tr("WP") + QString::number(id); -} - -void HUD::setImageSize(int width, int height, int depth, int channels) -{ - // Allocate raw image in correct size - if (width != receivedWidth || height != receivedHeight || depth != receivedDepth || channels != receivedChannels || image == NULL) { - // Set new size - if (width > 0) receivedWidth = width; - if (height > 0) receivedHeight = height; - if (depth > 1) receivedDepth = depth; - if (channels > 1) receivedChannels = channels; - - rawExpectedBytes = (receivedWidth * receivedHeight * receivedDepth * receivedChannels) / 8; - bytesPerLine = rawExpectedBytes / receivedHeight; - // Delete old buffers if necessary - rawImage = NULL; - if (rawBuffer1 != NULL) delete rawBuffer1; - if (rawBuffer2 != NULL) delete rawBuffer2; - - rawBuffer1 = (unsigned char*)malloc(rawExpectedBytes); - rawBuffer2 = (unsigned char*)malloc(rawExpectedBytes); - rawImage = rawBuffer1; - if (image) - delete image; - - // Set image format - // 8 BIT GREYSCALE IMAGE - if (depth <= 8 && channels == 1) { - image = new QImage(receivedWidth, receivedHeight, QImage::Format_Indexed8); - // Create matching color table - image->setColorCount(256); - for (int i = 0; i < 256; i++) { - image->setColor(i, qRgb(i, i, i)); - //qDebug() << __FILE__ << __LINE__ << std::hex << i; - } - - } - // 32 BIT COLOR IMAGE WITH ALPHA VALUES (#ARGB) - else { - image = new QImage(receivedWidth, receivedHeight, QImage::Format_ARGB32); - } - - // Fill first channel of image with black pixels - image->fill(qgcApp()->styleIsDark() ? 0 : 255); - glImage = *image; - - qDebug() << __FILE__ << __LINE__ << "Setting up image"; - - // Set size once - setFixedSize(receivedWidth, receivedHeight); - setMinimumSize(receivedWidth, receivedHeight); - setMaximumSize(receivedWidth, receivedHeight); - // Lock down the size - //setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); - //resize(receivedWidth, receivedHeight); - } - -} - -void HUD::startImage(int imgid, int width, int height, int depth, int channels) -{ - Q_UNUSED(imgid); - //qDebug() << "HUD: starting image (" << width << "x" << height << ", " << depth << "bits) with " << channels << "channels"; - - // Copy previous image to screen if it hasn't been finished properly - finishImage(); - - // Reset image size if necessary - setImageSize(width, height, depth, channels); - imageStarted = true; -} - -void HUD::finishImage() -{ - if (imageStarted) { - commitRawDataToGL(); - imageStarted = false; - } -} - -void HUD::commitRawDataToGL() -{ - qDebug() << __FILE__ << __LINE__ << "Copying raw data to GL buffer:" << rawImage << receivedWidth << receivedHeight << image->format(); - if (image != NULL) { - QImage::Format format = image->format(); - QImage* newImage = new QImage(rawImage, receivedWidth, receivedHeight, format); - if (format == QImage::Format_Indexed8) { - // Create matching color table - newImage->setColorCount(256); - for (int i = 0; i < 256; i++) { - newImage->setColor(i, qRgb(i, i, i)); - //qDebug() << __FILE__ << __LINE__ << std::hex << i; - } - } - - glImage = *newImage; - delete image; - image = newImage; - // Switch buffers - if (rawImage == rawBuffer1) { - rawImage = rawBuffer2; - //qDebug() << "Now buffer 2"; - } else { - rawImage = rawBuffer1; - //qDebug() << "Now buffer 1"; - } - } - update(); -} - -void HUD::saveImage(QString fileName) -{ - image->save(fileName); -} - -void HUD::saveImage() -{ - //Bring up popup - QString fileName = "output.png"; - saveImage(fileName); -} - -void HUD::selectOfflineDirectory() -{ - QString fileName = QGCFileDialog::getExistingDirectory(this, tr("Select image directory"), QStandardPaths::writableLocation(QStandardPaths::DesktopLocation)); - if (fileName != "") { - offlineDirectory = fileName; - } -} - -void HUD::enableHUDInstruments(bool enabled) -{ - HUDInstrumentsEnabled = enabled; -} - -void HUD::enableVideo(bool enabled) -{ - videoEnabled = enabled; -} - -void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int startIndex) -{ - Q_UNUSED(imgid); - // qDebug() << "at" << __FILE__ << __LINE__ << ": Received startindex" << startIndex << "and length" << length << "(" << startIndex+length << "of" << rawExpectedBytes << "bytes)"; - - if (imageStarted) - { - //if (rawLastIndex != startIndex) qDebug() << "PACKET LOSS!"; - - if (startIndex+length > rawExpectedBytes) - { - qDebug() << "HUD: OVERFLOW! startIndex:" << startIndex << "length:" << length << "image raw size" << ((receivedWidth * receivedHeight * receivedChannels * receivedDepth) / 8) - 1; - } - else - { - memcpy(rawImage+startIndex, imageData, length); - - rawLastIndex = startIndex+length; - - // Check if we just reached the end of the image - if (startIndex+length == rawExpectedBytes) - { - //qDebug() << "HUD: END OF IMAGE REACHED!"; - finishImage(); - rawLastIndex = 0; - } - } - - // for (int i = 0; i < length; i++) - // { - // for (int j = 0; j < receivedChannels; j++) - // { - // unsigned int x = (startIndex+i) % receivedWidth; - // unsigned int y = (unsigned int)((startIndex+i) / receivedWidth); - // qDebug() << "Setting pixel" << x << "," << y << "to" << (unsigned int)*(rawImage+startIndex+i); - // } - // } - } -} - -void HUD::copyImage(UASInterface* uas) -{ - UAS* u = qobject_cast(uas); - if (u) - { - QImage temp_im = u->getImage(); - if (temp_im.byteCount() > 0) - { - this->glImage = temp_im; - - // Save to directory if logging is enabled - if (imageLoggingEnabled) - { - temp_im.save(QString("%1/%2.png").arg(imageLogDirectory).arg(imageLogCounter)); - imageLogCounter++; - } - } - } -} - -void HUD::saveImages(bool save) -{ - if (save) - { - imageLogDirectory = QGCFileDialog::getExistingDirectory(this, tr("Select image log directory"), QStandardPaths::writableLocation(QStandardPaths::DesktopLocation)); - - qDebug() << "Logging to:" << imageLogDirectory; - - if (imageLogDirectory != "") - { - imageLogCounter = 0; - imageLoggingEnabled = true; - qDebug() << "Logging on"; - } - else - { - imageLoggingEnabled = false; - selectSaveDirectoryAction->setChecked(false); - } - } - else - { - imageLoggingEnabled = false; - selectSaveDirectoryAction->setChecked(false); - } - -} diff --git a/src/ui/HUD.h b/src/ui/HUD.h deleted file mode 100644 index aba2861b96330e6c640c9849673ebfa09b7413cf..0000000000000000000000000000000000000000 --- a/src/ui/HUD.h +++ /dev/null @@ -1,236 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Definition of Head up display - * - * @author Lorenz Meier - * - */ - -#ifndef HUD_H -#define HUD_H - -#include -#include -#include -#include -#include -#include -#include - -#include "UASInterface.h" -#include "MainWindow.h" -#include "Vehicle.h" - -/** - * @brief Displays a Head Up Display (HUD) - * - * This class represents a head up display (HUD) and draws this HUD in an OpenGL widget (QGLWidget). - * It can superimpose the HUD over the current live image stream (any arriving image stream will be auto- - * matically used as background), or it draws the classic blue-brown background known from instruments. - */ -class HUD : public QLabel -{ - Q_OBJECT -public: - HUD(int width = 640, int height = 480, QWidget* parent = NULL); - ~HUD(); - - void setImageSize(int width, int height, int depth, int channels); - void resize(int w, int h); - -public slots: - void styleChanged(bool styleIsDark); - - /** @brief Attitude from main autopilot / system state */ - void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp); - /** @brief Attitude from one specific component / redundant autopilot */ - void updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp); -// void updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); - void updateBattery(UASInterface*, double, double, double, int); - void receiveHeartbeat(UASInterface*); - void updateThrust(UASInterface*, double); - void updateLocalPosition(UASInterface*,double,double,double,quint64); - void updateGlobalPosition(UASInterface*,double,double,double,double,quint64); - void updateSpeed(UASInterface*,double,double,double,quint64); - void updateState(UASInterface*,QString); - void updateMode(int id,QString mode, QString description); - void updateLoad(UASInterface*, double); - void selectWaypoint(int uasId, int id); - - void startImage(int imgid, int width, int height, int depth, int channels); - void setPixels(int imgid, const unsigned char* imageData, int length, int startIndex); - void finishImage(); - void saveImage(); - void saveImage(QString fileName); - void saveImages(bool save); - /** @brief Select directory where to load the offline files from */ - void selectOfflineDirectory(); - /** @brief Enable the HUD instruments */ - void enableHUDInstruments(bool enabled); - /** @brief Enable Video */ - void enableVideo(bool enabled); - /** @brief Copy an image from the current active UAS */ - void copyImage(UASInterface* uas); - - -protected slots: - void _activeVehicleChanged(Vehicle* vehicle); - void paintRollPitchStrips(); - void paintPitchLines(float pitch, QPainter* painter); - /** @brief Paint text on top of the image and OpenGL drawings */ - void paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter); - /** @brief Setup the OpenGL view for drawing a sub-component of the HUD */ - void setupGLView(float referencePositionX, float referencePositionY, float referenceWidth, float referenceHeight); - void paintHUD(); - void paintPitchLinePos(QString text, float refPosX, float refPosY, QPainter* painter); - void paintPitchLineNeg(QString text, float refPosX, float refPosY, QPainter* painter); - - void drawLine(float refX1, float refY1, float refX2, float refY2, float width, const QColor& color, QPainter* painter); - void drawEllipse(float refX, float refY, float radiusX, float radiusY, float startDeg, float endDeg, float lineWidth, const QColor& color, QPainter* painter); - void drawCircle(float refX, float refY, float radius, float startDeg, float endDeg, float lineWidth, const QColor& color, QPainter* painter); - - void drawChangeRateStrip(float xRef, float yRef, float height, float minRate, float maxRate, float value, QPainter* painter,bool reverse = false); - void drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid=true); - - void drawPolygon(QPolygonF refPolygon, QPainter* painter); - -signals: - void visibilityChanged(bool visible); - -protected: - void commitRawDataToGL(); - /** @brief Convert reference coordinates to screen coordinates */ - float refToScreenX(float x); - /** @brief Convert reference coordinates to screen coordinates */ - float refToScreenY(float y); - /** @brief Convert mm line widths to QPen line widths */ - float refLineWidthToPen(float line); - /** @brief Rotate a polygon around a point clockwise */ - void rotatePolygonClockWiseRad(QPolygonF& p, float angle, QPointF origin); - /** @brief Preferred Size */ - QSize sizeHint() const; - /** @brief Start updating widget */ - void showEvent(QShowEvent* event); - /** @brief Stop updating widget */ - void hideEvent(QHideEvent* event); - void contextMenuEvent (QContextMenuEvent* event); - void createActions(); - - static const int updateInterval = 100; - - QImage* image; ///< Double buffer image - QImage glImage; ///< The background / camera image - UASInterface* uas; ///< The uas currently monitored - float yawInt; ///< The yaw integral. Used to damp the yaw indication. - QString mode; ///< The current vehicle mode - QString state; ///< The current vehicle state - QString fuelStatus; ///< Current fuel level / battery voltage - double scalingFactor; ///< Factor used to scale all absolute values to screen coordinates - float xCenterOffset, yCenterOffset; ///< Offset from center of window in mm coordinates - float vwidth; ///< Virtual width of this window, 200 mm per default. This allows to hardcode positions and aspect ratios. This virtual image plane is then scaled to the window size. - float vheight; ///< Virtual height of this window, 150 mm per default - float vGaugeSpacing; ///< Virtual spacing of the gauges from the center, 50 mm per default - float vPitchPerDeg; ///< Virtual pitch to mm conversion. Currently one degree is 3 mm up/down in the pitch markings - - int xCenter; ///< Center of the HUD instrument in pixel coordinates. Allows to off-center the whole instrument in its OpenGL window, e.g. to fit another instrument - int yCenter; ///< Center of the HUD instrument in pixel coordinates. Allows to off-center the whole instrument in its OpenGL window, e.g. to fit another instrument - - // Image buffers - unsigned char* rawBuffer1; ///< Double buffer 1 for the image - unsigned char* rawBuffer2; ///< Double buffer 2 for the image - unsigned char* rawImage; ///< Pointer to current complete image - int rawLastIndex; ///< The last byte index received of the image - int rawExpectedBytes; ///< Number of raw image bytes expected. Calculated by: image depth * channels * widht * height / 8 - int bytesPerLine; ///< Bytes per image line. Is calculated as: image depth * channels * width / 8 - bool imageStarted; ///< If an image is currently in transmission - int receivedDepth; ///< Image depth in bit for the current image - int receivedChannels; ///< Number of color channels - int receivedWidth; ///< Width in pixels of the current image - int receivedHeight; ///< Height in pixels of the current image - - // HUD colors - QColor defaultColor; ///< Color for most HUD elements, e.g. pitch lines, center cross, change rate gauges - QColor setPointColor; ///< Color for the current control set point, e.g. yaw desired - QColor warningColor; ///< Color for warning messages - QColor criticalColor; ///< Color for caution messages - QColor infoColor; ///< Color for normal/default messages - QColor fuelColor; ///< Current color for the fuel message, can be info, warning or critical color - - // Blink rates - int warningBlinkRate; ///< Blink rate of warning messages, will be rounded to the refresh rate - - QTimer* refreshTimer; ///< The main timer, controls the update rate - QPainter* HUDPainter; - QFont font; ///< The HUD font, per default the free Bitstream Vera SANS, which is very close to actual HUD fonts - QFontDatabase fontDatabase;///< Font database, only used to load the TrueType font file (the HUD font is directly loaded from file rather than from the system) - bool noCamera; ///< No camera images available, draw the ground/sky box to indicate the horizon - bool hardwareAcceleration; ///< Enable hardware acceleration - - float strongStrokeWidth; ///< Strong line stroke width, used throughout the HUD - float normalStrokeWidth; ///< Normal line stroke width, used throughout the HUD - float fineStrokeWidth; ///< Fine line stroke width, used throughout the HUD - - QString waypointName; ///< Waypoint name displayed in HUD - float roll; - float pitch; - float yaw; - QMap attitudes; - float rollLP; - float pitchLP; - float yawLP; - double yawDiff; - double xPos; - double yPos; - double zPos; - double xSpeed; - double ySpeed; - double zSpeed; - quint64 lastSpeedUpdate; - double totalSpeed; - double totalAcc; - double lat; - double lon; - double alt; - float load; - QString offlineDirectory; - QString nextOfflineImage; - bool HUDInstrumentsEnabled; - bool videoEnabled; - bool imageLoggingEnabled; - float xImageFactor; - float yImageFactor; - QAction* enableHUDAction; - QAction* enableVideoAction; - QAction* selectOfflineDirectoryAction; - QAction* selectVideoChannelAction; - QAction* selectSaveDirectoryAction; - void paintEvent(QPaintEvent *event); - bool imageRequested; - QString imageLogDirectory; - unsigned int imageLogCounter; -}; - -#endif // HUD_H diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 18097f521e4287ec2735e5258286f2d3bae38d9f..a69a0bb2d22c2079a4d729724d29bb770e5f2ff6 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -55,7 +55,6 @@ This file is part of the QGROUNDCONTROL project #include "QGCMapDisplay.h" #include "MAVLinkDecoder.h" #include "QGCMAVLinkMessageSender.h" -#include "QGCRGBDView.h" #include "UASQuickView.h" #include "QGCDataPlot2D.h" #include "Linecharts.h" @@ -96,11 +95,7 @@ const char* MainWindow::_customCommandWidgetName = "CUSTOM_COMMAND_DOCKWIDGET"; const char* MainWindow::_filesDockWidgetName = "FILE_VIEW_DOCKWIDGET"; const char* MainWindow::_uasStatusDetailsDockWidgetName = "UAS_STATUS_DETAILS_DOCKWIDGET"; const char* MainWindow::_mapViewDockWidgetName = "MAP_VIEW_DOCKWIDGET"; -const char* MainWindow::_hsiDockWidgetName = "HORIZONTAL_SITUATION_INDICATOR_DOCKWIDGET"; -const char* MainWindow::_hdd1DockWidgetName = "HEAD_DOWN_DISPLAY_1_DOCKWIDGET"; -const char* MainWindow::_hdd2DockWidgetName = "HEAD_DOWN_DISPLAY_2_DOCKWIDGET"; const char* MainWindow::_pfdDockWidgetName = "PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET"; -const char* MainWindow::_hudDockWidgetName = "HEAD_UP_DISPLAY_DOCKWIDGET"; const char* MainWindow::_uasInfoViewDockWidgetName = "UAS_INFO_INFOVIEW_DOCKWIDGET"; static MainWindow* _instance = NULL; ///< @brief MainWindow singleton @@ -403,11 +398,7 @@ void MainWindow::_buildCommonWidgets(void) { _filesDockWidgetName, "Onboard Files", Qt::RightDockWidgetArea }, { _uasStatusDetailsDockWidgetName, "Status Details", Qt::RightDockWidgetArea }, { _mapViewDockWidgetName, "Map view", Qt::RightDockWidgetArea }, - { _hsiDockWidgetName, "Horizontal Situation", Qt::BottomDockWidgetArea }, - { _hdd1DockWidgetName, "Flight Display", Qt::RightDockWidgetArea }, - { _hdd2DockWidgetName, "Actuator Status", Qt::RightDockWidgetArea }, { _pfdDockWidgetName, "Primary Flight Display", Qt::RightDockWidgetArea }, - { _hudDockWidgetName, "Video Downlink", Qt::RightDockWidgetArea }, { _uasInfoViewDockWidgetName, "Info View", Qt::LeftDockWidgetArea }, }; static const size_t cDockWidgetInfo = sizeof(rgDockWidgetInfo) / sizeof(rgDockWidgetInfo[0]); @@ -516,28 +507,8 @@ void MainWindow::_createInnerDockWidget(const QString& widgetName) widget = new UASInfoWidget(this); } else if (widgetName == _mapViewDockWidgetName) { widget = new QGCMapTool(this); - } else if (widgetName == _hsiDockWidgetName) { - widget = new HSIDisplay(this); - } else if (widgetName == _hdd1DockWidgetName) { - QStringList acceptList; - acceptList.append("-3.3,ATTITUDE.roll,rad,+3.3,s"); - acceptList.append("-3.3,ATTITUDE.pitch,deg,+3.3,s"); - acceptList.append("-3.3,ATTITUDE.yaw,deg,+3.3,s"); - HDDisplay *hddisplay = new HDDisplay(acceptList,"Flight Display",this); - hddisplay->addSource(mavlinkDecoder); - - widget = hddisplay; - } else if (widgetName == _hdd2DockWidgetName) { - QStringList acceptList; - acceptList.append("0,RAW_PRESSURE.pres_abs,hPa,65500"); - HDDisplay *hddisplay = new HDDisplay(acceptList,"Actuator Status",this); - hddisplay->addSource(mavlinkDecoder); - - widget = hddisplay; } else if (widgetName == _pfdDockWidgetName) { widget = new FlightDisplayWidget(this); - } else if (widgetName == _hudDockWidgetName) { - widget = new HUD(320,240,this); } else if (widgetName == _uasInfoViewDockWidgetName) { QGCTabbedInfoView* pInfoView = new QGCTabbedInfoView(this); pInfoView->addSource(mavlinkDecoder); diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 6e1bbf34de4634456f641131f219c2b05b0a4669..5d64f5457abce65f9990d5575fafa1d2227586e2 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -50,8 +50,6 @@ This file is part of the QGROUNDCONTROL project #include "Mouse6dofInput.h" #endif // QGC_MOUSE_ENABLED_WIN #include "ParameterEditorWidget.h" -#include "HDDisplay.h" -#include "HSIDisplay.h" #include "opmapcontrol.h" #include "MainToolBar.h" #include "LogCompressor.h" @@ -298,11 +296,7 @@ private: static const char* _filesDockWidgetName; static const char* _uasStatusDetailsDockWidgetName; static const char* _mapViewDockWidgetName; - static const char* _hsiDockWidgetName; - static const char* _hdd1DockWidgetName; - static const char* _hdd2DockWidgetName; static const char* _pfdDockWidgetName; - static const char* _hudDockWidgetName; static const char* _uasInfoViewDockWidgetName; QMap _mapName2DockWidget; diff --git a/src/ui/QGCRGBDView.cc b/src/ui/QGCRGBDView.cc deleted file mode 100644 index 7f224d1b17e7771ffc141dc210daf1e664ff63c9..0000000000000000000000000000000000000000 --- a/src/ui/QGCRGBDView.cc +++ /dev/null @@ -1,249 +0,0 @@ -#include -#include -#include - -#include "QGCRGBDView.h" -#include "MultiVehicleManager.h" - -QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) : - HUD(width, height, parent), - rgbEnabled(false), - depthEnabled(false) -{ - enableRGBAction = new QAction(tr("Enable RGB Image"), this); - enableRGBAction->setStatusTip(tr("Show the RGB image live stream in this window")); - enableRGBAction->setCheckable(true); - enableRGBAction->setChecked(rgbEnabled); - connect(enableRGBAction, SIGNAL(triggered(bool)), this, SLOT(enableRGB(bool))); - - enableDepthAction = new QAction(tr("Enable Depthmap"), this); - enableDepthAction->setStatusTip(tr("Show the Depthmap in this window")); - enableDepthAction->setCheckable(true); - enableDepthAction->setChecked(depthEnabled); - connect(enableDepthAction, SIGNAL(triggered(bool)), this, SLOT(enableDepth(bool))); - - connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &QGCRGBDView::_activeVehicleChanged); - _activeVehicleChanged(MultiVehicleManager::instance()->activeVehicle()); - - clearData(); - loadSettings(); -} - -QGCRGBDView::~QGCRGBDView() -{ - storeSettings(); -} - -void QGCRGBDView::storeSettings() -{ - QSettings settings; - settings.beginGroup("QGC_RGBDWIDGET"); - settings.setValue("STREAM_RGB_ON", rgbEnabled); - settings.setValue("STREAM_DEPTH_ON", depthEnabled); - settings.endGroup(); -} - -void QGCRGBDView::loadSettings() -{ - QSettings settings; - settings.beginGroup("QGC_RGBDWIDGET"); - rgbEnabled = settings.value("STREAM_RGB_ON", rgbEnabled).toBool(); - // Only enable depth if RGB is not on - if (!rgbEnabled) depthEnabled = settings.value("STREAM_DEPTH_ON", depthEnabled).toBool(); - settings.endGroup(); -} - -void QGCRGBDView::_activeVehicleChanged(Vehicle* vehicle) -{ - if (this->uas != NULL) - { - // Disconnect any previously connected active MAV - disconnect(this->uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*))); - - clearData(); - } - - if (vehicle) - { - // Now connect the new UAS - // Setup communication - connect(vehicle->uas(), SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*))); - } -} - -void QGCRGBDView::clearData(void) -{ - QImage offlineImg; - qDebug() << offlineImg.load(":/files/images/status/colorbars.png"); - - glImage = offlineImg; -} - -void QGCRGBDView::contextMenuEvent(QContextMenuEvent* event) -{ - QMenu menu(this); - // Update actions - enableHUDAction->setChecked(HUDInstrumentsEnabled); - //enableVideoAction->setChecked(videoEnabled); - enableRGBAction->setChecked(rgbEnabled); - enableDepthAction->setChecked(depthEnabled); - - menu.addAction(enableHUDAction); - menu.addAction(enableRGBAction); - menu.addAction(enableDepthAction); - //menu.addAction(selectHUDColorAction); - //menu.addAction(enableVideoAction); - //menu.addAction(selectOfflineDirectoryAction); - //menu.addAction(selectVideoChannelAction); - menu.exec(event->globalPos()); -} - -void QGCRGBDView::enableRGB(bool enabled) -{ - rgbEnabled = enabled; - videoEnabled = rgbEnabled | depthEnabled; - QWidget::resize(size().width(), size().height()); -} - -void QGCRGBDView::enableDepth(bool enabled) -{ - depthEnabled = enabled; - videoEnabled = rgbEnabled | depthEnabled; - QWidget::resize(size().width(), size().height()); -} - -float colormapJet[128][3] = { - {0.0f,0.0f,0.53125f}, - {0.0f,0.0f,0.5625f}, - {0.0f,0.0f,0.59375f}, - {0.0f,0.0f,0.625f}, - {0.0f,0.0f,0.65625f}, - {0.0f,0.0f,0.6875f}, - {0.0f,0.0f,0.71875f}, - {0.0f,0.0f,0.75f}, - {0.0f,0.0f,0.78125f}, - {0.0f,0.0f,0.8125f}, - {0.0f,0.0f,0.84375f}, - {0.0f,0.0f,0.875f}, - {0.0f,0.0f,0.90625f}, - {0.0f,0.0f,0.9375f}, - {0.0f,0.0f,0.96875f}, - {0.0f,0.0f,1.0f}, - {0.0f,0.03125f,1.0f}, - {0.0f,0.0625f,1.0f}, - {0.0f,0.09375f,1.0f}, - {0.0f,0.125f,1.0f}, - {0.0f,0.15625f,1.0f}, - {0.0f,0.1875f,1.0f}, - {0.0f,0.21875f,1.0f}, - {0.0f,0.25f,1.0f}, - {0.0f,0.28125f,1.0f}, - {0.0f,0.3125f,1.0f}, - {0.0f,0.34375f,1.0f}, - {0.0f,0.375f,1.0f}, - {0.0f,0.40625f,1.0f}, - {0.0f,0.4375f,1.0f}, - {0.0f,0.46875f,1.0f}, - {0.0f,0.5f,1.0f}, - {0.0f,0.53125f,1.0f}, - {0.0f,0.5625f,1.0f}, - {0.0f,0.59375f,1.0f}, - {0.0f,0.625f,1.0f}, - {0.0f,0.65625f,1.0f}, - {0.0f,0.6875f,1.0f}, - {0.0f,0.71875f,1.0f}, - {0.0f,0.75f,1.0f}, - {0.0f,0.78125f,1.0f}, - {0.0f,0.8125f,1.0f}, - {0.0f,0.84375f,1.0f}, - {0.0f,0.875f,1.0f}, - {0.0f,0.90625f,1.0f}, - {0.0f,0.9375f,1.0f}, - {0.0f,0.96875f,1.0f}, - {0.0f,1.0f,1.0f}, - {0.03125f,1.0f,0.96875f}, - {0.0625f,1.0f,0.9375f}, - {0.09375f,1.0f,0.90625f}, - {0.125f,1.0f,0.875f}, - {0.15625f,1.0f,0.84375f}, - {0.1875f,1.0f,0.8125f}, - {0.21875f,1.0f,0.78125f}, - {0.25f,1.0f,0.75f}, - {0.28125f,1.0f,0.71875f}, - {0.3125f,1.0f,0.6875f}, - {0.34375f,1.0f,0.65625f}, - {0.375f,1.0f,0.625f}, - {0.40625f,1.0f,0.59375f}, - {0.4375f,1.0f,0.5625f}, - {0.46875f,1.0f,0.53125f}, - {0.5f,1.0f,0.5f}, - {0.53125f,1.0f,0.46875f}, - {0.5625f,1.0f,0.4375f}, - {0.59375f,1.0f,0.40625f}, - {0.625f,1.0f,0.375f}, - {0.65625f,1.0f,0.34375f}, - {0.6875f,1.0f,0.3125f}, - {0.71875f,1.0f,0.28125f}, - {0.75f,1.0f,0.25f}, - {0.78125f,1.0f,0.21875f}, - {0.8125f,1.0f,0.1875f}, - {0.84375f,1.0f,0.15625f}, - {0.875f,1.0f,0.125f}, - {0.90625f,1.0f,0.09375f}, - {0.9375f,1.0f,0.0625f}, - {0.96875f,1.0f,0.03125f}, - {1.0f,1.0f,0.0f}, - {1.0f,0.96875f,0.0f}, - {1.0f,0.9375f,0.0f}, - {1.0f,0.90625f,0.0f}, - {1.0f,0.875f,0.0f}, - {1.0f,0.84375f,0.0f}, - {1.0f,0.8125f,0.0f}, - {1.0f,0.78125f,0.0f}, - {1.0f,0.75f,0.0f}, - {1.0f,0.71875f,0.0f}, - {1.0f,0.6875f,0.0f}, - {1.0f,0.65625f,0.0f}, - {1.0f,0.625f,0.0f}, - {1.0f,0.59375f,0.0f}, - {1.0f,0.5625f,0.0f}, - {1.0f,0.53125f,0.0f}, - {1.0f,0.5f,0.0f}, - {1.0f,0.46875f,0.0f}, - {1.0f,0.4375f,0.0f}, - {1.0f,0.40625f,0.0f}, - {1.0f,0.375f,0.0f}, - {1.0f,0.34375f,0.0f}, - {1.0f,0.3125f,0.0f}, - {1.0f,0.28125f,0.0f}, - {1.0f,0.25f,0.0f}, - {1.0f,0.21875f,0.0f}, - {1.0f,0.1875f,0.0f}, - {1.0f,0.15625f,0.0f}, - {1.0f,0.125f,0.0f}, - {1.0f,0.09375f,0.0f}, - {1.0f,0.0625f,0.0f}, - {1.0f,0.03125f,0.0f}, - {1.0f,0.0f,0.0f}, - {0.96875f,0.0f,0.0f}, - {0.9375f,0.0f,0.0f}, - {0.90625f,0.0f,0.0f}, - {0.875f,0.0f,0.0f}, - {0.84375f,0.0f,0.0f}, - {0.8125f,0.0f,0.0f}, - {0.78125f,0.0f,0.0f}, - {0.75f,0.0f,0.0f}, - {0.71875f,0.0f,0.0f}, - {0.6875f,0.0f,0.0f}, - {0.65625f,0.0f,0.0f}, - {0.625f,0.0f,0.0f}, - {0.59375f,0.0f,0.0f}, - {0.5625f,0.0f,0.0f}, - {0.53125f,0.0f,0.0f}, - {0.5f,0.0f,0.0f} -}; - -void QGCRGBDView::updateData(UASInterface *uas) -{ - Q_UNUSED(uas); -} diff --git a/src/ui/QGCRGBDView.h b/src/ui/QGCRGBDView.h deleted file mode 100644 index 67c6f46dceb72465e61b6e3cb48f2494f417cee8..0000000000000000000000000000000000000000 --- a/src/ui/QGCRGBDView.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef QGCRGBDVIEW_H -#define QGCRGBDVIEW_H - -#include "HUD.h" -#include "Vehicle.h" - -class QGCRGBDView : public HUD -{ - Q_OBJECT -public: - explicit QGCRGBDView(int width=640, int height=480, QWidget *parent = 0); - ~QGCRGBDView(); - -signals: - -public slots: - void clearData(void); - void enableRGB(bool enabled); - void enableDepth(bool enabled); - void updateData(UASInterface *uas); - -protected: - bool rgbEnabled; - bool depthEnabled; - QAction* enableRGBAction; - QAction* enableDepthAction; - - void contextMenuEvent (QContextMenuEvent* event); - /** @brief Store current configuration of widget */ - void storeSettings(); - /** @brief Load configuration of widget */ - void loadSettings(); - -private slots: - void _activeVehicleChanged(Vehicle* vehicle); -}; - -#endif // QGCRGBDVIEW_H