Commit c6ab1f05 authored by dongfang's avatar dongfang Committed by Michael Carpenter

First PrimaryFlightDisplay integration that will even compile. Non functionalgit status

parent d8b646be
......@@ -376,7 +376,8 @@ HEADERS += src/MG.h \
src/ui/submainwindow.h \
src/ui/dockwidgettitlebareventfilter.h \
src/ui/uas/UASQuickView.h \
src/ui/uas/UASQuickViewItem.h
src/ui/uas/UASQuickViewItem.h \
src/ui/PrimaryFlightDisplay.h
# Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010|win32-msvc2012::HEADERS += src/ui/map3D/QGCGoogleEarthView.h
......@@ -544,7 +545,8 @@ SOURCES += src/main.cc \
src/ui/submainwindow.cpp \
src/ui/dockwidgettitlebareventfilter.cpp \
src/ui/uas/UASQuickViewItem.cc \
src/ui/uas/UASQuickView.cc
src/ui/uas/UASQuickView.cc \
src/ui/PrimaryFlightDisplay.cpp
# Enable Google Earth only on Mac OS and Windows with Visual Studio compiler
macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010|win32-msvc2012::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc
......
......@@ -23,7 +23,7 @@ This file is part of the QGROUNDCONTROL project
/**
* @file
* @brief Head Up Display (HUD)
* @brief Head Up Display (HUD_old)
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
......@@ -52,14 +52,14 @@ This file is part of the QGROUNDCONTROL project
#endif
/**
* @warning The HUD widget will not start painting its content automatically
* to update the view, start the auto-update by calling HUD::start().
* @warning The HUD_old widget will not start painting its content automatically
* to update the view, start the auto-update by calling HUD_old::start().
*
* @param width
* @param height
* @param parent
*/
HUD::HUD(int width, int height, QWidget* parent)
HUD_old::HUD_old(int width, int height, QWidget* parent)
: QGLWidget(QGLFormat(QGL::SampleBuffers), parent),
uas(NULL),
yawInt(0.0f),
......@@ -119,7 +119,7 @@ HUD::HUD(int width, int height, QWidget* parent)
load(0.0f),
offlineDirectory(""),
nextOfflineImage(""),
hudInstrumentsEnabled(true),
HUDInstrumentsEnabled(true),
videoEnabled(false),
xImageFactor(1.0),
yImageFactor(1.0),
......@@ -151,7 +151,7 @@ HUD::HUD(int width, int height, QWidget* parent)
// Refresh timer
refreshTimer->setInterval(updateInterval);
connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD()));
connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD_old()));
// Resize to correct size and fill with image
resize(this->width(), this->height());
......@@ -186,17 +186,17 @@ HUD::HUD(int width, int height, QWidget* parent)
if (UASManager::instance()->getActiveUAS() != NULL) setActiveUAS(UASManager::instance()->getActiveUAS());
}
HUD::~HUD()
HUD_old::~HUD_old()
{
refreshTimer->stop();
}
QSize HUD::sizeHint() const
QSize HUD_old::sizeHint() const
{
return QSize(width(), (width()*3.0f)/4);
}
void HUD::showEvent(QShowEvent* event)
void HUD_old::showEvent(QShowEvent* event)
{
// React only to internal (pre-display)
// events
......@@ -205,7 +205,7 @@ void HUD::showEvent(QShowEvent* event)
emit visibilityChanged(true);
}
void HUD::hideEvent(QHideEvent* event)
void HUD_old::hideEvent(QHideEvent* event)
{
// React only to internal (pre-display)
// events
......@@ -214,27 +214,27 @@ void HUD::hideEvent(QHideEvent* event)
emit visibilityChanged(false);
}
void HUD::contextMenuEvent (QContextMenuEvent* event)
void HUD_old::contextMenuEvent (QContextMenuEvent* event)
{
QMenu menu(this);
// Update actions
enableHUDAction->setChecked(hudInstrumentsEnabled);
enableHUDAction->setChecked(HUDInstrumentsEnabled);
enableVideoAction->setChecked(videoEnabled);
menu.addAction(enableHUDAction);
//menu.addAction(selectHUDColorAction);
//menu.addAction(selectHUD_oldColorAction);
menu.addAction(enableVideoAction);
menu.addAction(selectOfflineDirectoryAction);
menu.addAction(selectSaveDirectoryAction);
menu.exec(event->globalPos());
}
void HUD::createActions()
void HUD_old::createActions()
{
enableHUDAction = new QAction(tr("Enable HUD"), this);
enableHUDAction->setStatusTip(tr("Show the HUD instruments in this window"));
enableHUDAction = new QAction(tr("Enable HUD_old"), this);
enableHUDAction->setStatusTip(tr("Show the HUD_old instruments in this window"));
enableHUDAction->setCheckable(true);
enableHUDAction->setChecked(hudInstrumentsEnabled);
enableHUDAction->setChecked(HUDInstrumentsEnabled);
connect(enableHUDAction, SIGNAL(triggered(bool)), this, SLOT(enableHUDInstruments(bool)));
enableVideoAction = new QAction(tr("Enable Video Live feed"), this);
......@@ -255,9 +255,9 @@ void HUD::createActions()
/**
*
* @param uas the UAS/MAV to monitor/display with the HUD
* @param uas the UAS/MAV to monitor/display with the HUD_old
*/
void HUD::setActiveUAS(UASInterface* uas)
void HUD_old::setActiveUAS(UASInterface* uas)
{
if (this->uas != NULL) {
// Disconnect any previously connected active MAV
......@@ -308,7 +308,7 @@ void HUD::setActiveUAS(UASInterface* uas)
}
}
//void HUD::updateAttitudeThrustSetPoint(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec)
//void HUD_old::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);
......@@ -316,7 +316,7 @@ void HUD::setActiveUAS(UASInterface* uas)
//// updateValue(uas, "thrust desired", thrustDesired, msec);
//}
void HUD::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp)
void HUD_old::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp)
{
Q_UNUSED(uas);
Q_UNUSED(timestamp);
......@@ -328,7 +328,7 @@ void HUD::updateAttitude(UASInterface* uas, double roll, double pitch, double ya
}
}
void HUD::updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp)
void HUD_old::updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp)
{
Q_UNUSED(uas);
Q_UNUSED(timestamp);
......@@ -338,7 +338,7 @@ void HUD::updateAttitude(UASInterface* uas, int component, double roll, double p
}
}
void HUD::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
void HUD_old::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
{
Q_UNUSED(uas);
Q_UNUSED(seconds);
......@@ -352,18 +352,18 @@ void HUD::updateBattery(UASInterface* uas, double voltage, double percent, int s
}
}
void HUD::receiveHeartbeat(UASInterface*)
void HUD_old::receiveHeartbeat(UASInterface*)
{
}
void HUD::updateThrust(UASInterface* uas, double thrust)
void HUD_old::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)
void HUD_old::updateLocalPosition(UASInterface* uas,double x,double y,double z,quint64 timestamp)
{
Q_UNUSED(uas);
Q_UNUSED(timestamp);
......@@ -372,7 +372,7 @@ void HUD::updateLocalPosition(UASInterface* uas,double x,double y,double z,quint
this->zPos = z;
}
void HUD::updateGlobalPosition(UASInterface* uas,double lat, double lon, double altitude, quint64 timestamp)
void HUD_old::updateGlobalPosition(UASInterface* uas,double lat, double lon, double altitude, quint64 timestamp)
{
Q_UNUSED(uas);
Q_UNUSED(timestamp);
......@@ -381,7 +381,7 @@ void HUD::updateGlobalPosition(UASInterface* uas,double lat, double lon, double
this->alt = altitude;
}
void HUD::updateSpeed(UASInterface* uas,double x,double y,double z,quint64 timestamp)
void HUD_old::updateSpeed(UASInterface* uas,double x,double y,double z,quint64 timestamp)
{
Q_UNUSED(uas);
Q_UNUSED(timestamp);
......@@ -397,9 +397,9 @@ void HUD::updateSpeed(UASInterface* uas,double x,double y,double z,quint64 times
* 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
* @param state short state text, displayed in HUD_old
*/
void HUD::updateState(UASInterface* uas,QString state)
void HUD_old::updateState(UASInterface* uas,QString state)
{
// Only one UAS is connected at a time
Q_UNUSED(uas);
......@@ -410,9 +410,9 @@ void HUD::updateState(UASInterface* uas,QString 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
* @param mode short mode text, displayed in HUD_old
*/
void HUD::updateMode(int id,QString mode, QString description)
void HUD_old::updateMode(int id,QString mode, QString description)
{
// Only one UAS is connected at a time
Q_UNUSED(id);
......@@ -420,7 +420,7 @@ void HUD::updateMode(int id,QString mode, QString description)
this->mode = mode;
}
void HUD::updateLoad(UASInterface* uas, double load)
void HUD_old::updateLoad(UASInterface* uas, double load)
{
Q_UNUSED(uas);
this->load = load;
......@@ -431,7 +431,7 @@ void HUD::updateLoad(UASInterface* uas, double load)
* @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)
float HUD_old::refToScreenX(float x)
{
//qDebug() << "sX: " << (scalingFactor * x) << "Orig:" << x;
return (scalingFactor * x);
......@@ -440,7 +440,7 @@ float HUD::refToScreenX(float 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)
float HUD_old::refToScreenY(float y)
{
//qDebug() << "sY: " << (scalingFactor * y);
return (scalingFactor * y);
......@@ -451,7 +451,7 @@ float HUD::refToScreenY(float y)
* the x and y center offsets.
*
*/
void HUD::paintCenterBackground(float roll, float pitch, float yaw)
void HUD_old::paintCenterBackground(float roll, float pitch, float yaw)
{
Q_UNUSED(yaw);
......@@ -459,7 +459,7 @@ void HUD::paintCenterBackground(float roll, float pitch, float yaw)
float referenceWidth = 70.0;
float referenceHeight = 70.0;
// HUD is assumed to be 200 x 150 mm
// HUD_old is assumed to be 200 x 150 mm
// so that positions can be hardcoded
// but can of course be scaled.
......@@ -522,7 +522,7 @@ void HUD::paintCenterBackground(float roll, float pitch, float yaw)
* @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)
void HUD_old::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);
......@@ -546,7 +546,7 @@ void HUD::paintText(QString text, QColor color, float fontSize, float refX, floa
painter->setPen(prevPen);
}
void HUD::initializeGL()
void HUD_old::initializeGL()
{
bool antialiasing = true;
......@@ -575,7 +575,7 @@ void HUD::initializeGL()
* @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)
void HUD_old::setupGLView(float referencePositionX, float referencePositionY, float referenceWidth, float referenceHeight)
{
int pixelWidth = (int)(referenceWidth * scalingFactor);
int pixelHeight = (int)(referenceHeight * scalingFactor);
......@@ -601,12 +601,12 @@ void HUD::setupGLView(float referencePositionX, float referencePositionY, float
//glScalef(scaleX, scaleY, 1.0f);
}
void HUD::paintRollPitchStrips()
void HUD_old::paintRollPitchStrips()
{
}
void HUD::paintEvent(QPaintEvent *event)
void HUD_old::paintEvent(QPaintEvent *event)
{
// Event is not needed
// the event is ignored as this widget
......@@ -614,7 +614,7 @@ void HUD::paintEvent(QPaintEvent *event)
Q_UNUSED(event);
}
void HUD::paintHUD()
void HUD_old::paintHUD_old()
{
if (isVisible()) {
// static quint64 interval = 0;
......@@ -711,7 +711,7 @@ void HUD::paintHUD()
// END OF OPENGL PAINTING
if (hudInstrumentsEnabled) {
if (HUDInstrumentsEnabled) {
//glEnable(GL_MULTISAMPLE);
......@@ -900,7 +900,7 @@ void HUD::paintHUD()
/**
* @param pitch pitch angle in degrees (-180 to 180)
*/
void HUD::paintPitchLines(float pitch, QPainter* painter)
void HUD_old::paintPitchLines(float pitch, QPainter* painter)
{
QString label;
......@@ -967,7 +967,7 @@ void HUD::paintPitchLines(float pitch, QPainter* painter)
}
}
void HUD::paintPitchLinePos(QString text, float refPosX, float refPosY, QPainter* painter)
void HUD_old::paintPitchLinePos(QString text, float refPosX, float refPosY, QPainter* painter)
{
//painter->setPen(QPen(QBrush, normalStrokeWidth));
......@@ -996,7 +996,7 @@ void HUD::paintPitchLinePos(QString text, float refPosX, float refPosY, QPainter
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)
void HUD_old::paintPitchLineNeg(QString text, float refPosX, float refPosY, QPainter* painter)
{
const float pitchWidth = 30.0f;
const float pitchGap = pitchWidth / 2.5f;
......@@ -1046,7 +1046,7 @@ void rotatePointClockWise(QPointF& p, float angle)
p.setY((-1.0f * sin(angle) * p.x()) + cos(angle) * p.y());
}
float HUD::refLineWidthToPen(float line)
float HUD_old::refLineWidthToPen(float line)
{
return line * 2.50f;
}
......@@ -1059,7 +1059,7 @@ float HUD::refLineWidthToPen(float line)
* @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)
void HUD_old::rotatePolygonClockWiseRad(QPolygonF& p, float angle, QPointF origin)
{
// Standard 2x2 rotation matrix, counter-clockwise
//
......@@ -1078,7 +1078,7 @@ void HUD::rotatePolygonClockWiseRad(QPolygonF& p, float angle, QPointF origin)
}
}
void HUD::drawPolygon(QPolygonF refPolygon, QPainter* painter)
void HUD_old::drawPolygon(QPolygonF refPolygon, QPainter* painter)
{
// Scale coordinates
QPolygonF draw(refPolygon.size());
......@@ -1091,7 +1091,7 @@ void HUD::drawPolygon(QPolygonF refPolygon, QPainter* painter)
painter->drawPolygon(draw);
}
void HUD::drawChangeRateStrip(float xRef, float yRef, float height, float minRate, float maxRate, float value, QPainter* painter,bool reverse)
void HUD_old::drawChangeRateStrip(float xRef, float yRef, float height, float minRate, float maxRate, float value, QPainter* painter,bool reverse)
{
float scaledValue = value;
......@@ -1154,7 +1154,7 @@ void HUD::drawChangeRateStrip(float xRef, float yRef, float height, float minRat
}
}
//void HUD::drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter)
//void HUD_old::drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter)
//{
// Q_UNUSED(maxWidth);
// Q_UNUSED(maxHeight);
......@@ -1231,7 +1231,7 @@ void HUD::drawChangeRateStrip(float xRef, float yRef, float height, float minRat
// }
//}
void HUD::drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid)
void HUD_old::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);
......@@ -1277,7 +1277,7 @@ void HUD::drawChangeIndicatorGauge(float xRef, float yRef, float radius, float e
drawPolygon(p, painter);
}
void HUD::drawLine(float refX1, float refY1, float refX2, float refY2, float width, const QColor& color, QPainter* painter)
void HUD_old::drawLine(float refX1, float refY1, float refX2, float refY2, float width, const QColor& color, QPainter* painter)
{
QPen pen(Qt::SolidLine);
pen.setWidth(refLineWidthToPen(width));
......@@ -1286,7 +1286,7 @@ void HUD::drawLine(float refX1, float refY1, float refX2, float refY2, float wid
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)
void HUD_old::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);
......@@ -1297,12 +1297,12 @@ void HUD::drawEllipse(float refX, float refY, float radiusX, float radiusY, floa
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)
void HUD_old::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::resizeGL(int w, int h)
void HUD_old::resizeGL(int w, int h)
{
if (isVisible()) {
glViewport(0, 0, w, h);
......@@ -1312,17 +1312,17 @@ void HUD::resizeGL(int w, int h)
glMatrixMode(GL_MODELVIEW);
glPolygonMode(GL_FRONT, GL_FILL);
//FIXME
paintHUD();
paintHUD_old();
}
}
void HUD::selectWaypoint(int uasId, int id)
void HUD_old::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)
void HUD_old::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) {
......@@ -1378,10 +1378,10 @@ void HUD::setImageSize(int width, int height, int depth, int channels)
}
void HUD::startImage(int imgid, int width, int height, int depth, int channels)
void HUD_old::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";
//qDebug() << "HUD_old: starting image (" << width << "x" << height << ", " << depth << "bits) with " << channels << "channels";
// Copy previous image to screen if it hasn't been finished properly
finishImage();
......@@ -1391,7 +1391,7 @@ void HUD::startImage(int imgid, int width, int height, int depth, int channels)
imageStarted = true;
}
void HUD::finishImage()
void HUD_old::finishImage()
{
if (imageStarted) {
commitRawDataToGL();
......@@ -1399,7 +1399,7 @@ void HUD::finishImage()
}
}
void HUD::commitRawDataToGL()
void HUD_old::commitRawDataToGL()
{
qDebug() << __FILE__ << __LINE__ << "Copying raw data to GL buffer:" << rawImage << receivedWidth << receivedHeight << image->format();
if (image != NULL) {
......@@ -1429,19 +1429,19 @@ void HUD::commitRawDataToGL()
update();
}
void HUD::saveImage(QString fileName)
void HUD_old::saveImage(QString fileName)
{
image->save(fileName);
}
void HUD::saveImage()
void HUD_old::saveImage()
{
//Bring up popup
QString fileName = "output.png";
saveImage(fileName);
}
void HUD::startImage(quint64 timestamp)
void HUD_old::startImage(quint64 timestamp)
{
if (videoEnabled && offlineDirectory != "") {
// Load and diplay image file
......@@ -1449,7 +1449,7 @@ void HUD::startImage(quint64 timestamp)
}
}
void HUD::selectOfflineDirectory()
void HUD_old::selectOfflineDirectory()
{
QString fileName = QFileDialog::getExistingDirectory(this, tr("Select image directory"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation));
if (fileName != "") {
......@@ -1457,17 +1457,17 @@ void HUD::selectOfflineDirectory()
}
}
void HUD::enableHUDInstruments(bool enabled)
void HUD_old::enableHUD_oldInstruments(bool enabled)
{
hudInstrumentsEnabled = enabled;
HUDInstrumentsEnabled = enabled;
}
void HUD::enableVideo(bool enabled)
void HUD_old::enableVideo(bool enabled)
{
videoEnabled = enabled;
}
void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int startIndex)
void HUD_old::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)";
......@@ -1478,7 +1478,7 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s
if (startIndex+length > rawExpectedBytes)
{
qDebug() << "HUD: OVERFLOW! startIndex:" << startIndex << "length:" << length << "image raw size" << ((receivedWidth * receivedHeight * receivedChannels * receivedDepth) / 8) - 1;
qDebug() << "HUD_old: OVERFLOW! startIndex:" << startIndex << "length:" << length << "image raw size" << ((receivedWidth * receivedHeight * receivedChannels * receivedDepth) / 8) - 1;
}
else
{
......@@ -1489,7 +1489,7 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s
// Check if we just reached the end of the image
if (startIndex+length == rawExpectedBytes)
{
//qDebug() << "HUD: END OF IMAGE REACHED!";
//qDebug() << "HUD_old: END OF IMAGE REACHED!";
finishImage();
rawLastIndex = 0;
}
......@@ -1507,11 +1507,11 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s
}
}
void HUD::copyImage()
void HUD_old::copyImage()
{
if (isVisible() && hudInstrumentsEnabled)
if (isVisible() && HUDInstrumentsEnabled)
{
//qDebug() << "HUD::copyImage()";
//qDebug() << "HUD_old::copyImage()";
UAS* u = dynamic_cast<UAS*>(this->uas);
if (u)
{
......@@ -1527,7 +1527,7 @@ void HUD::copyImage()
}
}
void HUD::saveImages(bool save)
void HUD_old::saveImages(bool save)
{
if (save)
{
......
......@@ -29,8 +29,8 @@ This file is part of the QGROUNDCONTROL project
*
*/
#ifndef HUD_H
#define HUD_H
#ifndef HUD_old_H
#define HUD_old_H
#include <QImage>
#include <QGLWidget>
......@@ -41,18 +41,18 @@ This file is part of the QGROUNDCONTROL project
#include "UASInterface.h"
/**
* @brief Displays a Head Up Display (HUD)
* @brief Displays a Head Up Display (HUD_old)
*
* 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-
* This class represents a head up display (HUD_old) and draws this HUD_old in an OpenGL widget (QGLWidget).
* It can superimpose the HUD_old 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 QGLWidget
class HUD_old : public QGLWidget
{
Q_OBJECT
public:
HUD(int width = 640, int height = 480, QWidget* parent = NULL);
~HUD();
HUD_old(int width = 640, int height = 480, QWidget* parent = NULL);
~HUD_old();
void setImageSize(int width, int height, int depth, int channels);
void resizeGL(int w, int h);
......@@ -90,7 +90,7 @@ public slots:
/** @brief Select directory where to load the offline files from */
void selectOfflineDirectory();
/** @brief Enable the HUD instruments */
void enableHUDInstruments(bool enabled);
void enableHUD_oldInstruments(bool enabled);
/** @brief Enable Video */
void enableVideo(bool enabled);
/** @brief Copy an image from the current active UAS */
......@@ -103,9 +103,9 @@ protected slots:
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 */
/** @brief Setup the OpenGL view for drawing a sub-component of the HUD_old */
void setupGLView(float referencePositionX, float referencePositionY, float referenceWidth, float referenceHeight);
void paintHUD();
void paintHUD_old();
void paintPitchLinePos(QString text, float refPosX, float refPosY, QPainter* painter);
void paintPitchLineNeg(QString text, float refPosX, float refPosY, QPainter* painter);
......@@ -156,8 +156,8 @@ protected:
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
int xCenter; ///< Center of the HUD_old 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_old 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
......@@ -172,8 +172,8 @@ protected:
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
// HUD_old colors
QColor defaultColor; ///< Color for most HUD_old 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
......@@ -184,17 +184,17 @@ protected:
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)
QPainter* HUD_oldPainter;
QFont font; ///< The HUD_old font, per default the free Bitstream Vera SANS, which is very close to actual HUD_old fonts
QFontDatabase fontDatabase;///< Font database, only used to load the TrueType font file (the HUD_old 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
float strongStrokeWidth; ///< Strong line stroke width, used throughout the HUD_old
float normalStrokeWidth; ///< Normal line stroke width, used throughout the HUD_old
float fineStrokeWidth; ///< Fine line stroke width, used throughout the HUD_old
QString waypointName; ///< Waypoint name displayed in HUD
QString waypointName; ///< Waypoint name displayed in HUD_old
float roll;
float pitch;
float yaw;
......@@ -218,7 +218,7 @@ protected:
float load;
QString offlineDirectory;
QString nextOfflineImage;
bool hudInstrumentsEnabled;
bool HUDInstrumentsEnabled;
bool videoEnabled;
bool dataStreamEnabled;
bool imageLoggingEnabled;
......@@ -235,4 +235,4 @@ protected:
unsigned int imageLogCounter;
};
#endif // HUD_H
#endif // HUD_old_H
......@@ -59,6 +59,7 @@ This file is part of the QGROUNDCONTROL project
#include "QGCFirmwareUpdate.h"
#include "QGCStatusBar.h"
#include "UASQuickView.h"
#include "PrimaryFlightDisplay.h"
#ifdef QGC_OSG_ENABLED
#include "Q3DWidgetFactory.h"
......@@ -566,11 +567,14 @@ void MainWindow::buildCommonWidgets()
connect(tempAction,SIGNAL(triggered(bool)),this, SLOT(showTool(bool)));
}
createDockWidget(simView,new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_SIMULATION,Qt::RightDockWidgetArea,this->width()/1.5);
// createDockWidget(simView,new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_SIMULATION,Qt::RightDockWidgetArea,this->width()/1.5);
createDockWidget(simView,new PrimaryFlightDisplay(320,240,this),tr("Primary Flight Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",VIEW_SIMULATION,Qt::RightDockWidgetArea,this->width()/1.5);
createDockWidget(pilotView,new UASListWidget(this),tr("Unmanned Systems"),"UNMANNED_SYSTEM_LIST_DOCKWIDGET",VIEW_FLIGHT,Qt::RightDockWidgetArea);
createDockWidget(pilotView,new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea,this->width()/1.8);
//createDockWidget(pilotView,new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea,this->width()/1.8);
createDockWidget(pilotView,new PrimaryFlightDisplay(320,240,this),tr("Primary Flight Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea,this->width()/1.8);
createDockWidget(pilotView,new UASQuickView(this),tr("Quick View"),"UAS_INFO_QUICKVIEW_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea);
createDockWidget(pilotView,new HSIDisplay(this),tr("Horizontal Situation"),"HORIZONTAL_SITUATION_INDICATOR_DOCKWIDGET",VIEW_FLIGHT,Qt::LeftDockWidgetArea);
pilotView->setTabPosition(Qt::LeftDockWidgetArea,QTabWidget::North);
......@@ -780,9 +784,10 @@ void MainWindow::loadDockWidget(QString name)
qDebug() << "Error loading window:" << name << "Unknown window type";
//createDockWidget(centerStack->currentWidget(),hddisplay,tr("Actuator Status"),"HEADS_DOWN_DISPLAY_2_DOCKWIDGET",currentView,Qt::RightDockWidgetArea);
}
else if (name == "HEAD_UP_DISPLAY_DOCKWIDGET")
else if (name == "PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET")
{
createDockWidget(centerStack->currentWidget(),new HUD(320,240,this),tr("Head Up Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",currentView,Qt::RightDockWidgetArea);
// createDockWidget(centerStack->currentWidget(),new HUD(320,240,this),tr("Head Up Display"),"PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET",currentView,Qt::RightDockWidgetArea);
createDockWidget(centerStack->currentWidget(),new PrimaryFlightDisplay(320,240,this),tr("Primary Flight Display"),"HEAD_UP_DISPLAY_DOCKWIDGET",currentView,Qt::RightDockWidgetArea);
}
else if (name == "UAS_INFO_QUICKVIEW_DOCKWIDGET")
{
......
#include "PrimaryFlightDisplay.h"
#include "UASManager.h"
//#include "ui_primaryflightdisplay.h"
#include <QDebug>
#include <QRectF>
#include <cmath>
#include <QPen>
#include <QPainter>
#include <QPainterPath>
#include <QResizeEvent>
#include <math.h>
/*
*@TODO:
* global fixed pens
* repaint on demand multiple canvases
* multi implementation with shared model class
*/
const int PrimaryFlightDisplay::tickValues[] = {10, 20, 30, 45, 60};
const QString PrimaryFlightDisplay::compassWindNames[] = {
QString("N"),
QString("NE"),
QString("E"),
QString("SE"),
QString("S"),
QString("SW"),
QString("W"),
QString("NW")
};
PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *parent) :
QWidget(parent),
roll(0),
pitch(0),
heading(NAN),
aboveASLAltitude(0),
GPSAltitude(0),
aboveHomeAltitude(0),
groundSpeed(0),
airSpeed(0),
mode(""),
state(""),
load(0),
HUDInstrumentBackground(QColor::fromHsvF(0, 0, 0.8, 0.3)),
font("Bitstream Vera Sans"),
refreshTimer(new QTimer(this)),
uas(NULL),
// energyStatus(tr("??.?V (??m:??s)"))
batteryVoltage(UNKNOWN_BATTERY),
batteryCharge(UNKNOWN_BATTERY),
whitePen(Qt::white),
redPen(QColor::fromHsvF(0, 0.6, 0.8)),
amberPen(QColor::fromHsvF(0.12, 0.6, 1.0)),
greenPen(QColor::fromHsvF(0.25, 0.8, 0.8)),
blackPen(QColor::fromHsvF(0, 0, 0.2)),
#ifdef SEPARATE_LAYOUT
instrumentEdgePen(QColor::fromHsvF(0, 0, 0.65, 1.0)),
instrumentBackground(QColor::fromHsvF(0, 0, 0.3, 1.0))
#else
instrumentEdgePen(QColor::fromHsvF(0, 0, 0.3, 0.5)),
instrumentBackground(QColor::fromHsvF(0, 0, 0.3, 0.3))
#endif
{
setMinimumSize(120, 80);
setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
// Connect with UAS
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
if (UASManager::instance()->getActiveUAS() != NULL) setActiveUAS(UASManager::instance()->getActiveUAS());
// Refresh timer
refreshTimer->setInterval(updateInterval);
connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD()));
}
PrimaryFlightDisplay::~PrimaryFlightDisplay()
{
refreshTimer->stop();
}
QSize PrimaryFlightDisplay::sizeHint() const
{
return QSize(width(), (width()*3.0f)/4);
}
void PrimaryFlightDisplay::showEvent(QShowEvent* event)
{
// React only to internal (pre-display)
// events
QWidget::showEvent(event);
refreshTimer->start(updateInterval);
emit visibilityChanged(true);
}
void PrimaryFlightDisplay::hideEvent(QHideEvent* event)
{
// React only to internal (pre-display)
// events
refreshTimer->stop();
QWidget::hideEvent(event);
emit visibilityChanged(false);
}
/*
* Interface against qgroundcontrol
*/
/**
*
* @param uas the UAS/MAV to monitor/display with the HUD
*/
void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas)
{
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, int)), this, SLOT(updateBattery(UASInterface*, 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,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
disconnect(this->uas, SIGNAL(speedChanged(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)));
}
if (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, int)), this, SLOT(updateBattery(UASInterface*, 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,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int)));
// Set new UAS
this->uas = uas;
}
}
void PrimaryFlightDisplay::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))
{
// TODO: Units conversion?
this->roll = roll;
this->pitch = pitch;
this->heading = yaw;
}
// TODO: Else-part. We really should have an "attitude bad or unknown" indication instead of just freezing.
}
/*
* TODO! Implementation or removal of this.
* Currently a dummy.
*/
void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp)
{
Q_UNUSED(uas);
Q_UNUSED(component);
Q_UNUSED(roll);
Q_UNUSED(pitch);
Q_UNUSED(yaw);
Q_UNUSED(timestamp);
}
void PrimaryFlightDisplay::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
{
Q_UNUSED(uas);
Q_UNUSED(seconds);
/*
energyStatus = tr("BAT [%1V | %2V%]").arg(voltage, 4, 'f', 1, QChar('0')).arg(percent, 2, 'f', 0, QChar('0'));
if (percent < 20.0f) {
fuelColor = warningColor;
} else if (percent < 10.0f) {
fuelColor = criticalColor;
} else {
fuelColor = infoColor;
}
*/
batteryVoltage = voltage;
batteryCharge = percent;
}
void PrimaryFlightDisplay::receiveHeartbeat(UASInterface*)
{
}
void PrimaryFlightDisplay::updateThrust(UASInterface* uas, double thrust)
{
Q_UNUSED(uas);
Q_UNUSED(thrust);
}
/*
* TODO! Implementation or removal of this.
* Currently a dummy.
*/
void PrimaryFlightDisplay::updateLocalPosition(UASInterface* uas,double x,double y,double z,quint64 timestamp)
{
Q_UNUSED(uas);
Q_UNUSED(x);
Q_UNUSED(y);
Q_UNUSED(z);
Q_UNUSED(timestamp);
}
void PrimaryFlightDisplay::updateGlobalPosition(UASInterface* uas,double lat, double lon, double altitude, quint64 timestamp)
{
Q_UNUSED(uas);
Q_UNUSED(lat);
Q_UNUSED(lon);
Q_UNUSED(timestamp);
// TODO: Examine whether this is really the GPS alt or the mix-alt coming in.
GPSAltitude = altitude;
}
/*
* TODO! Examine what data comes with this call, should we consider it airspeed, ground speed or
* should we not consider it at all?
*/
void PrimaryFlightDisplay::updateSpeed(UASInterface* uas,double x,double y,double z,quint64 timestamp)
{
Q_UNUSED(uas);
Q_UNUSED(timestamp);
Q_UNUSED(z);
/*
this->xSpeed = x;
this->ySpeed = y;
this->zSpeed = z;
*/
double newTotalSpeed = sqrt(x*x + y*y/*+ zSpeed*zSpeed */);
// totalAcc = (newTotalSpeed - totalSpeed) / ((double)(lastSpeedUpdate - timestamp)/1000.0);
groundSpeed = newTotalSpeed;
}
void PrimaryFlightDisplay::updateState(UASInterface* uas,QString state)
{
// Only one UAS is connected at a time
Q_UNUSED(uas);
this->state = state;
}
void PrimaryFlightDisplay::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 PrimaryFlightDisplay::updateLoad(UASInterface* uas, double load)
{
Q_UNUSED(uas);
this->load = load;
//updateValue(uas, "load", load, MG::TIME::getGroundTimeNow());
}
void PrimaryFlightDisplay::selectWaypoint(int uasId, int id) {
}
//void paintEvent(QPaintEvent *event);
//void resizeEvent(QResizeEvent *e);
/*
* Private and such
*/
void PrimaryFlightDisplay::resizeEvent(QResizeEvent *e) {
QWidget::resizeEvent(e);
whitePen.setWidthF(LINEWIDTH * e->size().height());
redPen.setWidthF(LINEWIDTH * e->size().height());
greenPen.setWidthF(LINEWIDTH * e->size().height());
instrumentEdgePen.setWidthF(LINEWIDTH * e->size().height());
//doPaint();
}
void PrimaryFlightDisplay::paintEvent(QPaintEvent *event)
{
// Event is not needed
// the event is ignored as this widget
// is refreshed automatically
Q_UNUSED(event);
makeDummyData();
doPaint();
}
void PrimaryFlightDisplay::paintOnTimer() {
/*
*TODO: if not visible, exit (well OK not critical, the timer should get stopped anyways)
*The whole tainted-flag shebang.
*well not really so critical. Worst problem is deletion?
*/
makeDummyData();
doPaint();
}
void PrimaryFlightDisplay::drawTextCenter (
QPainter& painter,
QString text,
float fontSize,
float x,
float y)
{
font.setPixelSize(fontSize*height());
painter.setFont(font);
QFontMetrics metrics = QFontMetrics(font);
QRect bounds = metrics.boundingRect(text);
int flags = Qt::AlignCenter | Qt::TextDontClip; // For some reason the bounds rect is too small!
painter.drawText(x /*+bounds.x()*/ -bounds.width()/2, y /*+bounds.y()*/ -bounds.height()/2, bounds.width(), bounds.height(), flags, text);
}
void PrimaryFlightDisplay::drawTextLeftCenter (
QPainter& painter,
QString text,
float fontSize,
float x,
float y)
{
font.setPixelSize(fontSize*height());
painter.setFont(font);
QFontMetrics metrics = QFontMetrics(font);
QRect bounds = metrics.boundingRect(text);
int flags = Qt::AlignLeft | Qt::TextDontClip; // For some reason the bounds rect is too small!
painter.drawText(x /*+bounds.x()*/, y /*+bounds.y()*/ -bounds.height()/2, bounds.width(), bounds.height(), flags, text);
}
void PrimaryFlightDisplay::drawTextRightCenter (
QPainter& painter,
QString text,
float fontSize,
float x,
float y)
{
font.setPixelSize(fontSize*height());
painter.setFont(font);
QFontMetrics metrics = QFontMetrics(font);
QRect bounds = metrics.boundingRect(text);
int flags = Qt::AlignRight | Qt::TextDontClip; // For some reason the bounds rect is too small!
painter.drawText(x /*+bounds.x()*/ -bounds.width(), y /*+bounds.y()*/ -bounds.height()/2, bounds.width(), bounds.height(), flags, text);
}
void PrimaryFlightDisplay::drawTextCenterTop (
QPainter& painter,
QString text,
float fontSize,
float x,
float y)
{
font.setPixelSize(fontSize*height());
painter.setFont(font);
QFontMetrics metrics = QFontMetrics(font);
QRect bounds = metrics.boundingRect(text);
int flags = Qt::AlignCenter | Qt::TextDontClip; // For some reason the bounds rect is too small!
painter.drawText(x /*+bounds.x()*/ -bounds.width()/2, y+bounds.height() /*+bounds.y()*/, bounds.width(), bounds.height(), flags, text);
}
void PrimaryFlightDisplay::drawTextCenterBottom (
QPainter& painter,
QString text,
float fontSize,
float x,
float y)
{
font.setPixelSize(fontSize*height());
painter.setFont(font);
QFontMetrics metrics = QFontMetrics(font);
QRect bounds = metrics.boundingRect(text);
int flags = Qt::AlignCenter;
painter.drawText(x /*+bounds.x()*/ -bounds.width()/2, y /*+bounds.y()*/, bounds.width(), bounds.height(), flags, text);
}
void PrimaryFlightDisplay::drawInstrumentBackground(QPainter& painter, QRectF edge) {
painter.setPen(instrumentEdgePen);
painter.drawRect(edge);
}
void PrimaryFlightDisplay::fillInstrumentBackground(QPainter& painter, QRectF edge) {
painter.setPen(instrumentEdgePen);
painter.setBrush(instrumentBackground);
painter.drawRect(edge);
painter.setBrush(Qt::NoBrush);
}
/*
void PrimaryFlightDisplay::prepareTransform(QPainter& painter, qreal width, qreal height) {
painter.resetTransform();
painter.translate(width/2, height/2);
}
void PrimaryFlightDisplay::transformToGlobalSystem(QPainter& painter, qreal width, qreal height, float roll, float pitch) {
prepareTransform(painter, width, height);
painter.rotate(roll);
painter.translate(0, pitch*height/PITCHTRANSLATION);
}
*/
qreal pitchAngleToTranslation(qreal viewHeight, float pitch) {
return pitch*viewHeight/PITCHTRANSLATION;
}
void PrimaryFlightDisplay::drawAIAirframeFixedFeatures(QPainter& painter, QRectF area) {
// red line from -7/10 to -5/10 half-width
// red line from 7/10 to 5/10 half-width
// red slanted line from -2/10 half-width to 0
// red slanted line from 2/10 half-width to 0
// red arrow thing under roll scale
// prepareTransform(painter, width, height);
painter.resetTransform();
painter.translate(area.center());
qreal w = area.width();
qreal h = area.height();
painter.setPen(redPen);
QPointF start(-0.35*w, 0);
QPointF end(-0.25*w, 0);
painter.drawLine(start, end);
start = QPointF(0.35*w, 0);
end = QPointF(0.25*w, 0);
painter.drawLine(start, end);
start = QPointF(0.0894*w, 0.894*0.05*w);
end = QPoint(0, 0);
// end = QPointF(-0.0894*width, -0.894*0.05*width);
painter.drawLine(start, end);
start = QPointF(-0.0894*w, 0.894*0.05*w);
end = QPoint(0, 0);
// end = QPointF(0.0894*width, -0.894*0.05*width);
painter.drawLine(start, end);
painter.setPen(redPen);
// painter.setBrush(Qt::SolidPattern);
QPainterPath markerPath(QPointF(0, -h*ROLL_SCALE_RADIUS+1));
markerPath.lineTo(-h*ROLL_SCALE_MARKERWIDTH/2, -h*(ROLL_SCALE_RADIUS-ROLL_SCALE_MARKERHEIGHT)+1);
markerPath.lineTo(h*ROLL_SCALE_MARKERWIDTH/2, -h*(ROLL_SCALE_RADIUS-ROLL_SCALE_MARKERHEIGHT)+1);
markerPath.closeSubpath();
painter.drawPath(markerPath);
}
void PrimaryFlightDisplay::drawAIGlobalFeatures(
QPainter& painter,
QRectF area) {
painter.resetTransform();
painter.translate(area.center());
painter.rotate(roll);
painter.translate(0, pitchAngleToTranslation(area.height(), pitch));
qreal w = area.width();
qreal h = area.height();
qreal halfsize= 90/PITCHTRANSLATION*h + w*sqrt(2)/2;
QPainterPath skyPath(QPointF(-w, 0));
skyPath.lineTo(-halfsize, -halfsize);
skyPath.lineTo(halfsize, -halfsize);
skyPath.lineTo(halfsize, 0);
skyPath.closeSubpath();
QLinearGradient skyGradient(0, -halfsize, 0, 0);
skyGradient.setColorAt(0, QColor::fromHsvF(0.6, 1.0, 0.7));
skyGradient.setColorAt(1, QColor::fromHsvF(0.6, 0.25, 0.9));
QBrush skyBrush(skyGradient);
painter.fillPath(skyPath, skyBrush);
QPainterPath groundPath(QPointF(-halfsize, 0));
groundPath.lineTo(-halfsize, halfsize);
groundPath.lineTo(halfsize, halfsize);
groundPath.lineTo(halfsize, 0);
groundPath.closeSubpath();
QLinearGradient groundGradient(0, h, 0, 0);
groundGradient.setColorAt(0, QColor::fromHsvF(0.25, 1, 0.5));
groundGradient.setColorAt(1, QColor::fromHsvF(0.25, 0.25, 0.5));
QBrush groundBrush(groundGradient);
painter.fillPath(groundPath, groundBrush);
painter.setPen(greenPen);
QPointF start(-w, 0);
QPoint end(w, 0);
painter.drawLine(start, end);
painter.resetTransform();
drawInstrumentBackground(painter, area);
}
void PrimaryFlightDisplay::drawPitchScale(
QPainter& painter,
QRectF area,
bool drawNumbersLeft,
bool drawNumbersRight
) {
// We should really do these transforms but they are assumed done by caller.
// painter.resetTransform();
// painter.translate(area.center());
// painter.rotate(roll);
painter.setPen(whitePen);
QTransform savedTransform = painter.transform();
qreal h = area.height();
// find the mark nearest center
int snap = round(pitch/PITCH_SCALE_RESOLUTION)*PITCH_SCALE_RESOLUTION;
int _min = snap-PITCH_SCALE_HALFRANGE;
int _max = snap+PITCH_SCALE_HALFRANGE;
for (int degrees=_min; degrees<=_max; degrees+=PITCH_SCALE_RESOLUTION) {
bool isMajor = degrees % (PITCH_SCALE_RESOLUTION*2) == 0;
float linewidth = isMajor ? PITCH_SCALE_MAJORLENGTH : PITCH_SCALE_MINORLENGTH;
float shift = pitchAngleToTranslation(h, pitch-degrees);
painter.translate(0, shift);
QPointF start(-linewidth*h, 0);
QPointF end(linewidth*h, 0);
painter.drawLine(start, end);
if (isMajor && (drawNumbersLeft||drawNumbersRight)) {
int displayDegrees = degrees;
if(displayDegrees>90) displayDegrees = 180-displayDegrees;
else if (displayDegrees<-90) displayDegrees = -180 - displayDegrees;
if (SHOW_ZERO_ON_SCALES || degrees) {
QString s_number; //= QString("%d").arg(degrees);
s_number.sprintf("%d", displayDegrees);
if (drawNumbersLeft) drawTextRightCenter(painter, s_number, SCALE_TEXT_SIZE, -PITCH_SCALE_MAJORLENGTH * h-10, 0);
if (drawNumbersRight) drawTextLeftCenter(painter, s_number, SCALE_TEXT_SIZE, PITCH_SCALE_MAJORLENGTH * h+10, 0);
}
}
painter.setTransform(savedTransform);
}
}
void PrimaryFlightDisplay::drawRollScale(
QPainter& painter,
QRectF area,
bool drawTicks,
bool drawNumbers) {
qreal h = area.height();
painter.setPen(whitePen);
// We should really do these transforms but they are assumed done by caller.
// painter.resetTransform();
// painter.translate(area.center());
// painter.rotate(roll);
qreal _size = h * ROLL_SCALE_RADIUS*2;
QRectF arcArea(-_size/2, -h * ROLL_SCALE_RADIUS, _size, _size);
painter.drawArc(arcArea, (90-ROLL_SCALE_RANGE)*16, ROLL_SCALE_RANGE*2*16);
if (drawTicks) {
int length = sizeof(tickValues)/sizeof(int);
qreal previousRotation = 0;
for (int i=0; i<length*2+1; i++) {
int degrees = (i==length) ? 0 : (i>length) ?-tickValues[i-length-1] : tickValues[i];
//degrees = 180 - degrees;
painter.rotate(degrees - previousRotation);
previousRotation = degrees;
QPointF start(0, -_size/2);
QPointF end(0, -(1.0+ROLL_SCALE_TICKMARKLENGTH)*_size/2);
painter.drawLine(start, end);
QString s_number; //= QString("%d").arg(degrees);
if (SHOW_ZERO_ON_SCALES || degrees)
s_number.sprintf("%d", abs(degrees));
if (drawNumbers) {
drawTextCenterBottom(painter, s_number, SCALE_TEXT_SIZE, 0, -(ROLL_SCALE_RADIUS+ROLL_SCALE_TICKMARKLENGTH*1.7)*h);
}
}
}
}
void PrimaryFlightDisplay::drawAIAttitudeScales(
QPainter& painter,
QRectF area
) {
// To save computations, we do these transformations once for both scales:
painter.resetTransform();
painter.translate(area.center());
painter.rotate(roll);
QTransform saved = painter.transform();
drawRollScale(painter, area, true, true);
painter.setTransform(saved);
drawPitchScale(painter, area, true, true);
}
#ifdef USE_DISK_COMPASS
void PrimaryFlightDisplay::drawCompassDisk(
QPainter& painter,
QRect area, // the area where to draw the disk, assumed quadratic.
float heading) {
float start = heading - COMPASS_DISK_SPAN/2;
float end = heading + COMPASS_DISK_SPAN/2;
int firstTick = ceil(start / COMPASS_DISK_RESOLUTION) * COMPASS_DISK_RESOLUTION;
int lastTick = floor(end / COMPASS_DISK_RESOLUTION) * COMPASS_DISK_RESOLUTION;
float radius = area.width()/2;
painter.resetTransform();
painter.setPen(instrumentEdgePen);
painter.setBrush(HUDInstrumentBackground);
painter.drawEllipse(area);
painter.setBrush(Qt::NoBrush);
painter.setPen(whitePen);
for (int tickYaw = firstTick; tickYaw <= lastTick; tickYaw += COMPASS_DISK_RESOLUTION) {
int displayTick = tickYaw;
if (displayTick < 0) displayTick+=360;
else if (displayTick>=360) displayTick-=360;
// yaw is in center.
float off = tickYaw - heading;
// wrap that to ]-180..180]
if (off<=-180) off+= 360; else if (off>180) off += 360;
painter.translate(area.center());
painter.rotate(off);
if (displayTick % 45 == 0) {
// draw a wind label
QString name = compassWindNames[displayTick / 45];
drawText(painter, name, Qt::AlignCenter | Qt::TextWordWrap, SCALE_TEXT_SIZE, 0, -radius*0.83);
}
QPointF p_start = QPointF(0, -radius);
painter.drawPoint(p_start);
// QPointF p_end = QPointF(0, radius);
// painter.drawLine(p_start, p_end);
painter.resetTransform();
}
painter.setPen(redPen);
painter.translate(area.center());
QPainterPath markerPath(QPointF(0, -radius-2));
markerPath.lineTo(radius*COMPASS_DISK_MARKERWIDTH/2, -radius-radius*COMPASS_DISK_MARKERHEIGHT-2);
markerPath.lineTo(-radius*COMPASS_DISK_MARKERWIDTH/2, -radius-radius*COMPASS_DISK_MARKERHEIGHT-2);
markerPath.closeSubpath();
painter.drawPath(markerPath);
float yshift = -radius*3/4;
QRectF compassRect(-radius/3, -radius/2, radius*2/3, radius/3);
painter.setPen(instrumentEdgePen);
painter.drawRoundedRect(compassRect, 3, 3);
if (heading < 0) heading += 360;
else if (heading >= 360) heading -= 360;
/* final safeguard for really stupid systems */
int yawCompass = static_cast<int>(heading) % 360;
QString yawAngle;
yawAngle.sprintf("%03d", yawCompass);
painter.setPen(whitePen);
drawText(painter, yawAngle, Qt::AlignCenter | Qt::TextWordWrap, SCALE_TEXT_SIZE*1.41, 0, yshift/2);
}
#endif
#ifdef USE_DISK2_COMPASS
void PrimaryFlightDisplay::drawCompassDisk(QPainter& painter, QRectF area) {
float start = heading - COMPASS_DISK2_SPAN/2;
float end = heading + COMPASS_DISK2_SPAN/2;
int firstTick = ceil(start / COMPASS_DISK2_RESOLUTION) * COMPASS_DISK2_RESOLUTION;
int lastTick = floor(end / COMPASS_DISK2_RESOLUTION) * COMPASS_DISK2_RESOLUTION;
float radius = area.width()/2;
float innerRadius = radius * 0.96;
painter.resetTransform();
float save = instrumentEdgePen.widthF();
instrumentEdgePen.setWidthF(save/2);
painter.setPen(instrumentEdgePen);
painter.setBrush(HUDInstrumentBackground);
painter.drawEllipse(area);
instrumentEdgePen.setWidthF(save);
painter.setBrush(Qt::NoBrush);
blackPen.setWidthF(2);
for (int tickYaw = firstTick; tickYaw <= lastTick; tickYaw += COMPASS_DISK2_RESOLUTION) {
int displayTick = tickYaw;
if (displayTick < 0) displayTick+=360;
else if (displayTick>=360) displayTick-=360;
// yaw is in center.
float off = tickYaw - heading;
// wrap that to ]-180..180]
if (off<=-180) off+= 360; else if (off>180) off += 360;
painter.translate(area.center());
painter.rotate(off);
bool drewArrow = false;
bool isMajor = displayTick % COMPASS_DISK2_MAJORTICK == 0;
if (displayTick==30 || displayTick==60 ||
displayTick==120 || displayTick==150 ||
displayTick==210 || displayTick==240 ||
displayTick==300 || displayTick==330) {
// draw a number
QString s_number;
s_number.sprintf("%d", displayTick/10);
painter.setPen(blackPen);
drawTextCenter(painter, s_number, COMPASS_SCALE_TEXT_SIZE, 0, -innerRadius*0.75);
} else {
if (displayTick % COMPASS_DISK2_ARROWTICK == 0) {
if (displayTick!=0) {
QPainterPath markerPath(QPointF(0, -innerRadius*(1-COMPASS_DISK_MARKERHEIGHT/2)));
markerPath.lineTo(innerRadius*COMPASS_DISK_MARKERWIDTH/4, -innerRadius);
markerPath.lineTo(-innerRadius*COMPASS_DISK_MARKERWIDTH/4, -innerRadius);
markerPath.closeSubpath();
painter.setPen(blackPen);
painter.setBrush(Qt::SolidPattern);
painter.drawPath(markerPath);
painter.setBrush(Qt::NoBrush);
drewArrow = true;
}
if (displayTick%90 == 0) {
// Also draw a label
QString name = compassWindNames[displayTick / 45];
painter.setPen(blackPen);
drawTextCenter(painter, name, COMPASS_SCALE_TEXT_SIZE*1.2, 0, -innerRadius*0.75);
}
}
}
// draw the scale lines. If an arrow was drawn, stay off from it.
QPointF p_start = drewArrow ? QPoint(0, -innerRadius*0.94) : QPoint(0, -innerRadius);
QPoint p_end = isMajor ? QPoint(0, -innerRadius*0.86) : QPoint(0, -innerRadius*0.90);
painter.setPen(blackPen);
painter.drawLine(p_start, p_end);
painter.resetTransform();
}
painter.setPen(blackPen);
//painter.setBrush(Qt::SolidPattern);
painter.translate(area.center());
QPainterPath markerPath(QPointF(0, -radius-2));
markerPath.lineTo(radius*COMPASS_DISK_MARKERWIDTH/2, -radius-radius*COMPASS_DISK_MARKERHEIGHT-2);
markerPath.lineTo(-radius*COMPASS_DISK_MARKERWIDTH/2, -radius-radius*COMPASS_DISK_MARKERHEIGHT-2);
markerPath.closeSubpath();
painter.drawPath(markerPath);
QRectF compassRect(-radius/3, -radius*0.52, radius*2/3, radius*0.28);
painter.setPen(instrumentEdgePen);
painter.drawRoundedRect(compassRect, instrumentEdgePen.widthF()*2/3, instrumentEdgePen.widthF()*2/3);
if (heading < 0) heading += 360;
else if (heading >= 360) heading -= 360;
/* final safeguard for really stupid systems */
int yawCompass = static_cast<int>(heading) % 360;
QString yawAngle;
yawAngle.sprintf("%03d", yawCompass);
painter.setPen(whitePen);
drawTextCenter(painter, yawAngle, SCALE_TEXT_SIZE*1.2, 0, -radius*0.38);
}
#endif
#ifdef USE_TAPE_COMPASS
void PrimaryFlightDisplay::drawCompassTape(
QPainter& painter,
float heading,
qreal width,
qreal y,
qreal height
) {
float start = heading - COMPASS_TAPE_SPAN/2;
float end = heading + COMPASS_TAPE_SPAN/2;
int firstTick = ceil(start / COMPASS_TAPE_RESOLUTION) * COMPASS_TAPE_RESOLUTION;
int lastTick = floor(end / COMPASS_TAPE_RESOLUTION) * COMPASS_TAPE_RESOLUTION;
painter.setPen(whitePen);
for (int tickYaw = firstTick; tickYaw <= lastTick; tickYaw += COMPASS_TAPE_RESOLUTION) {
int displayTick = tickYaw;
if (displayTick < 0) displayTick+=360;
else if (displayTick>=360) displayTick-=360;
// yaw is in center.
float off = tickYaw - heading;
// wrap that to ]-180..180]
if (off<=-180) off+= 360; else if (off>180) off += 360;
float x = width/2 + off*width/COMPASS_TAPE_SPAN;
QPointF p_start = QPointF(x, y-height/2);
QPointF p_end = QPointF(x, y+height/2);
if (displayTick % 45 == 0) {
// draw a wind label
QString name = compassWindNames[displayTick / 45];
drawText(painter, name, Qt::AlignLeft | Qt::TextWordWrap, SCALE_TEXT_SIZE, x, y);
painter.drawPoint(p_start);
painter.drawPoint(p_end);
} else {
QPointF p_start(x, y-height/2);
QPointF p_endQPointF(x, y+height/2);
painter.drawLine(p_start, p_end);
}
}
painter.setPen(redPen);
QPointF p_start(width/2, y-height/2);
QPointF p_end(width/2, y+height/2);
painter.drawLine(p_start, p_end);
}
#endif
void PrimaryFlightDisplay::drawAltimeter(
QPainter& painter,
QRectF area, // the area where to draw the tape.
// float width,
// float height,
float altitude,
float maxAltitude,
float vv
) {
Q_UNUSED(vv)
Q_UNUSED(maxAltitude)
painter.resetTransform();
fillInstrumentBackground(painter, area);
painter.setPen(whitePen);
float height = area.height();
float width = area.width();
float effectiveHalfHeight = height*0.45;
float tickmarkLeft = 0.3*width;
float tickmarkRight = 0.4*width;
float numbersLeft = 0.42*width;
float markerHalfHeight = 0.06*height;
float rightEdge = area.width()-instrumentEdgePen.widthF()*2;
float markerTip = tickmarkLeft;
// altitude scale
#ifdef ALTIMETER_PROJECTED
float range = 1.2;
float start = altitude - ALTIMETER_PROJECTED_SPAN/2;
float end = altitude + ALTIMETER_PROJECTED_SPAN/2;
int firstTick = ceil(start / ALTIMETER_PROJECTED_RESOLUTION) * ALTIMETER_PROJECTED_RESOLUTION;
int lastTick = floor(end / ALTIMETER_PROJECTED_RESOLUTION) * ALTIMETER_PROJECTED_RESOLUTION;
for (int tickAlt = firstTick; tickAlt <= lastTick; tickAlt += ALTIMETER_PROJECTED_RESOLUTION) {
// a number between 0 and 1. Use as radians directly.
float r = range*(tickAlt-altitude)/(ALTIMETER_PROJECTED_SPAN/2);
float y = effectiveHalfHeight * sin(r);
scale = cos(r);
if (scale<0) scale = -scale;
bool hasText = tickAlt % ALTIMETER_PROJECTED_MAJOR_RESOLUTION == 0;
#else
float start = altitude - ALTIMETER_LINEAR_SPAN/2;
float end = altitude + ALTIMETER_LINEAR_SPAN/2;
int firstTick = ceil(start / ALTIMETER_LINEAR_RESOLUTION) * ALTIMETER_LINEAR_RESOLUTION;
int lastTick = floor(end / ALTIMETER_LINEAR_RESOLUTION) * ALTIMETER_LINEAR_RESOLUTION;
for (int tickAlt = firstTick; tickAlt <= lastTick; tickAlt += ALTIMETER_LINEAR_RESOLUTION) {
float y = (tickAlt-altitude)*effectiveHalfHeight/(ALTIMETER_LINEAR_SPAN/2);
bool hasText = tickAlt % ALTIMETER_LINEAR_MAJOR_RESOLUTION == 0;
#endif
painter.resetTransform();
painter.translate(area.left(), area.center().y() - y);
//painter.scale(1, scale);
painter.drawLine(tickmarkLeft, 0, tickmarkRight, 0);
if (hasText) {
QString s_alt;
s_alt.sprintf("%d", tickAlt);
drawTextLeftCenter(painter, s_alt, SCALE_TEXT_SIZE, numbersLeft, 0);
}
}
QPainterPath markerPath(QPoint(markerTip, 0));
markerPath.lineTo(markerTip+markerHalfHeight, markerHalfHeight);
markerPath.lineTo(rightEdge, markerHalfHeight);
markerPath.lineTo(rightEdge, -markerHalfHeight);
markerPath.lineTo(markerTip+markerHalfHeight, -markerHalfHeight);
markerPath.closeSubpath();
painter.resetTransform();
painter.translate(area.left(), area.center().y());
painter.setPen(blackPen);
painter.setBrush(Qt::SolidPattern);
painter.drawPath(markerPath);
painter.setBrush(Qt::NoBrush);
painter.setPen(whitePen);
QString s_alt;
s_alt.sprintf("%3.0f", altitude);
float xCenter = (markerTip+rightEdge)/2;
drawTextCenter(painter, s_alt, SCALE_TEXT_SIZE, xCenter, 0);
}
void PrimaryFlightDisplay::drawSysStatsPanel (
QPainter& painter,
QRectF area) {
// Timer
// Battery
// Armed/not
QString s_volts("12.4V 21A");
QString s_arm("Armed");
painter.resetTransform();
drawInstrumentBackground(painter, area);
painter.translate(area.center());
painter.setPen(amberPen);
drawTextCenter(painter, s_volts, SMALL_TEXT_SIZE, 0, -area.height()/6);
painter.setPen(redPen);
drawTextCenter(painter, s_arm, SCALE_TEXT_SIZE, 0, area.height()/6);
}
void PrimaryFlightDisplay::drawLinkStatsPanel (
QPainter& painter,
QRectF area) {
// UAV Id
// Droprates up, down
QString s_linkStat("100%");
QString s_upTime("01:23:34");
painter.resetTransform();
drawInstrumentBackground(painter, area);
painter.translate(area.center());
painter.setPen(amberPen);
drawTextCenter(painter, s_linkStat, SMALL_TEXT_SIZE, 0, -area.height()/6);
drawTextCenter(painter, s_upTime, SMALL_TEXT_SIZE, 0, area.height()/6);
}
void PrimaryFlightDisplay::drawMissionStatsPanel (
QPainter& painter,
QRectF area) {
// Flight mode
// next WP
// next WP dist
QString s_flightMode("Auto");
QString s_nextWP("1234m\u21924");
painter.resetTransform();
drawInstrumentBackground(painter, area);
painter.translate(area.center());
painter.setPen(amberPen);
drawTextCenter(painter, s_flightMode, SMALL_TEXT_SIZE, 0, -area.height()/6);
drawTextCenter(painter, s_nextWP, SMALL_TEXT_SIZE, 0, area.height()/6);
}
void PrimaryFlightDisplay::drawSensorsStatsPanel (
QPainter& painter,
QRectF area) {
// GPS fixmode and #sats
// Home alt.?
// Groundspeed?
QString s_GPS("GPS 3D(8)");
QString s_homealt("H.alt 472m");
painter.resetTransform();
drawInstrumentBackground(painter, area);
painter.translate(area.center());
painter.setPen(amberPen);
drawTextCenter(painter, s_GPS, SMALL_TEXT_SIZE, 0, -area.height()/6);
drawTextCenter(painter, s_homealt, SMALL_TEXT_SIZE, 0, area.height()/6);
}
void PrimaryFlightDisplay::paintSeparate() {
QPainter painter;
painter.begin(this);
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
qreal margin = 5;
#if defined(USE_DISK_COMPASS) || defined(USE_DISK2_COMPASS)
// Centered and square. Margin is outside and single.
QRectF AIArea((width()-height())/2 + margin, margin, height()-2 * margin, height()-2 * margin);
#else
QRectF AIArea(0, 0, width(), height()*0.95);
qreal compassHeight = height() - AIArea.height();
#endif
painter.fillRect(rect(), Qt::black);
painter.setClipping(true);
painter.setClipRect(AIArea);
drawAIGlobalFeatures(painter, AIArea);
drawAIAttitudeScales(painter, AIArea);
drawAIAirframeFixedFeatures(painter, AIArea);
#if defined(USE_DISK_COMPASS) || defined(USE_DISK2_COMPASS)
qreal h = AIArea.height();
QRectF compassArea(AIArea.x()+h*0.60, AIArea.y()+h*0.80, h/2, h/2);
drawCompassDisk(painter, compassArea);
#else
drawCompassTape(painter, yaw, width(), y, compassHeight);
#endif
painter.setClipping(false);
qreal tapeGaugesWidth = width()-AIArea.width() - 4*margin;
// Hack: Lower bound of corner instruments coincide with upper bounds of altimeter/airspeed meter
qreal sidePanelsHeight = height()-2*margin;
// X: To the right of AI and with single margin again. That is, 3 single margins plus width of AI.
// Y: 1 single margin below above gadget.
QRectF altimeterArea(AIArea.width()+AIArea.x()+margin, height()*1/5+margin, tapeGaugesWidth/2, sidePanelsHeight*3/5);
drawAltimeter(painter, altimeterArea, aboveASLAltitude, 1000, 0);
QRectF velocityArea(margin, height()*1/5+margin, tapeGaugesWidth/2, sidePanelsHeight*3/5);
drawAltimeter(painter, velocityArea, aboveASLAltitude, 1000, 0);
QRectF sensorsStatsArea(margin, margin, tapeGaugesWidth/2, sidePanelsHeight/5);
drawSensorsStatsPanel(painter, sensorsStatsArea);
QRectF linkStatsArea(AIArea.width()+AIArea.x()+margin, margin, tapeGaugesWidth/2, sidePanelsHeight/5);
drawLinkStatsPanel(painter, linkStatsArea);
QRectF sysStatsArea(margin, sidePanelsHeight*4/5+margin, tapeGaugesWidth/2, sidePanelsHeight/5);
drawSysStatsPanel(painter, sysStatsArea);
QRectF missionStatsArea(AIArea.width()+AIArea.x()+margin, sidePanelsHeight*4/5+margin, tapeGaugesWidth/2, sidePanelsHeight/5);
drawMissionStatsPanel(painter, missionStatsArea);
painter.end();
}
void PrimaryFlightDisplay::paintAllInOne() {
// QT PAINTING
QPainter painter;
painter.begin(this);
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
painter.setRenderHint(QPainter::TextAntialiasing);
#if defined(USE_DISK_COMPASS) || defined(USE_DISK2_COMPASS)
//QRectF AIArea(0, 0, width(), height());
QRectF AIArea(0, 0, width(), height());
#else
QRectF AIArea(0, 0, width(), height()*0.95);
qreal compassHeight = height() - AIArea.height();
#endif
drawAIGlobalFeatures(painter, AIArea);
drawAIAttitudeScales(painter, AIArea);
drawAIAirframeFixedFeatures(painter, AIArea);
#if defined(USE_DISK_COMPASS) || defined(USE_DISK2_COMPASS)
QRectF compassArea(width()-275, AIArea.height()-68, 170, 170);
drawCompassDisk(painter, compassArea);
#else
drawCompassTape(painter, yaw, width(), y, compassHeight);
#endif
QRectF altimeterArea(AIArea.width()*7/8, AIArea.height()*1/5, AIArea.width()/8, AIArea.height()*3/5);
drawAltimeter(painter, altimeterArea, aboveASLAltitude, 1000, 0);
QRectF velocityArea(0, AIArea.height()*1/5, AIArea.width()/8, AIArea.height()*3/5);
drawAltimeter(painter, velocityArea, aboveASLAltitude, 1000, 0);
painter.end();
}
void PrimaryFlightDisplay::makeDummyData() {
roll = -15;
pitch = 10;
heading = 28;
aboveASLAltitude = 123;
}
void PrimaryFlightDisplay::doPaint() {
#ifdef SEPARATE_LAYOUT
paintSeparate();
#else
paintAllInOne();
#endif
}
#ifndef PRIMARYFLIGHTDISPLAY_H
#define PRIMARYFLIGHTDISPLAY_H
#include <QWidget>
#include <QPen>
#include "UASInterface.h"
#define SEPARATE_LAYOUT
#define LINEWIDTH 0.007
// all in units of display height
#define ROLL_SCALE_RADIUS 0.42
#define ROLL_SCALE_TICKMARKLENGTH 0.04
#define ROLL_SCALE_MARKERWIDTH 0.06
#define ROLL_SCALE_MARKERHEIGHT 0.04
// scale max. degrees
#define ROLL_SCALE_RANGE 60
// fraction of height to translate for each degree of pitch.
#define PITCHTRANSLATION 65.0
// 10 degrees for each line
#define PITCH_SCALE_RESOLUTION 5
#define PITCH_SCALE_MAJORLENGTH 0.08
#define PITCH_SCALE_MINORLENGTH 0.04
#define PITCH_SCALE_HALFRANGE 20
//#define USE_TAPE_COMPASS
//#define USE_DISK_COMPASS
#define USE_DISK2_COMPASS
// We want no numbers on the scale, just principal winds or half-winds and ticks.
// With whole winds there are 45 deg per wind, with half-winds 22.5
#define COMPASS_TAPE_RESOLUTION 15
// number of degrees side to side
#define COMPASS_TAPE_SPAN 120
// The number of degrees to either side of the heading to draw the compass disk.
// 180 is valid, this will draw a complete disk. If the disk is partly clipped
// away, less will do.
#define COMPASS_DISK_SPAN 180
#define COMPASS_DISK_RESOLUTION 15
#define COMPASS_DISK_MARKERWIDTH 0.2
#define COMPASS_DISK_MARKERHEIGHT 0.133
#define COMPASS_DISK2_SPAN 180
#define COMPASS_DISK2_RESOLUTION 5
#define COMPASS_DISK2_MAJORTICK 10
#define COMPASS_DISK2_ARROWTICK 45
#define COMPASS_DISK2_MAJORLINEWIDTH 0.006
#define COMPASS_DISK2_MINORLINEWIDTH 0.004
#define COMPASS_SCALE_TEXT_SIZE 0.03
// The altitude difference between top and bottom of scale
#define ALTIMETER_LINEAR_SPAN 35
// every 5 meters there is a tick mark
#define ALTIMETER_LINEAR_RESOLUTION 5
// every 10 meters there is a number
#define ALTIMETER_LINEAR_MAJOR_RESOLUTION 10
// min. and max. vertical velocity
// The altitude difference between top and bottom of scale
#define ALTIMETER_PROJECTED_SPAN 50
// every 5 meters there is a tick mark
#define ALTIMETER_PROJECTED_RESOLUTION 5
// every 10 meters there is a number
#define ALTIMETER_PROJECTED_MAJOR_RESOLUTION 10
// min. and max. vertical velocity
//#define ALTIMETER_PROJECTED
#define ALTIMETER_VVI_SPAN 10
#define ALTIMETER_VVI_LOGARITHMIC true
#define SHOW_ZERO_ON_SCALES true
#define SCALE_TEXT_SIZE 0.042
#define SMALL_TEXT_SIZE 0.035
#define UNKNOWN_BATTERY -1
class PrimaryFlightDisplay : public QWidget
{
Q_OBJECT
public:
PrimaryFlightDisplay(int width = 640, int height = 480, QWidget* parent = NULL);
~PrimaryFlightDisplay();
/** @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, int);
void receiveHeartbeat(UASInterface*);
void updateThrust(UASInterface*, double);
void updateLocalPosition(UASInterface*,double,double,double,quint64);
void updateGlobalPosition(UASInterface*,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 paintEvent(QPaintEvent *event);
void resizeEvent(QResizeEvent *e);
// from HUD.h:
/** @brief Preferred Size */
QSize sizeHint() const;
/** @brief Start updating widget */
void showEvent(QShowEvent* event);
/** @brief Stop updating widget */
void hideEvent(QHideEvent* event);
// dongfang: We have no context menu. Viewonly.
// void contextMenuEvent (QContextMenuEvent* event);
// dongfang: What is that?
void createActions();
static const int updateInterval = 40;
public slots:
/** @brief Set the currently monitored UAS */
virtual void setActiveUAS(UASInterface* uas);
protected slots:
void paintOnTimer();
signals:
void visibilityChanged(bool visible);
private:
//void prepareTransform(QPainter& painter, qreal width, qreal height);
//void transformToGlobalSystem(QPainter& painter, qreal width, qreal height, float roll, float pitch);
void drawTextCenter(QPainter& painter, QString text, float fontSize, float x, float y);
void drawTextLeftCenter(QPainter& painter, QString text, float fontSize, float x, float y);
void drawTextRightCenter(QPainter& painter, QString text, float fontSize, float x, float y);
void drawTextCenterBottom(QPainter& painter, QString text, float fontSize, float x, float y);
void drawTextCenterTop(QPainter& painter, QString text, float fontSize, float x, float y);
void drawAIGlobalFeatures(QPainter& painter, QRectF area);
void drawAIAirframeFixedFeatures(QPainter& painter, QRectF area);
void drawPitchScale(QPainter& painter, QRectF area, bool drawNumbersLeft, bool drawNumbersRight);
void drawRollScale(QPainter& painter, QRectF area, bool drawTicks, bool drawNumbers);
void drawAIAttitudeScales(QPainter& painter, QRectF area);
#if defined(USE_DISK_COMPASS) || defined(USE_DISK2_COMPASS)
void drawCompassDisk(QPainter& painter, QRectF area);
#else
void drawCompassTape(QPainter& painter, QRectF area, float yaw);
#endif
void drawAltimeter(QPainter& painter, QRectF area, float altitude, float maxAltitude, float vv);
void fillInstrumentBackground(QPainter& painter, QRectF edge);
void drawInstrumentBackground(QPainter& painter, QRectF edge);
void drawLinkStatsPanel(QPainter& painter, QRectF area);
void drawSysStatsPanel(QPainter& painter, QRectF area);
void drawMissionStatsPanel(QPainter& painter, QRectF area);
void drawSensorsStatsPanel(QPainter& painter, QRectF area);
void makeDummyData();
void doPaint();
void paintAllInOne();
void paintSeparate();
float roll;
float pitch;
float heading;
// APM: GPS and baro mix. From the GLOBAL_POSITION_INT or VFR_HUD messages.
float aboveASLAltitude;
float GPSAltitude;
// APM: GPS and baro mix above home (GPS) altitude. This value comes from the GLOBAL_POSITION_INT message.
// Do !!!NOT!!! ever do altitude calculations at the ground station. There are enough pitfalls already.
// The MP "set home altitude" button will not be repeated here if it did that.
float aboveHomeAltitude;
float groundSpeed;
float airSpeed;
QString mode;
QString state;
float load;
QPen whitePen;
QPen redPen;
QPen amberPen;
QPen greenPen;
QPen blackPen;
QPen instrumentEdgePen;
QBrush instrumentBackground;
QBrush HUDInstrumentBackground;
QFont font;
QTimer* refreshTimer; ///< The main timer, controls the update rate
UASInterface* uas; ///< The uas currently monitored
//QString energyStatus; ///< Current fuel level / battery voltage
double batteryVoltage;
double batteryCharge;
static const int tickValues[];
static const QString compassWindNames[];
signals:
public slots:
};
#endif // PRIMARYFLIGHTDISPLAY_H
......@@ -6,7 +6,7 @@
#include "UASManager.h"
QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) :
HUD(width, height, parent),
HUD_old(width, height, parent),
rgbEnabled(false),
depthEnabled(false)
{
......@@ -70,7 +70,7 @@ void QGCRGBDView::setActiveUAS(UASInterface* uas)
connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
}
HUD::setActiveUAS(uas);
HUD_old::setActiveUAS(uas);
}
void QGCRGBDView::clearData(void)
......@@ -85,7 +85,7 @@ void QGCRGBDView::contextMenuEvent(QContextMenuEvent* event)
{
QMenu menu(this);
// Update actions
enableHUDAction->setChecked(hudInstrumentsEnabled);
enableHUDAction->setChecked(HUDInstrumentsEnabled);
//enableVideoAction->setChecked(videoEnabled);
enableRGBAction->setChecked(rgbEnabled);
enableDepthAction->setChecked(depthEnabled);
......
......@@ -3,7 +3,7 @@
#include "HUD.h"
class QGCRGBDView : public HUD
class QGCRGBDView : public HUD_old
{
Q_OBJECT
public:
......
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