HSIDisplay.cc 54.4 KB
Newer Older
1 2
/*=====================================================================

3
QGroundControl Open Source Ground Control Station
4

5
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
6

7
This file is part of the QGROUNDCONTROL project
8

9
    QGROUNDCONTROL is free software: you can redistribute it and/or modify
10 11 12 13
    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.

14
    QGROUNDCONTROL is distributed in the hope that it will be useful,
15 16 17 18 19
    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
20
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
21 22 23 24 25 26 27 28 29 30 31 32 33

======================================================================*/

/**
 * @file
 *   @brief Implementation of Horizontal Situation Indicator class
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QFile>
#include <QStringList>
34
#include <QPainter>
35 36 37
#include <QGraphicsScene>
#include <QHBoxLayout>
#include <QDoubleSpinBox>
38 39
#include "UASManager.h"
#include "HSIDisplay.h"
40
#include "QGC.h"
pixhawk's avatar
pixhawk committed
41
#include "Waypoint.h"
42
#include "UASWaypointManager.h"
43
#include <qmath.h>
lm's avatar
lm committed
44
//#include "Waypoint2DIcon.h"
45
#include "MAV2DIcon.h"
46
#include "MainWindow.h"
47 48 49

#include <QDebug>

50

51
HSIDisplay::HSIDisplay(QWidget *parent) :
52
    HDDisplay(QStringList(), "HSI", parent),
53 54 55 56 57 58 59
    dragStarted(false),
    leftDragStarted(false),
    mouseHasMoved(false),
    startX(0.0f),
    startY(0.0f),
    actionPending(false),
    directSending(false),
60 61 62 63 64 65 66 67 68
    gpsSatellites(),
    satellitesUsed(0),
    attXSet(0.0f),
    attYSet(0.0f),
    attYawSet(0.0f),
    altitudeSet(1.0f),
    posXSet(0.0f),
    posYSet(0.0f),
    posZSet(0.0f),
69 70
    attXSaturation(0.2f),
    attYSaturation(0.2f),
71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95
    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
96
    uiZSetCoordinate(0.0f),
97 98
    uiYawSet(0.0f),
    metricWidth(4.0),
99 100
    xCenterPos(0),
    yCenterPos(0),
101 102 103 104 105 106 107 108 109
    positionLock(false),
    attControlEnabled(false),
    xyControlEnabled(false),
    zControlEnabled(false),
    yawControlEnabled(false),
    positionFix(0),
    gpsFix(0),
    visionFix(0),
    laserFix(0),
110
    iruFix(0),
111
    mavInitialized(false),
112 113
    topMargin(20.0f),
    bottomMargin(14.0f),
114 115 116 117 118 119 120 121
    attControlKnown(false),
    xyControlKnown(false),
    zControlKnown(false),
    yawControlKnown(false),
    positionFixKnown(false),
    visionFixKnown(false),
    gpsFixKnown(false),
    iruFixKnown(false),
122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147
    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),
148 149 150 151 152 153 154
    actuatorsOK(false),
    setPointKnown(false),
    positionSetPointKnown(false),
    userSetPointSet(false),
    userXYSetPointSet(false),
    userZSetPointSet(false),
    userYawSetPointSet(false)
155
{
156
    refreshTimer->setInterval(updateInterval);
157

pixhawk's avatar
pixhawk committed
158
    columns = 1;
159
    this->setAutoFillBackground(true);
160 161 162
    QPalette pal = palette();
    pal.setColor(backgroundRole(), QGC::colorBlack);
    setPalette(pal);
163

164
    vwidth = 80.0f;
165
    vheight = 100.0f;
pixhawk's avatar
pixhawk committed
166

167
    xCenterPos = vwidth/2.0f;
pixhawk's avatar
pixhawk committed
168
    yCenterPos = vheight/2.0f + topMargin - bottomMargin;
169

170 171 172
    uas = NULL;
    resetMAVState();

173 174
    // Do first update
    setMetricWidth(metricWidth);
175 176 177
    // 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."));
178

179 180
    // XXX this looks a potential recursive issue
    //connect(&statusClearTimer, SIGNAL(timeout()), this, SLOT(clearStatusMessage()));
181

182 183 184 185
    if (UASManager::instance()->getActiveUAS())
    {
        setActiveUAS(UASManager::instance()->getActiveUAS());
    }
186

187 188
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
            this, SLOT(setActiveUAS(UASInterface*)));
189

190 191
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
            this, SLOT(setActiveUAS(UASInterface*)));
192

193
    setFocusPolicy(Qt::StrongFocus);
194 195
}

196 197 198 199 200
HSIDisplay::~HSIDisplay()
{

}

201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226
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;
227 228 229 230

    // Setpoints
    positionSetPointKnown = false;
    setPointKnown = false;
231 232
}

lm's avatar
lm committed
233 234 235 236
void HSIDisplay::paintEvent(QPaintEvent * event)
{
    Q_UNUSED(event);
    //paintGL();
237 238 239
    //    static quint64 interval = 0;
    //    //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
    //    interval = MG::TIME::getGroundTimeNow();
240
    renderOverlay();
lm's avatar
lm committed
241 242
}

243
void HSIDisplay::renderOverlay()
244
{
245
    if (!isVisible() || !uas) return;
246 247 248
#if (QGC_EVENTLOOP_DEBUG)
    qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif
249 250
    // Center location of the HSI gauge items

251
    //float bottomMargin = 3.0f;
252 253

    // Size of the ring instrument
254
    //const float margin = 0.1f;  // 10% margin of total width on each side
255
    float baseRadius = (vheight - topMargin - bottomMargin) / 2.0f - (topMargin + bottomMargin) / 2.8f;
256

257 258 259 260 261 262 263 264
    // 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;

265
    QPainter painter(viewport());
266 267
    painter.setRenderHint(QPainter::Antialiasing, true);
    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
268

269 270
    // Draw base instrument
    // ----------------------
lm's avatar
lm committed
271
    painter.setBrush(Qt::NoBrush);
272 273 274 275 276 277 278 279 280 281 282 283 284 285 286

    // 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);
287
        labelColor = QColor(26, 75, 95);
288 289 290 291 292 293 294
        valueColor = QColor(40, 40, 40);
        statusColor = QGC::colorOrange;
        waypointLineColor = QGC::colorDarkYellow;
        attitudeColor = QColor(200, 20, 20);
    }
    else
    {
295
        ringColor = QColor(255, 255, 255);
296 297 298 299 300 301 302 303 304
        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
305 306
    QPen pen;
    pen.setColor(ringColor);
307
    pen.setWidth(refLineWidthToPen(1.0f));
lm's avatar
lm committed
308
    painter.setPen(pen);
lm's avatar
lm committed
309
    const int ringCount = 2;
310 311
    for (int i = 0; i < ringCount; i++)
    {
312
        float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.35f * i+1) / 2.0f - bottomMargin / 2.0f;
313
        drawCircle(xCenterPos, yCenterPos, radius, 1.0f, ringColor, &painter);
314
        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
315 316
    }

317 318 319
    // Draw orientation labels
    // Translate and rotate coordinate frame
    painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
320 321 322
    const float yawDeg = ((yaw/M_PI)*180.0f);
    int yawRotate = static_cast<int>(yawDeg) % 360;
    painter.rotate(-yawRotate);
323 324
    paintText(tr("N"), ringColor, 3.5f, - 1.0f, - baseRadius - 5.5f, &painter);
    paintText(tr("S"), ringColor, 3.5f, - 1.0f, + baseRadius + 1.5f, &painter);
325
    paintText(tr("E"), ringColor, 3.5f, + baseRadius + 3.0f, - 1.25f, &painter);
326
    paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter);
327
    painter.rotate(+yawRotate);
328 329
    painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);

330
    // Draw trail
331 332 333 334 335 336
    //    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);
337 338


lm's avatar
lm committed
339
    // Draw center indicator
340 341 342 343 344
    //    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);
345

346 347
    if (uas)
    {
348 349 350
        // Translate to center
        painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
        QColor uasColor = uas->getColor();
351
        MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
lm's avatar
lm committed
352
        //MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
353 354 355
        // Translate back
        painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
    }
356 357

    // ----------------------
358

359 360 361
    // Draw satellites
    drawGPS(painter);

362 363 364
    // Draw state indicator

    // Draw position
365
    drawPositionDirection(xCenterPos, yCenterPos, baseRadius, positionColor, &painter);
366 367

    // Draw attitude
368
    drawAttitudeDirection(xCenterPos, yCenterPos, baseRadius, attitudeColor, &painter);
369 370


371
    // Draw position setpoints in body coordinates
lm's avatar
lm committed
372

373 374 375 376 377 378
    float xSpDiff = uiXSetCoordinate - bodyXSetCoordinate;
    float ySpDiff = uiYSetCoordinate - bodyYSetCoordinate;
    float zSpDiff = uiZSetCoordinate - bodyZSetCoordinate;

    float setPointDist = sqrt(xSpDiff*xSpDiff + ySpDiff*ySpDiff + zSpDiff*zSpDiff);

379 380 381 382
    float angleDiff = uiYawSet - bodyYawSet;

    float normAngleDiff = fabs(atan2(sin(angleDiff), cos(angleDiff)));

383 384 385 386 387
    // Labels on outer part and bottom

    // Draw waypoints
    drawWaypoints(painter);

388
    // Draw status flags
389 390 391 392 393
    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);
394

395
    // Draw position lock indicators
396 397 398 399 400 401 402 403 404 405 406 407 408 409 410
    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);
411 412

    // Draw speed to top left
413 414
    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);
415 416 417

    // Draw crosstrack error to top right
    float crossTrackError = 0;
418 419
    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);
420 421

    // Draw position to bottom left
422 423
    if (localAvailable > 0)
    {
424 425
        // Position
        QString str;
426
        float offset = (globalAvailable > 0) ? -3.0f : 0.0f;
427
        str.sprintf("%05.2f %05.2f %05.2f m", x, y, z);
428 429
        paintText(tr("POS"), labelColor, 2.6f, 2, vheight - offset - 4.0f, &painter);
        paintText(str, valueColor, 2.6f, 10, vheight - offset - 4.0f, &painter);
430 431
    }

432 433
    if (globalAvailable > 0)
    {
434 435
        // Position
        QString str;
436
        str.sprintf("lat: %05.2f lon: %06.2f alt: %06.2f", lat, lon, alt);
437 438
        paintText(tr("GPS"), labelColor, 2.6f, 2, vheight- 4.0f, &painter);
        paintText(str, valueColor, 2.6f, 10, vheight - 4.0f, &painter);
439 440
    }

441 442 443
    // Draw Safety
    double x1, y1, z1, x2, y2, z2;
    UASManager::instance()->getLocalNEDSafetyLimits(&x1, &y1, &z1, &x2, &y2, &z2);
444
    //    drawSafetyArea(QPointF(x1, y1), QPointF(x2, y2), QGC::colorYellow, painter);
445 446

    // Draw status message
447
    paintText(statusMessage, statusColor, 2.8f, 8, 15, &painter);
448 449 450 451 452

    // Draw setpoint over waypoints
    if (positionSetPointKnown || setPointKnown)
    {
        // Draw setpoint
453
        drawSetpointXYZYaw(bodyXSetCoordinate, bodyYSetCoordinate, bodyZSetCoordinate, bodyYawSet, setpointColor, painter);
454 455 456 457 458 459
        // 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);
460
        drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, setpointColor, &painter);
461 462 463 464
    }

    if ((userSetPointSet || dragStarted) && ((normAngleDiff > 0.05f) || !(setPointDist < 0.08f && mavInitialized)))
    {
465
        drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, setpointColor, painter);
466
    }
467
}
468

469
void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter)
470 471 472 473 474
{
    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)
475
{
476
    QColor statusColor;
477
    QColor labelColor;
478 479 480
    if (MainWindow::instance()->getStyle() == MainWindow::QGC_MAINWINDOW_STYLE_LIGHT)
    {
        statusColor = QColor(40, 40, 40);
481
        labelColor = QColor(26, 75, 95);
482 483 484 485
    }
    else
    {
        statusColor = QColor(250, 250, 250);
486
        labelColor = QGC::colorCyan;
487
    }
488

489 490 491 492
    // Draw the label.
    paintText(label, labelColor, 2.6f, x, y+0.8f, &painter);

    // Determine color of status rectangle.
493
    if (!ok) {
494
        painter.setBrush(QGC::colorDarkYellow);
495 496 497 498 499 500
    } else {
        if(status) {
            painter.setBrush(QGC::colorGreen);
        } else {
            painter.setBrush(Qt::darkGray);
        }
501 502
    }
    painter.setPen(Qt::NoPen);
503

504
    // Draw the status rectangle.
505 506 507
    float indicatorWidth = refToScreenX(7.0f);
    float indicatorHeight = refToScreenY(4.0f);
    painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), indicatorWidth, indicatorHeight));
508
    paintText((status) ? tr("ON") : tr("OFF"), statusColor, 2.6f, x+7.9f, y+0.8f, &painter);
509

510
    // Cross out instrument if state unknown
511 512
    if (!known)
    {
513
        QPen pen(Qt::yellow);
514
        pen.setWidth(3);
515 516 517 518 519 520 521 522 523 524 525 526 527 528 529
        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);
    }
530 531
}

532
void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter)
533
{
534 535
    // Select color scheme based on light or dark window theme.
    QColor labelColor;
536 537 538
    QColor negStatusColor(200, 20, 20);
    QColor intermediateStatusColor (Qt::yellow);
    QColor posStatusColor(20, 200, 20);
539 540 541 542
    QColor statusColor;
    if (MainWindow::instance()->getStyle() == MainWindow::QGC_MAINWINDOW_STYLE_LIGHT)
    {
        statusColor = QColor(40, 40, 40);
543
        labelColor = QColor(26, 75, 95);
544 545 546 547
    }
    else
    {
        statusColor = QColor(250, 250, 250);
548
        labelColor = QGC::colorCyan;
549
    }
550

551 552 553 554
    // Draw the label.
    paintText(label, labelColor, 2.6f, x, y+0.8f, &painter);

    // based on the status, choose both the coloring and lock text.
555 556 557
    QString lockText;
    switch (status) {
    case 1:
558
        painter.setBrush(intermediateStatusColor.dark(150));
559 560 561
        lockText = tr("LOC");
        break;
    case 2:
562
        painter.setBrush(intermediateStatusColor.dark(150));
563 564 565
        lockText = tr("2D");
        break;
    case 3:
566
        painter.setBrush(posStatusColor);
567 568 569
        lockText = tr("3D");
        break;
    default:
570
        painter.setBrush(negStatusColor);
571 572 573
        lockText = tr("NO");
        break;
    }
574

575 576 577 578 579 580
    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);
581

582 583 584
    // Cross out instrument if state unknown
    if (!known) {
        QPen pen(Qt::yellow);
585
        pen.setWidth(3);
586 587 588 589 590 591 592 593 594 595 596 597 598 599 600
        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);
    }
601 602
}

lm's avatar
lm committed
603 604
void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock)
{
605
    Q_UNUSED(uas);
606
    bool changed = positionLock != lock;
lm's avatar
lm committed
607
    positionLock = lock;
608 609 610 611
    if (changed)
    {
        update();
    }
lm's avatar
lm committed
612 613
}

pixhawk's avatar
pixhawk committed
614
void HSIDisplay::updateAttitudeControllerEnabled(bool enabled)
lm's avatar
lm committed
615
{
616
    bool changed = attControlEnabled != enabled;
lm's avatar
lm committed
617
    attControlEnabled = enabled;
618
    attControlKnown = true;
619 620 621 622
    if (changed)
    {
        update();
    }
lm's avatar
lm committed
623 624
}

pixhawk's avatar
pixhawk committed
625
void HSIDisplay::updatePositionXYControllerEnabled(bool enabled)
lm's avatar
lm committed
626
{
627
    bool changed = xyControlEnabled != enabled;
lm's avatar
lm committed
628
    xyControlEnabled = enabled;
629
    xyControlKnown = true;
630 631 632 633
    if (changed)
    {
        update();
    }
lm's avatar
lm committed
634 635
}

pixhawk's avatar
pixhawk committed
636
void HSIDisplay::updatePositionZControllerEnabled(bool enabled)
lm's avatar
lm committed
637
{
638
    bool changed = zControlEnabled != enabled;
lm's avatar
lm committed
639
    zControlEnabled = enabled;
640
    zControlKnown = true;
641 642 643 644
    if (changed)
    {
        update();
    }
lm's avatar
lm committed
645 646
}

LM's avatar
LM committed
647 648
void HSIDisplay::updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance)
{
649 650 651 652 653
    Q_UNUSED(quality);
    Q_UNUSED(name);
    Q_UNUSED(type);
    Q_UNUSED(id);
    Q_UNUSED(time);
LM's avatar
LM committed
654 655 656 657 658 659 660 661 662 663 664 665 666 667 668
    // 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
669 670 671 672
QPointF HSIDisplay::metricWorldToBody(QPointF world)
{
    // First translate to body-centered coordinates
    // Rotate around -yaw
673
    float angle = -yaw - M_PI;
674
    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
675 676 677 678 679 680 681
    return result;
}

QPointF HSIDisplay::metricBodyToWorld(QPointF body)
{
    // First rotate into world coordinates
    // then translate to world position
682
    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
683
    return result;
684
}
685

686 687
QPointF HSIDisplay::screenToMetricBody(QPointF ref)
{
688
    return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth);
689 690 691 692
}

QPointF HSIDisplay::refToMetricBody(QPointF &ref)
{
lm's avatar
lm committed
693
    return QPointF(-((ref.y() - yCenterPos)/ vwidth) * metricWidth - x, ((ref.x() - xCenterPos) / vwidth) * metricWidth - y);
694 695 696 697 698
}

/**
 * @see refToScreenX()
 */
