HSIDisplay.cc 54.2 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 <QDebug>

40
41
#include "UASManager.h"
#include "HSIDisplay.h"
42
#include "QGC.h"
pixhawk's avatar
pixhawk committed
43
#include "Waypoint.h"
44
#include "UASWaypointManager.h"
45
#include <qmath.h>
46
#include "MAV2DIcon.h"
47
#include "QGCApplication.h"
48

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

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

162
    vwidth = 80.0f;
163
    vheight = 100.0f;
pixhawk's avatar
pixhawk committed
164

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

168
169
170
    uas = NULL;
    resetMAVState();

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

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

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

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

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

191
    setFocusPolicy(Qt::StrongFocus);
192
193
}

194
195
196
197
198
HSIDisplay::~HSIDisplay()
{

}

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

    // Setpoints
    positionSetPointKnown = false;
    setPointKnown = false;
229
230
}

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

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

249
    //float bottomMargin = 3.0f;
lm's avatar
lm committed
250
251

    // Size of the ring instrument
pixhawk's avatar
pixhawk committed
252
    //const float margin = 0.1f;  // 10% margin of total width on each side
253
    float baseRadius = (vheight - topMargin - bottomMargin) / 2.0f - (topMargin + bottomMargin) / 2.8f;
254

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

263
    QPainter painter(viewport());
264
265
    painter.setRenderHint(QPainter::Antialiasing, true);
    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
266

lm's avatar
lm committed
267
268
    // Draw base instrument
    // ----------------------
lm's avatar
lm committed
269
    painter.setBrush(Qt::NoBrush);
270
271
272
273
274
275
276
277
278
279

    // 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;
280
    if (qgcApp()->styleIsDark())
281
    {
282
        ringColor = QColor(255, 255, 255);
283
284
        positionColor = QColor(20, 20, 200);
        setpointColor = QColor(150, 250, 150);
285
286
        labelColor = QGC::colorCyan;
        valueColor = QColor(255, 255, 255);
287
        statusColor = QGC::colorOrange;
288
        waypointLineColor = QGC::colorYellow;
289
290
291
292
        attitudeColor = QColor(200, 20, 20);
    }
    else
    {
293
        ringColor = QGC::colorBlack;
294
295
        positionColor = QColor(20, 20, 200);
        setpointColor = QColor(150, 250, 150);
296
297
        labelColor = QColor(26, 75, 95);
        valueColor = QColor(40, 40, 40);
298
        statusColor = QGC::colorOrange;
299
        waypointLineColor = QGC::colorDarkYellow;
300
301
302
        attitudeColor = QColor(200, 20, 20);
    }

lm's avatar
lm committed
303
304
    QPen pen;
    pen.setColor(ringColor);
305
    pen.setWidth(refLineWidthToPen(1.0f));
lm's avatar
lm committed
306
    painter.setPen(pen);
lm's avatar
lm committed
307
    const int ringCount = 2;
308
309
    for (int i = 0; i < ringCount; i++)
    {
310
        float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.35f * i+1) / 2.0f - bottomMargin / 2.0f;
311
        drawCircle(xCenterPos, yCenterPos, radius, 1.0f, ringColor, &painter);
312
        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
313
314
    }

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

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


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

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

    // ----------------------
pixhawk's avatar
pixhawk committed
356

357
358
359
    // Draw satellites
    drawGPS(painter);

pixhawk's avatar
pixhawk committed
360
361
362
    // Draw state indicator

    // Draw position
363
    drawPositionDirection(xCenterPos, yCenterPos, baseRadius, positionColor, &painter);
lm's avatar
lm committed
364
365

    // Draw attitude
366
    drawAttitudeDirection(xCenterPos, yCenterPos, baseRadius, attitudeColor, &painter);
pixhawk's avatar
pixhawk committed
367
368


369
    // Draw position setpoints in body coordinates
lm's avatar
lm committed
370

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

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

377
378
379
380
    float angleDiff = uiYawSet - bodyYawSet;

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

381
382
383
384
385
    // Labels on outer part and bottom

    // Draw waypoints
    drawWaypoints(painter);

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

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

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

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

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

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

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

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

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

    if ((userSetPointSet || dragStarted) && ((normAngleDiff > 0.05f) || !(setPointDist < 0.08f && mavInitialized)))
    {
463
        drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, setpointColor, painter);
464
    }
465
}
lm's avatar
lm committed
466

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

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

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

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

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

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

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

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

573
574
575
576
577
578
    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);
579

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

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

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

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

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

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

QPointF HSIDisplay::metricBodyToWorld(QPointF body)
{
    // First rotate into world coordinates
    // then translate to world position
680
    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
681
    return result;
682
}
lm's avatar
lm committed
683

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

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

/**
 * @see refToScreenX()
 */
pixhawk's avatar
pixhawk committed
697
QPointF HSIDisplay::metricBodyToRef(QPointF &metric)
698
699
700
701
{
    return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos);
}

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

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

pixhawk's avatar
pixhawk committed
712
713
714
715
716
717
718
719
QPointF HSIDisplay::metricBodyToScreen(QPointF metric)
{
    QPointF ref = metricBodyToRef(metric);
    ref.setX(refToScreenX(ref.x()));
    ref.setY(refToScreenY(ref.y()));
    return ref;
}

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

        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)));
948
    }
949

950
951
    if (uas)
    {
952
953
954
955
        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)));
956
957
        connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,double,quint64)),
                this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64)));
958
959
        connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)),
                this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64)));
960
961
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
        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)),
For faster browsing, not all history is shown. View entire blame