Skip to content

Q
qgroundcontrol
Commit 1d0feb3d authored by Don Gagne's avatar Don Gagne
Browse files
Options

Remove HDD, HUD, HSI 

These use OpenGL implementation which prevented moving to ANGLE
support. Since they are rarely used they are being remove instead of
being upgrade.
parent 46fd2413
Hide whitespace changes
Inline Side-by-side
Showing with 0 additions and 5369 deletions
QGCApplication.pro
View file @ 1d0feb3d
......@@ -162,7 +162,6 @@ INCLUDEPATH += \
FORMS += \
src/QGCQmlWidgetHolder.ui \
src/ui/HDDisplay.ui \
src/ui/Linechart.ui \
src/ui/LogReplayLinkConfigurationWidget.ui \
src/ui/MainWindow.ui \
......@@ -272,9 +271,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 \
......@@ -314,7 +310,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 \
......@@ -405,9 +400,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 \
......@@ -447,7 +439,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 \
......
src/ui/HDDisplay.cc deleted 100644 → 0
View file @ 46fd2413
/*=====================================================================
======================================================================*/
/**
* @file
* @brief Implementation of Head Down Display (HDD)
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <QFile>
#include <QGLWidget>
#include <QStringList>
#include <QGraphicsTextItem>
#include <QDockWidget>
#include <QInputDialog>
#include <QMouseEvent>
#include <QMenu>
#include <QSettings>
#include <QDebug>
#include <qmath.h>
#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<QAction*> HDDisplay::getItemRemoveActions()
{
QList<QAction*> 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<QAction*>(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<QDockWidget*>(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<float, float> goodRange, QPair<float, float> 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<QString, double> 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);
//}
src/ui/HDDisplay.h deleted 100644 → 0
View file @ 46fd2413
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of Head Down Display (HDD)
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#ifndef HDDISPLAY_H
#define HDDISPLAY_H
#include <QGraphicsView>
#include <QColor>
#include <QTimer>
#include <QFontDatabase>
#include <QMap>
#include <QContextMenuEvent>
#include <QPair>
#include <cmath>
#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<QAction*> 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<float, float> goodRange, QPair<float, float> 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<QString, double> values; ///< The variables this HUD displays
QMap<QString, QString> units; ///< The units
QMap<QString, float> valuesDot; ///< First derivative of the variable
QMap<QString, float> valuesMean; ///< Mean since system startup for this variable
QMap<QString, int> valuesCount; ///< Number of values received so far
QMap<QString, quint64> lastUpdate; ///< The last update time for this variable
QMap<QString, float> minValues; ///< The minimum value this variable is assumed to have
QMap<QString, float> maxValues; ///< The maximum value this variable is assumed to have
QMap<QString, bool> symmetric; ///< Draw the gauge / dial symmetric bool = yes
QMap<QString, bool> intValues; ///< Is the gauge value an integer?
QMap<QString, QString> customNames; ///< Custom names for the data names
QMap<QString, QPair<float, float> > goodRanges; ///< The range of good values
QMap<QString, QPair<float, float> > 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
src/ui/HDDisplay.ui deleted 100644 → 0
View file @ 46fd2413
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>HDDisplay</class>
<widget class="QWidget" name="HDDisplay">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>400</width>
<height>300</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="QGraphicsView" name="view"/>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>
src/ui/HSIDisplay.cc deleted 100644 → 0
View file @ 46fd2413
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Implementation of Horizontal Situation Indicator class
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <QFile>
#include <QStringList>
#include <QPainter>
#include <QGraphicsScene>
#include <QHBoxLayout>
#include <QDoubleSpinBox>
#include <QDebug>
#include "MultiVehicleManager.h"
#include "HomePositionManager.h"
#include "HSIDisplay.h"
#include "QGC.h"
#include "MissionItem.h"
#include "UASWaypointManager.h"
#include <qmath.h>
#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<double>::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<int>(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<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
//MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((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 <ENTER> 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 <ENTER> 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 <ENTER> 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 <ENTER> 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->acceptanceRadius();
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<MissionItem*>& 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->frame();
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<int>(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<int, GPSSatellite*> 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);
}
src/ui/HSIDisplay.h deleted 100644 → 0
View file @ 46fd2413
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of of Horizontal Situation Indicator class
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#ifndef HSIDISPLAY_H
#define HSIDISPLAY_H
#include <QWidget>
#include <QColor>
#include <QTimer>
#include <QMap>
#include <QPair>
#include <QMouseEvent>
#include <cmath>
#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<int, QString> objectNames;
QMap<int, int> objectTypes;
QMap<int, float> objectQualities;
QMap<int, float> objectBearings;
QMap<int, float> objectDistances;
bool dragStarted;
bool leftDragStarted;
bool mouseHasMoved;
float startX;
float startY;
QTimer statusClearTimer;
QString statusMessage;
bool actionPending;
bool directSending;
QMap<int, GPSSatellite*> 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
src/ui/HUD.cc deleted 100644 → 0
View file @ 46fd2413
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Head Up Display (HUD)
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <QShowEvent>
#include <QContextMenuEvent>
#include <QMenu>
#include <QStandardPaths>
#include <QPaintEvent>
#include <QDebug>
#include <cmath>
#include <qmath.h>
#include <limits>
#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<UAS*>(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*>(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<int>(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<QString, float> 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*>(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);
}
}
src/ui/HUD.h deleted 100644 → 0
View file @ 46fd2413
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of Head up display
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#ifndef HUD_H
#define HUD_H
#include <QImage>
#include <QWidget>
#include <QLabel>
#include <QPainter>
#include <QFontDatabase>
#include <QTimer>
#include <QVector3D>
#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<uint8_t, QVector3D> 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
src/ui/MainWindow.cc
View file @ 1d0feb3d
......@@ -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);
......
src/ui/MainWindow.h
View file @ 1d0feb3d
......@@ -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<QString, QDockWidget*> _mapName2DockWidget;
......
src/ui/QGCRGBDView.cc deleted 100644 → 0
View file @ 46fd2413
#include <QMenu>
#include <QContextMenuEvent>
#include <QSettings>
#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);
}
src/ui/QGCRGBDView.h deleted 100644 → 0
View file @ 46fd2413
#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
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment