Skip to content
HSIDisplay.cc 54.4 KiB
Newer Older
/*=====================================================================

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 "UASManager.h"
#include "HSIDisplay.h"
#include "QGC.h"
pixhawk's avatar
pixhawk committed
#include "Waypoint.h"
lm's avatar
lm committed
//#include "Waypoint2DIcon.h"
HSIDisplay::HSIDisplay(QWidget *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),
LM's avatar
LM committed
    uiZSetCoordinate(0.0f),
    uiYawSet(0.0f),
    metricWidth(4.0),
    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);
pixhawk's avatar
pixhawk committed
    columns = 1;
    this->setAutoFillBackground(true);
    QPalette pal = palette();
    pal.setColor(backgroundRole(), QGC::colorBlack);
    setPalette(pal);
    xCenterPos = vwidth/2.0f;
pixhawk's avatar
pixhawk committed
    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()));
    if (UASManager::instance()->getActiveUAS())
    {
        setActiveUAS(UASManager::instance()->getActiveUAS());
    }
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
            this, SLOT(setActiveUAS(UASInterface*)));
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
            this, SLOT(setActiveUAS(UASInterface*)));
    setFocusPolicy(Qt::StrongFocus);
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;
lm's avatar
lm committed
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();
lm's avatar
lm committed
}

#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;

    painter.setRenderHint(QPainter::Antialiasing, true);
    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
    // Draw base instrument
    // ----------------------
lm's avatar
lm committed
    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 (MainWindow::instance()->getStyle() == MainWindow::QGC_MAINWINDOW_STYLE_LIGHT)
    {
        ringColor = QGC::colorBlack;
        positionColor = QColor(20, 20, 200);
        setpointColor = QColor(150, 250, 150);
        valueColor = QColor(40, 40, 40);
        statusColor = QGC::colorOrange;
        waypointLineColor = QGC::colorDarkYellow;
        attitudeColor = QColor(200, 20, 20);
    }
    else
    {
        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);
    }

lm's avatar
lm committed
    QPen pen;
    pen.setColor(ringColor);
    pen.setWidth(refLineWidthToPen(1.0f));
lm's avatar
lm committed
    painter.setPen(pen);
lm's avatar
lm committed
    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);
lm's avatar
lm committed
    }

    // 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);

    //    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);
lm's avatar
lm committed
    // 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);
        // 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);
lm's avatar
lm committed
        //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
lm's avatar
lm committed

    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);

    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);
    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);
    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
    float crossTrackError = 0;
    paintText(tr("XTRACK"), labelColor, 2.2f, 54, topMargin+2, &painter);
    paintText(tr("%1 m").arg(crossTrackError, 5, 'f', 2, '0'), valueColor, 2.2f, 67, topMargin+2, &painter);

    // Draw position to bottom left
        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);
        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 Safety
    double x1, y1, z1, x2, y2, z2;
    UASManager::instance()->getLocalNEDSafetyLimits(&x1, &y1, &z1, &x2, &y2, &z2);
    //    drawSafetyArea(QPointF(x1, y1), QPointF(x2, y2), QGC::colorYellow, painter);
    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)
    if (MainWindow::instance()->getStyle() == MainWindow::QGC_MAINWINDOW_STYLE_LIGHT)
    {
        statusColor = QColor(40, 40, 40);
    // Draw the label.
    paintText(label, labelColor, 2.6f, x, y+0.8f, &painter);

    // Determine color of status rectangle.
        painter.setBrush(QGC::colorDarkYellow);
    } else {
        if(status) {
            painter.setBrush(QGC::colorGreen);
        } else {
            painter.setBrush(Qt::darkGray);
        }
    }
    painter.setPen(Qt::NoPen);
    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
        QPen pen(Qt::yellow);
        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 (MainWindow::instance()->getStyle() == MainWindow::QGC_MAINWINDOW_STYLE_LIGHT)
    {
        statusColor = QColor(40, 40, 40);
    // 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:
        lockText = tr("3D");
        break;
    default:
        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);
        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);
    }
lm's avatar
lm committed
void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock)
{
    Q_UNUSED(uas);
    bool changed = positionLock != lock;
lm's avatar
lm committed
    positionLock = lock;
lm's avatar
lm committed
}

pixhawk's avatar
pixhawk committed
void HSIDisplay::updateAttitudeControllerEnabled(bool enabled)
lm's avatar
lm committed
{
    bool changed = attControlEnabled != enabled;
lm's avatar
lm committed
    attControlEnabled = enabled;
    attControlKnown = true;
lm's avatar
lm committed
}

pixhawk's avatar
pixhawk committed
void HSIDisplay::updatePositionXYControllerEnabled(bool enabled)
lm's avatar
lm committed
{
    bool changed = xyControlEnabled != enabled;
lm's avatar
lm committed
    xyControlEnabled = enabled;
    xyControlKnown = true;
lm's avatar
lm committed
}

pixhawk's avatar
pixhawk committed
void HSIDisplay::updatePositionZControllerEnabled(bool enabled)
lm's avatar
lm committed
{
    bool changed = zControlEnabled != enabled;
lm's avatar
lm committed
    zControlEnabled = enabled;
    zControlKnown = true;
lm's avatar
lm committed
}

LM's avatar
LM committed
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);
LM's avatar
LM committed
    // 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);
}

pixhawk's avatar
pixhawk committed
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()));
pixhawk's avatar
pixhawk committed
    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);
pixhawk's avatar
pixhawk committed
    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)
{
lm's avatar
lm committed
    return QPointF(-((ref.y() - yCenterPos)/ vwidth) * metricWidth - x, ((ref.x() - xCenterPos) / vwidth) * metricWidth - y);
}

/**
 * @see refToScreenX()
 */
pixhawk's avatar
pixhawk committed
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;
}

pixhawk's avatar
pixhawk committed
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->posF());
pixhawk's avatar
pixhawk committed
        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)
{
LM's avatar
LM committed
    // FIXME hardcode yaw to current value
    //setBodySetpointCoordinateYaw(0);
        if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton)
        {
            if (dragStarted)
            {
LM's avatar
LM committed
                if (!directSending) setBodySetpointCoordinateYaw(uiYawSet);
                dragStarted = false;
            }
        }
        if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::LeftButton)
        {
            if (leftDragStarted)
            {
LM's avatar
LM committed
                if (!directSending) setBodySetpointCoordinateZ(uiZSetCoordinate);
                leftDragStarted = 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;
pixhawk's avatar
pixhawk committed
    }
void HSIDisplay::mouseMoveEvent(QMouseEvent * event)
{
    if (event->type() == QMouseEvent::MouseMove)
    {
        if (dragStarted) uiYawSet -= 0.06f*(startX - event->x()) / this->frameSize().width();
            //            uiZSetCoordinate -= 0.06f*(startY - event->y()) / this->frameSize().height();
            //            setBodySetpointCoordinateZ(uiZSetCoordinate);

        if (leftDragStarted || dragStarted) mouseHasMoved = true;
pixhawk's avatar
pixhawk committed
    }
pixhawk's avatar
pixhawk committed
void HSIDisplay::keyPressEvent(QKeyEvent* event)
{
    QPointF bodySP = metricWorldToBody(QPointF(uiXSetCoordinate, uiYSetCoordinate));

    if ((event->key() == Qt::Key_Enter || event->key() == Qt::Key_Return) && actionPending)
pixhawk's avatar
pixhawk committed
    {
        actionPending = false;
        statusMessage = "SETPOINT SENT";
        statusClearTimer.start();
        sendBodySetPointCoordinates();
    }
LM's avatar
LM committed
    else
LM's avatar
LM committed
        // FIXME hardcode yaw to current value
        setBodySetpointCoordinateYaw(0);


        // Reset setpoints to current position / orientation
        // if not initialized
        if (!userYawSetPointSet)
LM's avatar
LM committed
            setBodySetpointCoordinateYaw(0);
LM's avatar
LM committed

        if (!userZSetPointSet)
LM's avatar
LM committed
            setBodySetpointCoordinateZ(0);
LM's avatar
LM committed
        if (!userXYSetPointSet)
LM's avatar
LM committed
            setBodySetpointCoordinateXY(0, 0);
LM's avatar
LM committed

        if ((event->key() ==  Qt::Key_W))
LM's avatar
LM committed
            setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()+0.2, +1.5), bodySP.y());
LM's avatar
LM committed
        if ((event->key() ==  Qt::Key_S))
LM's avatar
LM committed
            setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y());
LM's avatar
LM committed

        if ((event->key() ==  Qt::Key_A))
        {
            setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()-0.2, +1.5));
        }

LM's avatar
LM committed
        if ((event->key() ==  Qt::Key_D))
        {
            setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()+0.2, +1.5));
        }

LM's avatar
LM committed
        if ((event->key() ==  Qt::Key_Up))
pixhawk's avatar
pixhawk committed
            setBodySetpointCoordinateZ(-0.2);
LM's avatar
LM committed
        if ((event->key() ==  Qt::Key_Down))
pixhawk's avatar
pixhawk committed
            setBodySetpointCoordinateZ(+0.2);
LM's avatar
LM committed
        if ((event->key() ==  Qt::Key_Left))
LM's avatar
LM committed
            setBodySetpointCoordinateYaw(-0.2);
LM's avatar
LM committed
        if ((event->key() ==  Qt::Key_Right))
LM's avatar
LM committed
            setBodySetpointCoordinateYaw(0.2);
PIXHAWK Team's avatar
PIXHAWK Team committed
    }
LM's avatar
LM committed
    // Overwrite any existing non-zero body setpoints
    // for escape
    if ((event->key() == Qt::Key_Escape) || (event->key() == Qt::Key_R))
PIXHAWK Team's avatar
PIXHAWK Team committed
    {
LM's avatar
LM committed
        setBodySetpointCoordinateXY(0, 0);
        setBodySetpointCoordinateZ(0);
        setBodySetpointCoordinateYaw(0);
        statusMessage = "CANCELLED, PRESS <ENTER> TO SEND";
PIXHAWK Team's avatar
PIXHAWK Team committed
    }

    if ((event->key() == Qt::Key_T))
PIXHAWK Team's avatar
PIXHAWK Team committed
    {
        directSending = !directSending;
        statusMessage = (directSending) ? "DIRECT SEND ON" : "DIRECT SEND OFF";
        statusClearTimer.start();
PIXHAWK Team's avatar
PIXHAWK Team committed
    }

    if (actionPending && (directSending || (event->key() == Qt::Key_Escape)))
PIXHAWK Team's avatar
PIXHAWK Team committed
    {
        actionPending = false;
        statusMessage = "SETPOINT SENT";
        statusClearTimer.start();
        sendBodySetPointCoordinates();
PIXHAWK Team's avatar
PIXHAWK Team committed
    }

    HDDisplay::keyPressEvent(event);
pixhawk's avatar
pixhawk committed
}

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::setActiveUAS(UASInterface* uas)
{
    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,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
        disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,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)));
LM's avatar
LM committed
        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)));
        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,quint64)),
                this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
        connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)),
                this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
        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)),