699
QPointF HSIDisplay::metricBodyToRef(QPointF &metric)
700 701 702 703
{
    return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos);
}

704 705 706 707 708 709 710 711 712 713
double HSIDisplay::metricToRef(double metric)
{
    return (metric / metricWidth) * vwidth;
}

double HSIDisplay::refToMetric(double ref)
{
    return (ref/vwidth) * metricWidth;
}

714 715 716 717 718 719 720 721
QPointF HSIDisplay::metricBodyToScreen(QPointF metric)
{
    QPointF ref = metricBodyToRef(metric);
    ref.setX(refToScreenX(ref.x()));
    ref.setY(refToScreenY(ref.y()));
    return ref;
}

722 723
void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
{
724 725
    if (event->type() == QMouseEvent::MouseButtonDblClick)
    {
726
        QPointF p = screenToMetricBody(event->localPos());
pixhawk's avatar
pixhawk committed
727 728 729 730 731
        if (!directSending)
        {
            setBodySetpointCoordinateXY(p.x(), p.y());
            if (!userZSetPointSet) setBodySetpointCoordinateZ(0.0);
        }
732
        //        qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos;
733 734 735 736 737
    }
}

void HSIDisplay::mouseReleaseEvent(QMouseEvent * event)
{
LM's avatar
LM committed
738
    // FIXME hardcode yaw to current value
739
    //setBodySetpointCoordinateYaw(0);
740
    if (mouseHasMoved)
741
    {
742 743 744 745
        if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton)
        {
            if (dragStarted)
            {
LM's avatar
LM committed
746
                if (!directSending) setBodySetpointCoordinateYaw(uiYawSet);
747 748 749 750 751 752 753
                dragStarted = false;
            }
        }
        if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::LeftButton)
        {
            if (leftDragStarted)
            {
LM's avatar
LM committed
754
                if (!directSending) setBodySetpointCoordinateZ(uiZSetCoordinate);
755
                leftDragStarted = false;
756 757
            }
        }
758
    }
759
    mouseHasMoved = false;
760 761 762 763 764 765 766 767 768
}

void HSIDisplay::mousePressEvent(QMouseEvent * event)
{
    if (event->type() == QMouseEvent::MouseButtonPress)
    {
        if (event->button() == Qt::RightButton)
        {
            startX = event->x();
769 770
            // Start tracking mouse move
            dragStarted = true;
771 772 773
        }
        else if (event->button() == Qt::LeftButton)
        {
774 775
            startY = event->y();
            leftDragStarted = true;
776
        }
pixhawk's avatar
pixhawk committed
777
    }
778
    mouseHasMoved = false;
779
}
780

781 782 783 784
void HSIDisplay::mouseMoveEvent(QMouseEvent * event)
{
    if (event->type() == QMouseEvent::MouseMove)
    {
785
        if (dragStarted) uiYawSet -= 0.06f*(startX - event->x()) / this->frameSize().width();
786 787 788

        if (leftDragStarted)
        {
789 790
            //            uiZSetCoordinate -= 0.06f*(startY - event->y()) / this->frameSize().height();
            //            setBodySetpointCoordinateZ(uiZSetCoordinate);
791
        }
792 793

        if (leftDragStarted || dragStarted) mouseHasMoved = true;
pixhawk's avatar
pixhawk committed
794
    }
795 796
}

pixhawk's avatar
pixhawk committed
797 798
void HSIDisplay::keyPressEvent(QKeyEvent* event)
{
799 800
    QPointF bodySP = metricWorldToBody(QPointF(uiXSetCoordinate, uiYSetCoordinate));

801
    if ((event->key() == Qt::Key_Enter || event->key() == Qt::Key_Return) && actionPending)
pixhawk's avatar
pixhawk committed
802 803 804 805 806 807
    {
        actionPending = false;
        statusMessage = "SETPOINT SENT";
        statusClearTimer.start();
        sendBodySetPointCoordinates();
    }
LM's avatar
LM committed
808
    else
809
    {
LM's avatar
LM committed
810 811 812 813 814 815 816
        // FIXME hardcode yaw to current value
        setBodySetpointCoordinateYaw(0);


        // Reset setpoints to current position / orientation
        // if not initialized
        if (!userYawSetPointSet)
817
        {
LM's avatar
LM committed
818
            setBodySetpointCoordinateYaw(0);
819
        }
LM's avatar
LM committed
820 821

        if (!userZSetPointSet)
822
        {
LM's avatar
LM committed
823
            setBodySetpointCoordinateZ(0);
824 825
        }

LM's avatar
LM committed
826
        if (!userXYSetPointSet)
827
        {
LM's avatar
LM committed
828
            setBodySetpointCoordinateXY(0, 0);
829
        }
LM's avatar
LM committed
830 831

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

LM's avatar
LM committed
836
        if ((event->key() ==  Qt::Key_S))
837
        {
LM's avatar
LM committed
838
            setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y());
839
        }
LM's avatar
LM committed
840 841

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

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

LM's avatar
LM committed
851
        if ((event->key() ==  Qt::Key_Up))
852
        {
pixhawk's avatar
pixhawk committed
853
            setBodySetpointCoordinateZ(-0.2);
854 855
        }

LM's avatar
LM committed
856
        if ((event->key() ==  Qt::Key_Down))
857
        {
pixhawk's avatar
pixhawk committed
858
            setBodySetpointCoordinateZ(+0.2);
859 860
        }

LM's avatar
LM committed
861
        if ((event->key() ==  Qt::Key_Left))
862
        {
LM's avatar
LM committed
863
            setBodySetpointCoordinateYaw(-0.2);
864 865
        }

LM's avatar
LM committed
866
        if ((event->key() ==  Qt::Key_Right))
867
        {
LM's avatar
LM committed
868
            setBodySetpointCoordinateYaw(0.2);
869
        }
PIXHAWK Team's avatar
PIXHAWK Team committed
870
    }
871 872


LM's avatar
LM committed
873 874 875
    // 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
876
    {
LM's avatar
LM committed
877
        setBodySetpointCoordinateXY(0, 0);
878 879 880
        setBodySetpointCoordinateZ(0);
        setBodySetpointCoordinateYaw(0);
        statusMessage = "CANCELLED, PRESS <ENTER> TO SEND";
PIXHAWK Team's avatar
PIXHAWK Team committed
881
    }
882 883

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

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

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

901 902 903 904 905
void HSIDisplay::contextMenuEvent (QContextMenuEvent* event)
{
    event->ignore();
}

906 907
void HSIDisplay::setMetricWidth(double width)
{
908
    if (width != metricWidth) {
909 910 911 912 913
        metricWidth = width;
        emit metricWidthChanged(metricWidth);
    }
}

914 915 916 917 918 919
/**
 *
 * @param uas the UAS/MAV to monitor/display with the HUD
 */
void HSIDisplay::setActiveUAS(UASInterface* uas)
{
920 921 922
    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)));
923
        disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64)));
924
        disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64)));
925
        disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
926
        disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
927
        disconnect(this->uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
928 929 930 931 932 933 934 935 936 937 938
        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
939
        disconnect(this->uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float)));
940 941 942 943 944 945 946 947 948 949

        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)));
950
    }
951

952 953
    if (uas)
    {
954 955 956 957
        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)));
958 959
        connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,double,quint64)),
                this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64)));
960 961
        connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)),
                this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64)));
962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007
        connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)),
                this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
        connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)),
                this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
        connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)),
                this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
        connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)),
                this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
        connect(uas, SIGNAL(attitudeControlEnabled(bool)),
                this, SLOT(updateAttitudeControllerEnabled(bool)));
        connect(uas, SIGNAL(positionXYControlEnabled(bool)),
                this, SLOT(updatePositionXYControllerEnabled(bool)));
        connect(uas, SIGNAL(positionZControlEnabled(bool)),
                this, SLOT(updatePositionZControllerEnabled(bool)));
        connect(uas, SIGNAL(positionYawControlEnabled(bool)),
                this, SLOT(updatePositionYawControllerEnabled(bool)));

        connect(uas, SIGNAL(localizationChanged(UASInterface*,int)),
                this, SLOT(updateLocalization(UASInterface*,int)));
        connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)),
                this, SLOT(updateVisionLocalization(UASInterface*,int)));
        connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)),
                this, SLOT(updateGpsLocalization(UASInterface*,int)));
        connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)),
                this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int)));
        connect(uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)),
                this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float)));

        connect(uas, SIGNAL(gyroStatusChanged(bool,bool,bool)),
                this, SLOT(updateGyroStatus(bool,bool,bool)));
        connect(uas, SIGNAL(accelStatusChanged(bool,bool,bool)),
                this, SLOT(updateAccelStatus(bool,bool,bool)));
        connect(uas, SIGNAL(magSensorStatusChanged(bool,bool,bool)),
                this, SLOT(updateMagSensorStatus(bool,bool,bool)));
        connect(uas, SIGNAL(baroStatusChanged(bool,bool,bool)),
                this, SLOT(updateBaroStatus(bool,bool,bool)));
        connect(uas, SIGNAL(airspeedStatusChanged(bool,bool,bool)),
                this, SLOT(updateAirspeedStatus(bool,bool,bool)));
        connect(uas, SIGNAL(opticalFlowStatusChanged(bool,bool,bool)),
                this, SLOT(updateOpticalFlowStatus(bool,bool,bool)));
        connect(uas, SIGNAL(laserStatusChanged(bool,bool,bool)),
                this, SLOT(updateLaserStatus(bool,bool,bool)));
        connect(uas, SIGNAL(groundTruthSensorStatusChanged(bool,bool,bool)),
                this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool)));
        connect(uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)),
                this, SLOT(updateActuatorStatus(bool,bool,bool)));
1008 1009 1010 1011 1012 1013
        statusClearTimer.start(3000);
    }
    else
    {
        statusClearTimer.stop();
    }
1014

1015
    this->uas = uas;
1016

1017
    resetMAVState();
1018 1019
}

1020 1021 1022 1023 1024 1025 1026
void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time)
{
    Q_UNUSED(uas);
    Q_UNUSED(time);
    this->vx = vx;
    this->vy = vy;
    this->vz = vz;
1027
    this->speed = sqrt(pow(vx, 2.0) + pow(vy, 2.0) + pow(vz, 2.0));
1028 1029 1030 1031
}

void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
{
1032 1033 1034 1035 1036
    if (uas)
    {
        userSetPointSet = true;
        userXYSetPointSet = true;
        // Set coordinates and send them out to MAV
1037

1038 1039 1040 1041 1042 1043
        QPointF sp(x, y);
        sp = metricBodyToWorld(sp);
        uiXSetCoordinate = sp.x();
        uiYSetCoordinate = sp.y();

        qDebug() << "Attempting to set new setpoint at x: " << x << "metric y:" << y;
lm's avatar
lm committed
1044

pixhawk's avatar
pixhawk committed
1045

pixhawk's avatar
pixhawk committed
1046 1047 1048 1049
        //sendBodySetPointCoordinates();
        statusMessage = "POSITION SET, PRESS <ENTER> TO SEND";
        actionPending = true;
        statusClearTimer.start();
pixhawk's avatar
pixhawk committed
1050
    }
1051 1052 1053 1054
}

void HSIDisplay::setBodySetpointCoordinateZ(double z)
{
1055 1056 1057
    if (uas)
    {
        userSetPointSet = true;
LM's avatar
LM committed
1058
        userZSetPointSet = true;
1059 1060 1061 1062 1063 1064
        // Set coordinates and send them out to MAV
        uiZSetCoordinate = z+uas->getLocalZ();
        statusMessage = "Z SET, PRESS <ENTER> TO SEND";
        actionPending = true;
        statusClearTimer.start();
    }
pixhawk's avatar
pixhawk committed
1065
    //sendBodySetPointCoordinates();
1066 1067 1068 1069
}

void HSIDisplay::setBodySetpointCoordinateYaw(double yaw)
{
1070
    if (uas)
1071
    {
1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083
        if (!userXYSetPointSet && setPointKnown)
        {
            uiXSetCoordinate = bodyXSetCoordinate;
            uiYSetCoordinate = bodyYSetCoordinate;
        }
        else if (!userXYSetPointSet && mavInitialized)
        {
            QPointF coord = metricBodyToWorld(QPointF(0.0, 0.0));
            uiXSetCoordinate = coord.x();
            uiYSetCoordinate = coord.y();
        }
        userSetPointSet = true;
LM's avatar
LM committed
1084
        userYawSetPointSet = true;
1085 1086 1087 1088 1089
        // Set coordinates and send them out to MAV
        uiYawSet = atan2(sin(yaw+uas->getYaw()), cos(yaw+uas->getYaw()));
        statusMessage = "YAW SET, PRESS <ENTER> TO SEND";
        statusClearTimer.start();
        actionPending = true;
1090
    }
pixhawk's avatar
pixhawk committed
1091
    //sendBodySetPointCoordinates();
1092 1093 1094 1095 1096
}

void HSIDisplay::sendBodySetPointCoordinates()
{
    // Send the coordinates to the MAV
1097
    if (uas)
1098
    {
1099
        uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
1100
    }
1101 1102
}

1103
void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec)
1104
{
1105 1106 1107 1108 1109 1110
    Q_UNUSED(uas);
    Q_UNUSED(usec);
    attXSet = pitchDesired;
    attYSet = rollDesired;
    attYawSet = yawDesired;
    altitudeSet = thrustDesired;
1111 1112
}

lm's avatar
lm committed
1113 1114 1115 1116 1117 1118 1119 1120 1121
void HSIDisplay::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time)
{
    Q_UNUSED(uas);
    Q_UNUSED(time);
    this->roll = roll;
    this->pitch = pitch;
    this->yaw = yaw;
}

1122 1123
void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired)
{
1124
    Q_UNUSED(uasid);
1125 1126 1127 1128 1129
    uiXSetCoordinate = xDesired;
    uiYSetCoordinate = yDesired;
    uiZSetCoordinate = zDesired;
    uiYawSet = yawDesired;
    userXYSetPointSet = true;
LM's avatar
LM committed
1130 1131
    userZSetPointSet = true;
    userYawSetPointSet = true;
1132 1133 1134
    userSetPointSet = true;
}

lm's avatar
lm committed
1135
void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec)
1136
{
1137
    Q_UNUSED(uasid);
1138
    Q_UNUSED(usec);
1139 1140 1141 1142
    bodyXSetCoordinate = xDesired;
    bodyYSetCoordinate = yDesired;
    bodyZSetCoordinate = zDesired;
    bodyYawSet = yawDesired;
pixhawk's avatar
pixhawk committed
1143
    mavInitialized = true;
1144
    setPointKnown = true;
1145
    positionSetPointKnown = true;
pixhawk's avatar
pixhawk committed
1146

1147 1148 1149 1150
    if (!userSetPointSet && !dragStarted)
    {
        uiXSetCoordinate = bodyXSetCoordinate;
        uiYSetCoordinate = bodyYSetCoordinate;
1151
        //        uiZSetCoordinate = bodyZSetCoordinate;
1152 1153
        uiYawSet= bodyYawSet;
    }
1154 1155 1156 1157
}

void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec)
{
1158 1159 1160 1161
    this->x = x;
    this->y = y;
    this->z = z;
    localAvailable = usec;
1162 1163
}

1164
void HSIDisplay::updateGlobalPosition(UASInterface*, double lat, double lon, double altAMSL, double altWGS84, quint64 usec)
1165
{
1166
    Q_UNUSED(altAMSL);
1167 1168
    this->lat = lat;
    this->lon = lon;
1169
    this->alt = altWGS84;
1170
    globalAvailable = usec;
1171 1172
}

lm's avatar
lm committed
1173
void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float azimuth, float snr, bool used)
lm's avatar
lm committed
1174 1175 1176
{
    Q_UNUSED(uasid);
    // If slot is empty, insert object
1177 1178 1179 1180 1181 1182
    if (satid != 0) { // Satellite PRNs currently range from 1-32, but are never zero
        if (gpsSatellites.contains(satid)) {
            gpsSatellites.value(satid)->update(satid, elevation, azimuth, snr, used);
        } else {
            gpsSatellites.insert(satid, new GPSSatellite(satid, elevation, azimuth, snr, used));
        }
lm's avatar
lm committed
1183 1184 1185
    }
}

1186 1187 1188
void HSIDisplay::updatePositionYawControllerEnabled(bool enabled)
{
    yawControlEnabled = enabled;
1189
    yawControlKnown = true;
1190 1191 1192 1193 1194 1195 1196 1197 1198
}

/**
 * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
 */
void HSIDisplay::updateLocalization(UASInterface* uas, int fix)
{
    Q_UNUSED(uas);
    positionFix = fix;
1199
    positionFixKnown = true;
1200
    //qDebug() << "LOCALIZATION FIX CALLED";
1201 1202 1203 1204 1205 1206 1207 1208
}
/**
 * @param fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D localization, 3: 3D localization
 */
void HSIDisplay::updateGpsLocalization(UASInterface* uas, int fix)
{
    Q_UNUSED(uas);
    gpsFix = fix;
1209
    gpsFixKnown = true;
1210 1211 1212 1213 1214 1215 1216 1217
}
/**
 * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
 */
void HSIDisplay::updateVisionLocalization(UASInterface* uas, int fix)
{
    Q_UNUSED(uas);
    visionFix = fix;
1218
    visionFixKnown = true;
1219
    //qDebug() << "VISION FIX GOT CALLED";
1220 1221
}

1222 1223 1224 1225 1226 1227 1228
/**
 * @param fix 0: lost, 1-N: Localized with N ultrasound or infrared sensors
 */
void HSIDisplay::updateInfraredUltrasoundLocalization(UASInterface* uas, int fix)
{
    Q_UNUSED(uas);
    iruFix = fix;
1229
    iruFixKnown = true;
1230 1231
}

lm's avatar
lm committed
1232 1233
QColor HSIDisplay::getColorForSNR(float snr)
{
lm's avatar
lm committed
1234
    QColor color;
1235
    if (snr > 0 && snr < 30) {
lm's avatar
lm committed
1236
        color = QColor(250, 10, 10);
1237
    } else if (snr >= 30 && snr < 35) {
lm's avatar
lm committed
1238
        color = QColor(230, 230, 10);
1239
    } else if (snr >= 35 && snr < 40) {
lm's avatar
lm committed
1240
        color = QColor(90, 200, 90);
1241
    } else if (snr >= 40) {
lm's avatar
lm committed
1242
        color = QColor(20, 200, 20);
1243
    } else {
lm's avatar
lm committed
1244 1245 1246
        color = QColor(180, 180, 180);
    }
    return color;
lm's avatar
lm committed
1247 1248
}

1249
void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter)
1250
{
1251 1252
    if (uas)
    {
1253
        float radius = vwidth / 18.0f;
1254
        QPen pen(color);
1255
        pen.setWidthF(refLineWidthToPen(1.6f));
1256 1257 1258 1259 1260 1261 1262 1263
        pen.setColor(color);
        painter.setPen(pen);
        painter.setBrush(Qt::NoBrush);
        QPointF in(x, y);
        // Transform from body to world coordinates
        in = metricWorldToBody(in);
        // Scale from metric to screen reference coordinates
        QPointF p = metricBodyToRef(in);
1264 1265 1266 1267 1268 1269 1270 1271 1272 1273

        QPolygonF poly(4);
        // Top point
        poly.replace(0, QPointF(p.x()-radius, p.y()-radius));
        // Right point
        poly.replace(1, QPointF(p.x()+radius, p.y()-radius));
        // Bottom point
        poly.replace(2, QPointF(p.x()+radius, p.y()+radius));
        poly.replace(3, QPointF(p.x()-radius, p.y()+radius));

1274
        // Label
pixhawk's avatar
pixhawk committed
1275
        paintText(QString("z: %1").arg(z, 3, 'f', 1, QChar(' ')), color, 2.1f, p.x()-radius, p.y()-radius-3.5f, &painter);
1276

1277 1278
        drawPolygon(poly, &painter);

1279
        radius *= 0.8f;
1280
        drawLine(p.x(), p.y(), p.x()+sin(-yaw + uas->getYaw() + M_PI) * radius, p.y()+cos(-yaw + uas->getYaw() + M_PI) * radius, refLineWidthToPen(0.6f), color, &painter);
1281 1282

        // Draw center dot
1283 1284 1285
        painter.setBrush(color);
        drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter);
    }
1286 1287
}

1288
void HSIDisplay::drawWaypoint(QPainter& painter, const QColor& color, float width, const Waypoint *w, const QPointF& p)
1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305 1306 1307
{
    painter.setBrush(Qt::NoBrush);

    // Setup pen for foreground
    QPen pen(color);
    pen.setWidthF(width);

    // DRAW WAYPOINT
    float waypointSize = vwidth / 20.0f * 2.0f;
    QPolygonF poly(4);
    // Top point
    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
    // Right point
    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
    // Bottom point
    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));

    float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
1308 1309
    float acceptRadius = w->getAcceptanceRadius();
    double yawDiff = w->getYaw()/180.0*M_PI-yaw;
1310 1311 1312

    // Draw background
    pen.setColor(Qt::black);
1313
    painter.setPen(pen);
1314
    drawLine(p.x(), p.y(), p.x()+sin(yawDiff) * radius, p.y()-cos(yawDiff) * radius, refLineWidthToPen(0.4f*3.0f), Qt::black, &painter);
1315
    drawPolygon(poly, &painter);
1316
    drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 3.0, Qt::black, &painter);
1317 1318 1319 1320

    // Draw foreground
    pen.setColor(color);
    painter.setPen(pen);
1321
    drawLine(p.x(), p.y(), p.x()+sin(yawDiff) * radius, p.y()-cos(yawDiff) * radius, refLineWidthToPen(0.4f), color, &painter);
1322 1323 1324 1325
    drawPolygon(poly, &painter);
    drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter);
}

1326 1327
void HSIDisplay::drawWaypoints(QPainter& painter)
{
1328 1329
    if (uas)
    {
1330
        // Grab all waypoints.
Lorenz Meier's avatar
Lorenz Meier committed
1331
        const QList<Waypoint*>& list = uas->getWaypointManager()->getWaypointEditableList();
1332
        const int numWaypoints = list.size();
1333

1334
        // Do not work on empty lists
1335 1336 1337 1338
        if (list.size() == 0)
        {
            return;
        }
1339

1340
        // Make sure any drawn shapes are not filled-in.
1341
        painter.setBrush(Qt::NoBrush);
pixhawk's avatar
pixhawk committed
1342

1343
        QPointF lastWaypoint;
1344
        for (int i = 0; i < numWaypoints; i++)
1345
        {
1346
            const Waypoint *w = list.at(i);
1347
            QPointF in;
1348 1349 1350 1351 1352 1353 1354
            // Use local coordinates as-is.
            int frameRef = w->getFrame();
            if (frameRef == MAV_FRAME_LOCAL_NED)
            {
                in = QPointF(w->getX(), w->getY());
            }
            else if (frameRef == MAV_FRAME_LOCAL_ENU)
1355
            {
1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370
                in = QPointF(w->getY(), w->getX());
            }
            // Convert global coordinates into the local ENU frame, then display them.
            else if (frameRef == MAV_FRAME_GLOBAL || frameRef == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
                // Get the position of the GPS origin for the MAV.

                // Transform the lat/lon for this waypoint into the local frame
                double e, n, u;
                UASManager::instance()->wgs84ToEnu(w->getX(), w->getY(),w->getZ(), &e, &n, &u);
                in = QPointF(n, e);
            }
            // Otherwise we don't process this waypoint.
            // FIXME: This code will probably fail if the last waypoint found is not a valid one.
            else {
                continue;
1371
            }
1372

1373 1374 1375 1376 1377
            // Transform from world to body coordinates
            in = metricWorldToBody(in);
            // Scale from metric to screen reference coordinates
            QPointF p = metricBodyToRef(in);

1378 1379
            // Select color based on if this is the current waypoint.
            if (w->getCurrent())
1380
            {
1381
                drawWaypoint(painter, QGC::colorYellow, refLineWidthToPen(0.8f), w, p);
1382 1383 1384
            }
            else
            {
1385
                drawWaypoint(painter, QGC::colorCyan, refLineWidthToPen(0.4f), w, p);
1386 1387
            }

1388
            // Draw connecting line from last waypoint to this one.
1389 1390
            if (!lastWaypoint.isNull())
            {
1391 1392
                drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f*2.0f), Qt::black, &painter);
                drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f), QGC::colorCyan, &painter);
1393 1394
            }
            lastWaypoint = p;
pixhawk's avatar
pixhawk committed
1395 1396
        }
    }
1397 1398
}

1399 1400 1401 1402 1403
void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter)
{
    QPen pen(color);
    pen.setWidthF(refLineWidthToPen(0.1f));
    pen.setColor(color);
1404
    pen.setBrush(Qt::NoBrush);
1405
    painter.setPen(pen);
1406
    painter.drawRect(QRectF(metricBodyToScreen(metricWorldToBody(topLeft)), metricBodyToScreen(metricWorldToBody(bottomRight))));
1407
}
lm's avatar
lm committed
1408

1409 1410 1411
void HSIDisplay::drawGPS(QPainter &painter)
{
    float xCenter = xCenterPos;
1412
    float yCenter = yCenterPos;
lm's avatar
lm committed
1413

1414 1415 1416 1417 1418
    const float yawDeg = ((yaw/M_PI)*180.0f);
    int yawRotate = static_cast<int>(yawDeg) % 360;
    // XXX check rotation direction

    // Max satellite circle radius
lm's avatar
lm committed
1419 1420 1421
    const float margin = 0.15f;  // 20% margin of total width on each side
    float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f;
    quint64 currTime = MG::TIME::getGroundTimeNowUsecs();
lm's avatar
lm committed
1422

pixhawk's avatar
pixhawk committed
1423 1424 1425 1426 1427
    // Draw satellite labels
    //    QString label;
    //    label.sprintf("%05.1f", value);
    //    paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter);

lm's avatar
lm committed
1428
    QMapIterator<int, GPSSatellite*> i(gpsSatellites);
1429
    while (i.hasNext()) {
lm's avatar
lm committed
1430 1431 1432
        i.next();
        GPSSatellite* sat = i.value();

1433 1434
        // Check if update is not older than 10 seconds, else delete satellite
        if (sat->lastUpdate + 10000000 < currTime) {
lm's avatar
lm committed
1435 1436
            // Delete and go to next satellite
            gpsSatellites.remove(i.key());
1437
            if (i.hasNext()) {
lm's avatar
lm committed
1438 1439
                i.next();
                sat = i.value();
1440
            } else {
lm's avatar
lm committed
1441 1442 1443 1444
                continue;
            }
        }

1445
        if (sat) {
lm's avatar
lm committed
1446 1447 1448 1449
            // Draw satellite
            QBrush brush;
            QColor color = getColorForSNR(sat->snr);
            brush.setColor(color);
1450
            if (sat->used) {
lm's avatar
lm committed
1451
                brush.setStyle(Qt::SolidPattern);
1452
            } else {
lm's avatar
lm committed
1453 1454 1455 1456 1457 1458
                brush.setStyle(Qt::NoBrush);
            }
            painter.setPen(Qt::SolidLine);
            painter.setPen(color);
            painter.setBrush(brush);

1459 1460
            float xPos = xCenter + (sin(((sat->azimuth/255.0f)*360.0f-yawRotate)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
            float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f-yawRotate)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
lm's avatar
lm committed
1461

1462
            // Draw circle for satellite, filled for used satellites
lm's avatar
lm committed
1463
            drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter);
1464
            // Draw satellite PRN
lm's avatar
lm committed
1465
            paintText(QString::number(sat->id), QColor(255, 255, 255), 2.9f, xPos+1.7f, yPos+2.0f, &painter);
lm's avatar
lm committed
1466 1467
        }
    }
1468 1469
}

1470
void HSIDisplay::drawObjects(QPainter &painter)
1471
{
1472
    Q_UNUSED(painter);
1473 1474
}

1475
void HSIDisplay::drawPositionDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
1476
{
1477
    if (xyControlKnown && xyControlEnabled) {
1478
        // Draw the needle
1479
        const float maxWidth = radius / 5.0f;
1480
        const float minWidth = maxWidth * 0.3f;
1481

1482
        float angle = atan2(posXSet, -posYSet);
1483
        angle -= (float)M_PI/2.0f;
1484

1485
        QPolygonF p(6);
1486

1487
        //radius *= ((posXSaturation + posYSaturation) - sqrt(pow(posXSet, 2), pow(posYSet, 2))) / (2*posXSaturation);
lm's avatar
lm committed
1488

1489
        radius *= sqrt(pow(posXSet, 2) + pow(posYSet, 2)) / sqrt(posXSaturation + posYSaturation);
lm's avatar
lm committed
1490

1491 1492 1493 1494 1495 1496
        p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f));
        p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f));
        p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f));
        p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.4f));
        p.replace(4, QPointF(xRef,               yRef-radius * 0.36f));
        p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f));
1497

1498
        rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef));
1499

1500 1501 1502 1503 1504 1505 1506
        QBrush indexBrush;
        indexBrush.setColor(color);
        indexBrush.setStyle(Qt::SolidPattern);
        painter->setPen(Qt::SolidLine);
        painter->setPen(color);
        painter->setBrush(indexBrush);
        drawPolygon(p, painter);
lm's avatar
lm committed
1507

1508 1509
        //qDebug() << "DRAWING POS SETPOINT X:" << posXSet << "Y:" << posYSet << angle;
    }
1510 1511
}

1512
void HSIDisplay::drawAttitudeDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
1513
{
1514
    if (attControlKnown && attControlEnabled) {
1515 1516 1517
        // Draw the needle
        const float maxWidth = radius / 10.0f;
        const float minWidth = maxWidth * 0.3f;
1518

1519
        float angle = atan2(attYSet, -attXSet);
lm's avatar
lm committed
1520

1521
        radius *= sqrt(attXSet*attXSet + attYSet*attYSet) / sqrt(attXSaturation*attXSaturation + attYSaturation*attYSaturation);
1522

1523
        QPolygonF p(6);
1524

1525 1526 1527 1528 1529 1530
        p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f));
        p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f));
        p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f));
        p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.4f));
        p.replace(4, QPointF(xRef,               yRef-radius * 0.36f));
        p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f));
1531

1532
        rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef));
1533

1534 1535 1536 1537 1538 1539 1540
        QBrush indexBrush;
        indexBrush.setColor(color);
        indexBrush.setStyle(Qt::SolidPattern);
        painter->setPen(Qt::SolidLine);
        painter->setPen(color);
        painter->setBrush(indexBrush);
        drawPolygon(p, painter);
1541

1542
        // TODO Draw Yaw indicator
lm's avatar
lm committed
1543

1544 1545
        //qDebug() << "DRAWING ATT SETPOINT X:" << attXSet << "Y:" << attYSet << angle;
    }
1546 1547 1548 1549
}

void HSIDisplay::drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
{
1550
    if (zControlKnown && zControlEnabled) {
1551 1552 1553 1554 1555 1556 1557 1558 1559 1560 1561 1562 1563 1564
        // Draw the circle
        QPen circlePen(Qt::SolidLine);
        circlePen.setWidth(refLineWidthToPen(0.5f));
        circlePen.setColor(color);
        painter->setBrush(Qt::NoBrush);
        painter->setPen(circlePen);
        drawCircle(xRef, yRef, radius, 200.0f, color, painter);
        //drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.0f, color, painter);

        //    // Draw the value
        //    QString label;
        //    label.sprintf("%05.1f", value);
        //    paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter);
    }
1565
}
1566

1567 1568 1569
void HSIDisplay::wheelEvent(QWheelEvent* event)
{
    double zoomScale = 0.005; // Scaling of zoom value
1570
    if(event->delta() > 0) {
1571 1572
        // Reduce width -> Zoom in
        metricWidth -= event->delta() * zoomScale;
1573
    } else {
1574 1575 1576
        // Increase width -> Zoom out
        metricWidth -= event->delta() * zoomScale;
    }
1577
    metricWidth = qBound(0.5, metricWidth, 9999.0);
1578 1579
    emit metricWidthChanged(metricWidth);
}
pixhawk's avatar
pixhawk committed
1580

1581 1582 1583 1584
void HSIDisplay::showEvent(QShowEvent* event)
{
    // React only to internal (pre-display)
    // events
1585
    Q_UNUSED(event) {
1586 1587 1588 1589 1590 1591 1592 1593
        refreshTimer->start(updateInterval);
    }
}

void HSIDisplay::hideEvent(QHideEvent* event)
{
    // React only to internal (post-display)
    // events
1594
    Q_UNUSED(event) {
1595
        refreshTimer->stop();
1596 1597 1598
    }
}

1599 1600
void HSIDisplay::updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat)
{
1601 1602 1603 1604 1605 1606
    Q_UNUSED(roll);
    Q_UNUSED(pitch);
    Q_UNUSED(yaw);
    Q_UNUSED(thrust);
    Q_UNUSED(xHat);
    Q_UNUSED(yHat);
1607 1608 1609 1610
}

void HSIDisplay::pressKey(int key)
{
1611
    Q_UNUSED(key);
1612
}
pixhawk's avatar
pixhawk committed
1613 1614