HSIDisplay.cc 46 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
47
48

#include <QDebug>

49

50
HSIDisplay::HSIDisplay(QWidget *parent) :
51
52
53
54
55
56
57
58
59
60
    HDDisplay(NULL, "HSI", parent),
    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
61
62
    attXSaturation(0.2f),
    attYSaturation(0.2f),
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
    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
88
    uiZSetCoordinate(0.0f),
89
90
91
92
93
94
95
96
97
98
99
100
101
    uiYawSet(0.0f),
    metricWidth(4.0),
    positionLock(false),
    attControlEnabled(false),
    xyControlEnabled(false),
    zControlEnabled(false),
    yawControlEnabled(false),
    positionFix(0),
    gpsFix(0),
    visionFix(0),
    laserFix(0),
    mavInitialized(false),
    bottomMargin(10.0f),
102
    dragStarted(false),
103
    topMargin(12.0f),
104
    leftDragStarted(false),
pixhawk's avatar
pixhawk committed
105
    mouseHasMoved(false),
106
    actionPending(false),
LM's avatar
LM committed
107
    directSending(false),
108
    userSetPointSet(false),
LM's avatar
LM committed
109
110
111
    userXYSetPointSet(false),
    userZSetPointSet(false),
    userYawSetPointSet(false)
112
{
113
    refreshTimer->setInterval(updateInterval);
114

pixhawk's avatar
pixhawk committed
115
    columns = 1;
116
    this->setAutoFillBackground(true);
117
118
119
    QPalette pal = palette();
    pal.setColor(backgroundRole(), QGC::colorBlack);
    setPalette(pal);
120

121
122
    vwidth = 80.0f;
    vheight = 80.0f;
pixhawk's avatar
pixhawk committed
123

124
    xCenterPos = vwidth/2.0f;
pixhawk's avatar
pixhawk committed
125
    yCenterPos = vheight/2.0f + topMargin - bottomMargin;
126

127
128
129
    uas = NULL;
    resetMAVState();

130
131
    // Do first update
    setMetricWidth(metricWidth);
132
133
134
    // 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."));
135
136

    connect(&statusClearTimer, SIGNAL(timeout()), this, SLOT(clearStatusMessage()));
137
138
139
    statusClearTimer.start(3000);

    setFocusPolicy(Qt::StrongFocus);
140
141
}

142
143
144
145
146
HSIDisplay::~HSIDisplay()
{

}

147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
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;
173
174
175
176

    // Setpoints
    positionSetPointKnown = false;
    setPointKnown = false;
177
178
}

lm's avatar
lm committed
179
180
181
182
void HSIDisplay::paintEvent(QPaintEvent * event)
{
    Q_UNUSED(event);
    //paintGL();
183
184
185
    //    static quint64 interval = 0;
    //    //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
    //    interval = MG::TIME::getGroundTimeNow();
186
    renderOverlay();
lm's avatar
lm committed
187
188
}

189
void HSIDisplay::renderOverlay()
190
{
191
    if (!isVisible()) return;
192
193
194
#if (QGC_EVENTLOOP_DEBUG)
    qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif
lm's avatar
lm committed
195
196
    // Center location of the HSI gauge items

197
    //float bottomMargin = 3.0f;
lm's avatar
lm committed
198
199

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

203
204
205
206
207
208
209
210
    // 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;

211
    QPainter painter(viewport());
212
213
    painter.setRenderHint(QPainter::Antialiasing, true);
    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
214

lm's avatar
lm committed
215
216
    // Draw base instrument
    // ----------------------
lm's avatar
lm committed
217
    painter.setBrush(Qt::NoBrush);
218
    const QColor ringColor = QColor(200, 250, 200);
lm's avatar
lm committed
219
220
221
222
    QPen pen;
    pen.setColor(ringColor);
    pen.setWidth(refLineWidthToPen(0.1f));
    painter.setPen(pen);
lm's avatar
lm committed
223
    const int ringCount = 2;
224
225
    for (int i = 0; i < ringCount; i++)
    {
226
        float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.35f * i+1) / 2.0f - bottomMargin / 2.0f;
227
        drawCircle(xCenterPos, yCenterPos, radius, 0.1f, ringColor, &painter);
228
        paintText(tr("%1 m").arg(refToMetric(radius), 5, 'f', 1, ' '), QGC::colorCyan, 1.6f, vwidth/2-4, vheight/2+radius+2.2, &painter);
lm's avatar
lm committed
229
230
    }

231
232
233
    // Draw orientation labels
    // Translate and rotate coordinate frame
    painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
234
235
236
    const float yawDeg = ((yaw/M_PI)*180.0f);
    int yawRotate = static_cast<int>(yawDeg) % 360;
    painter.rotate(-yawRotate);
237
238
    paintText(tr("N"), ringColor, 3.5f, - 1.0f, - baseRadius - 5.5f, &painter);
    paintText(tr("S"), ringColor, 3.5f, - 1.0f, + baseRadius + 1.5f, &painter);
239
    paintText(tr("E"), ringColor, 3.5f, + baseRadius + 3.0f, - 1.25f, &painter);
240
    paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter);
241
    painter.rotate(+yawRotate);
242
243
    painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);

244
    // Draw trail
245
246
247
248
249
250
    //    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);
251
252


lm's avatar
lm committed
253
    // Draw center indicator
254
255
256
257
258
    //    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);
259

260
261
    if (uas)
    {
262
263
264
        // Translate to center
        painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
        QColor uasColor = uas->getColor();
265
        MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
lm's avatar
lm committed
266
        //MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast<int>((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f);
267
268
269
        // Translate back
        painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
    }
lm's avatar
lm committed
270
271

    // ----------------------
pixhawk's avatar
pixhawk committed
272

273
274
275
    // Draw satellites
    drawGPS(painter);

pixhawk's avatar
pixhawk committed
276
277
278
    // Draw state indicator

    // Draw position
lm's avatar
lm committed
279
    QColor positionColor(20, 20, 200);
280
    drawPositionDirection(xCenterPos, yCenterPos, baseRadius, positionColor, &painter);
lm's avatar
lm committed
281
282
283

    // Draw attitude
    QColor attitudeColor(200, 20, 20);
284
    drawAttitudeDirection(xCenterPos, yCenterPos, baseRadius, attitudeColor, &painter);
pixhawk's avatar
pixhawk committed
285
286


287
    // Draw position setpoints in body coordinates
lm's avatar
lm committed
288

289
290
291
292
293
294
    float xSpDiff = uiXSetCoordinate - bodyXSetCoordinate;
    float ySpDiff = uiYSetCoordinate - bodyYSetCoordinate;
    float zSpDiff = uiZSetCoordinate - bodyZSetCoordinate;

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

295
296
297
298
    float angleDiff = uiYawSet - bodyYawSet;

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

299
300
    //if (((userSetPointSet) || (normAngleDiff > 0.05f) || dragStarted) && !(setPointDist < 0.08f && mavInitialized))

301

302
303
304
305
306
    // Labels on outer part and bottom

    // Draw waypoints
    drawWaypoints(painter);

307
    // Draw status flags
308
309
310
311
    drawStatusFlag(2,  1, tr("ATT"), attControlEnabled, attControlKnown, painter);
    drawStatusFlag(22, 1, tr("PXY"), xyControlEnabled,  xyControlKnown,  painter);
    drawStatusFlag(44, 1, tr("PZ"),  zControlEnabled,   zControlKnown,   painter);
    drawStatusFlag(66, 1, tr("YAW"), yawControlEnabled, yawControlKnown, painter);
312

313
    // Draw position lock indicators
314
315
316
317
    drawPositionLock(2,  5, tr("POS"), positionFix, positionFixKnown, painter);
    drawPositionLock(22, 5, tr("VIS"), visionFix,   visionFixKnown,   painter);
    drawPositionLock(44, 5, tr("GPS"), gpsFix,      gpsFixKnown,      painter);
    drawPositionLock(66, 5, tr("IRU"), iruFix,      iruFixKnown,      painter);
318
319
320

    // Draw speed to top left
    paintText(tr("SPEED"), QGC::colorCyan, 2.2f, 2, 11, &painter);
pixhawk's avatar
pixhawk committed
321
    paintText(tr("%1 m/s").arg(speed, 5, 'f', 2, '0'), Qt::white, 2.2f, 12, 11, &painter);
322
323
324

    // Draw crosstrack error to top right
    float crossTrackError = 0;
325
326
    paintText(tr("XTRACK"), QGC::colorCyan, 2.2f, 54, 11, &painter);
    paintText(tr("%1 m").arg(crossTrackError, 5, 'f', 2, '0'), Qt::white, 2.2f, 67, 11, &painter);
327
328

    // Draw position to bottom left
329
330
    if (localAvailable > 0)
    {
331
332
        // Position
        QString str;
333
        float offset = (globalAvailable > 0) ? -3.0f : 0.0f;
334
        str.sprintf("%05.2f %05.2f %05.2f m", x, y, z);
335
336
        paintText(tr("POS"), QGC::colorCyan, 2.6f, 2, vheight - offset - 2.0f, &painter);
        paintText(str, Qt::white, 2.6f, 10, vheight - offset - 2.0f, &painter);
337
338
    }

339
340
    if (globalAvailable > 0)
    {
341
342
        // Position
        QString str;
343
        str.sprintf("lat: %05.2f lon: %06.2f alt: %06.2f", lat, lon, alt);
344
345
        paintText(tr("GPS"), QGC::colorCyan, 2.6f, 2, vheight- 2.0f, &painter);
        paintText(str, Qt::white, 2.6f, 10, vheight - 2.0f, &painter);
346
347
    }

348
349
350
    // Draw Safety
    double x1, y1, z1, x2, y2, z2;
    UASManager::instance()->getLocalNEDSafetyLimits(&x1, &y1, &z1, &x2, &y2, &z2);
351
    //    drawSafetyArea(QPointF(x1, y1), QPointF(x2, y2), QGC::colorYellow, painter);
352
353

    // Draw status message
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
    paintText(statusMessage, QGC::colorOrange, 2.8f, 8, 15, &painter);

    // Draw setpoint over waypoints
    if (positionSetPointKnown || setPointKnown)
    {
        // Draw setpoint
        drawSetpointXYZYaw(bodyXSetCoordinate, bodyYSetCoordinate, bodyZSetCoordinate, bodyYawSet, QGC::colorYellow, painter);
        // Draw travel direction line
        QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
        // Transform from body to world coordinates
        m = metricWorldToBody(m);
        // Scale from metric body to screen reference units
        QPointF s = metricBodyToRef(m);
        drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::colorYellow, &painter);
    }

    if ((userSetPointSet || dragStarted) && ((normAngleDiff > 0.05f) || !(setPointDist < 0.08f && mavInitialized)))
    {
        QColor spColor(150, 250, 150);
        drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, spColor, painter);
    }
375
}
lm's avatar
lm committed
376

377
void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter)
378
{
379
    paintText(label, QGC::colorCyan, 2.6f, x, y+0.8f, &painter);
380
    QColor statusColor(250, 250, 250);
381
    if(status) {
pixhawk's avatar
pixhawk committed
382
        painter.setBrush(QGC::colorGreen);
383
    } else {
384
        painter.setBrush(QGC::colorDarkYellow);
385
386
    }
    painter.setPen(Qt::NoPen);
387
388
389
390
391

    float indicatorWidth = refToScreenX(7.0f);
    float indicatorHeight = refToScreenY(4.0f);

    painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), indicatorWidth, indicatorHeight));
392
    paintText((status) ? tr("ON") : tr("OFF"), statusColor, 2.6f, x+7.9f, y+0.8f, &painter);
393
    // Cross out instrument if state unknown
394
395
    if (!known)
    {
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
        QPen pen(Qt::yellow);
        pen.setWidth(2);
        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);
    }
413
414
}

415
void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter)
416
{
417
418
419
420
421
422
423
424
425
426
427
428
    paintText(label, QGC::colorCyan, 2.6f, x, y+0.8f, &painter);
    QColor negStatusColor(200, 20, 20);
    QColor intermediateStatusColor (Qt::yellow);
    QColor posStatusColor(20, 200, 20);
    QColor statusColor(250, 250, 250);
    if (status == 3) {
        painter.setBrush(posStatusColor);
    } else if (status == 2) {
        painter.setBrush(intermediateStatusColor.dark(150));
    } else {
        painter.setBrush(negStatusColor);
    }
429

430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
    // Lock text
    QString lockText;
    switch (status) {
    case 1:
        lockText = tr("LOC");
        break;
    case 2:
        lockText = tr("2D");
        break;
    case 3:
        lockText = tr("3D");
        break;
    default:
        lockText = tr("NO");
        break;
    }
446

447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
    float indicatorWidth = refToScreenX(7.0f);
    float indicatorHeight = refToScreenY(4.0f);

    painter.setPen(Qt::NoPen);
    painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), refToScreenX(7.0f), refToScreenY(4.0f)));
    paintText(lockText, statusColor, 2.6f, x+7.9f, y+0.8f, &painter);
    // Cross out instrument if state unknown
    if (!known) {
        QPen pen(Qt::yellow);
        pen.setWidth(2);
        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);
    }
472
473
}

lm's avatar
lm committed
474
475
void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock)
{
pixhawk's avatar
pixhawk committed
476
    Q_UNUSED(uas);
477
    bool changed = positionLock != lock;
lm's avatar
lm committed
478
    positionLock = lock;
479
480
481
482
    if (changed)
    {
        update();
    }
lm's avatar
lm committed
483
484
}

pixhawk's avatar
pixhawk committed
485
void HSIDisplay::updateAttitudeControllerEnabled(bool enabled)
lm's avatar
lm committed
486
{
487
    bool changed = attControlEnabled != enabled;
lm's avatar
lm committed
488
    attControlEnabled = enabled;
489
    attControlKnown = true;
490
491
492
493
    if (changed)
    {
        update();
    }
lm's avatar
lm committed
494
495
}

pixhawk's avatar
pixhawk committed
496
void HSIDisplay::updatePositionXYControllerEnabled(bool enabled)
lm's avatar
lm committed
497
{
498
    bool changed = xyControlEnabled != enabled;
lm's avatar
lm committed
499
    xyControlEnabled = enabled;
500
    xyControlKnown = true;
501
502
503
504
    if (changed)
    {
        update();
    }
lm's avatar
lm committed
505
506
}

pixhawk's avatar
pixhawk committed
507
void HSIDisplay::updatePositionZControllerEnabled(bool enabled)
lm's avatar
lm committed
508
{
509
    bool changed = zControlEnabled != enabled;
lm's avatar
lm committed
510
    zControlEnabled = enabled;
511
    zControlKnown = true;
512
513
514
515
    if (changed)
    {
        update();
    }
lm's avatar
lm committed
516
517
}

LM's avatar
LM committed
518
519
void HSIDisplay::updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance)
{
520
521
522
523
524
    Q_UNUSED(quality);
    Q_UNUSED(name);
    Q_UNUSED(type);
    Q_UNUSED(id);
    Q_UNUSED(time);
LM's avatar
LM committed
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
    // 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
540
541
542
543
QPointF HSIDisplay::metricWorldToBody(QPointF world)
{
    // First translate to body-centered coordinates
    // Rotate around -yaw
pixhawk's avatar
pixhawk committed
544
    float angle = -yaw - M_PI;
545
    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
546
547
548
549
550
551
552
    return result;
}

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

557
558
QPointF HSIDisplay::screenToMetricBody(QPointF ref)
{
559
    return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth);
560
561
562
563
}

QPointF HSIDisplay::refToMetricBody(QPointF &ref)
{
lm's avatar
Changes    
lm committed
564
    return QPointF(-((ref.y() - yCenterPos)/ vwidth) * metricWidth - x, ((ref.x() - xCenterPos) / vwidth) * metricWidth - y);
565
566
567
568
569
}

/**
 * @see refToScreenX()
 */
pixhawk's avatar
pixhawk committed
570
QPointF HSIDisplay::metricBodyToRef(QPointF &metric)
571
572
573
574
{
    return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos);
}

575
576
577
578
579
580
581
582
583
584
double HSIDisplay::metricToRef(double metric)
{
    return (metric / metricWidth) * vwidth;
}

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

pixhawk's avatar
pixhawk committed
585
586
587
588
589
590
591
592
QPointF HSIDisplay::metricBodyToScreen(QPointF metric)
{
    QPointF ref = metricBodyToRef(metric);
    ref.setX(refToScreenX(ref.x()));
    ref.setY(refToScreenY(ref.y()));
    return ref;
}

593
594
void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
{
595
596
    if (event->type() == QMouseEvent::MouseButtonDblClick)
    {
597
        QPointF p = screenToMetricBody(event->posF());
LM's avatar
LM committed
598
        if (!directSending) setBodySetpointCoordinateXY(p.x(), p.y());
599
        //        qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos;
600
601
602
603
604
    }
}

void HSIDisplay::mouseReleaseEvent(QMouseEvent * event)
{
LM's avatar
LM committed
605
606
    // FIXME hardcode yaw to current value
    setBodySetpointCoordinateYaw(0);
607
    if (mouseHasMoved)
608
    {
609
610
611
612
        if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton)
        {
            if (dragStarted)
            {
LM's avatar
LM committed
613
                if (!directSending) setBodySetpointCoordinateYaw(uiYawSet);
614
615
616
617
618
619
620
                dragStarted = false;
            }
        }
        if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::LeftButton)
        {
            if (leftDragStarted)
            {
LM's avatar
LM committed
621
                if (!directSending) setBodySetpointCoordinateZ(uiZSetCoordinate);
622
                leftDragStarted = false;
623
624
            }
        }
625
    }
626
    mouseHasMoved = false;
627
628
629
630
631
632
633
634
635
}

void HSIDisplay::mousePressEvent(QMouseEvent * event)
{
    if (event->type() == QMouseEvent::MouseButtonPress)
    {
        if (event->button() == Qt::RightButton)
        {
            startX = event->x();
636
637
            // Start tracking mouse move
            dragStarted = true;
638
639
640
        }
        else if (event->button() == Qt::LeftButton)
        {
641
642
            startY = event->y();
            leftDragStarted = true;
643
        }
pixhawk's avatar
pixhawk committed
644
    }
645
    mouseHasMoved = false;
646
}
647

648
649
650
651
void HSIDisplay::mouseMoveEvent(QMouseEvent * event)
{
    if (event->type() == QMouseEvent::MouseMove)
    {
652
        if (dragStarted) uiYawSet -= 0.06f*(startX - event->x()) / this->frameSize().width();
653
654
655

        if (leftDragStarted)
        {
656
657
            //            uiZSetCoordinate -= 0.06f*(startY - event->y()) / this->frameSize().height();
            //            setBodySetpointCoordinateZ(uiZSetCoordinate);
658
        }
659
660

        if (leftDragStarted || dragStarted) mouseHasMoved = true;
pixhawk's avatar
pixhawk committed
661
    }
662
663
}

pixhawk's avatar
pixhawk committed
664
665
void HSIDisplay::keyPressEvent(QKeyEvent* event)
{
666
667
    QPointF bodySP = metricWorldToBody(QPointF(uiXSetCoordinate, uiYSetCoordinate));

668
    if ((event->key() == Qt::Key_Enter || event->key() == Qt::Key_Return) && actionPending)
pixhawk's avatar
pixhawk committed
669
670
671
672
673
674
    {
        actionPending = false;
        statusMessage = "SETPOINT SENT";
        statusClearTimer.start();
        sendBodySetPointCoordinates();
    }
LM's avatar
LM committed
675
    else
676
    {
LM's avatar
LM committed
677
678
679
680
681
682
683
        // FIXME hardcode yaw to current value
        setBodySetpointCoordinateYaw(0);


        // Reset setpoints to current position / orientation
        // if not initialized
        if (!userYawSetPointSet)
684
        {
LM's avatar
LM committed
685
            setBodySetpointCoordinateYaw(0);
686
        }
LM's avatar
LM committed
687
688

        if (!userZSetPointSet)
689
        {
LM's avatar
LM committed
690
            setBodySetpointCoordinateZ(0);
691
692
        }

LM's avatar
LM committed
693
        if (!userXYSetPointSet)
694
        {
LM's avatar
LM committed
695
            setBodySetpointCoordinateXY(0, 0);
696
        }
LM's avatar
LM committed
697
698

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

LM's avatar
LM committed
703
        if ((event->key() ==  Qt::Key_S))
704
        {
LM's avatar
LM committed
705
            setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y());
706
        }
LM's avatar
LM committed
707
708

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

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

LM's avatar
LM committed
718
        if ((event->key() ==  Qt::Key_Up))
719
        {
LM's avatar
LM committed
720
            setBodySetpointCoordinateZ(-0.5);
721
722
        }

LM's avatar
LM committed
723
        if ((event->key() ==  Qt::Key_Down))
724
        {
LM's avatar
LM committed
725
            setBodySetpointCoordinateZ(+0.5);
726
727
        }

LM's avatar
LM committed
728
        if ((event->key() ==  Qt::Key_Left))
729
        {
LM's avatar
LM committed
730
            setBodySetpointCoordinateYaw(-0.2);
731
732
        }

LM's avatar
LM committed
733
        if ((event->key() ==  Qt::Key_Right))
734
        {
LM's avatar
LM committed
735
            setBodySetpointCoordinateYaw(0.2);
736
        }
PIXHAWK Team's avatar
PIXHAWK Team committed
737
    }
738
739


LM's avatar
LM committed
740
741
742
    // 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
743
    {
LM's avatar
LM committed
744
        setBodySetpointCoordinateXY(0, 0);
745
746
747
        setBodySetpointCoordinateZ(0);
        setBodySetpointCoordinateYaw(0);
        statusMessage = "CANCELLED, PRESS <ENTER> TO SEND";
PIXHAWK Team's avatar
PIXHAWK Team committed
748
    }
749
750

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

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

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

768
769
770
771
772
void HSIDisplay::contextMenuEvent (QContextMenuEvent* event)
{
    event->ignore();
}

773
774
void HSIDisplay::setMetricWidth(double width)
{
775
    if (width != metricWidth) {
776
777
778
779
780
        metricWidth = width;
        emit metricWidthChanged(metricWidth);
    }
}

781
782
783
784
785
786
/**
 *
 * @param uas the UAS/MAV to monitor/display with the HUD
 */
void HSIDisplay::setActiveUAS(UASInterface* uas)
{
787
788
789
790
791
792
    if (this->uas != NULL) {
        disconnect(this->uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
        disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
        disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
        disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
        disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
793
        disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
794
795
796
797
798
799
800
801
802
803
804
805
        disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
        disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));

        disconnect(this->uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool)));
        disconnect(this->uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(bool)));
        disconnect(this->uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(bool)));
        disconnect(this->uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(bool)));

        disconnect(this->uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int)));
        disconnect(this->uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int)));
        disconnect(this->uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int)));
        disconnect(this->uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int)));
LM's avatar
LM committed
806
        disconnect(this->uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float)));
807
    }
808

809
810
811
812
813
    connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
    connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
    connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
    connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
    connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
814
    connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
815
816
    connect(uas, SIGNAL(speedChanged(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)));
817

818
819
820
821
    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)));
822

823
824
825
826
    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)));
LM's avatar
LM committed
827
    connect(uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float)));
828

829
    this->uas = uas;
830

831
    resetMAVState();
832
833
}

834
835
836
837
838
839
840
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;
pixhawk's avatar
pixhawk committed
841
    this->speed = sqrt(pow(vx, 2.0) + pow(vy, 2.0) + pow(vz, 2.0));
842
843
844
845
}

void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
{
846
847
848
849
850
    if (uas)
    {
        userSetPointSet = true;
        userXYSetPointSet = true;
        // Set coordinates and send them out to MAV
851

852
853
854
855
856
857
        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
858

pixhawk's avatar
pixhawk committed
859

pixhawk's avatar
pixhawk committed
860
861
862
863
        //sendBodySetPointCoordinates();
        statusMessage = "POSITION SET, PRESS <ENTER> TO SEND";
        actionPending = true;
        statusClearTimer.start();
pixhawk's avatar
pixhawk committed
864
    }
865
866
867
868
}

void HSIDisplay::setBodySetpointCoordinateZ(double z)
{
869
870
871
    if (uas)
    {
        userSetPointSet = true;
LM's avatar
LM committed
872
        userZSetPointSet = true;
873
874
875
876
877
878
        // 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
879
    //sendBodySetPointCoordinates();
880
881
882
883
}

void HSIDisplay::setBodySetpointCoordinateYaw(double yaw)
{
884
    if (uas)
885
    {
886
887
888
889
890
891
892
893
894
895
896
897
        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
898
        userYawSetPointSet = true;
899
900
901
902
903
        // 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;
904
    }
pixhawk's avatar
pixhawk committed
905
    //sendBodySetPointCoordinates();
906
907
908
909
910
}

void HSIDisplay::sendBodySetPointCoordinates()
{
    // Send the coordinates to the MAV
911
    if (uas)
912
    {
913
        uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
914
    }
915
916
}

lm's avatar
lm committed
917
void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec)
pixhawk's avatar
pixhawk committed
918
{
lm's avatar
lm committed
919
920
921
922
923
924
    Q_UNUSED(uas);
    Q_UNUSED(usec);
    attXSet = pitchDesired;
    attYSet = rollDesired;
    attYawSet = yawDesired;
    altitudeSet = thrustDesired;
pixhawk's avatar
pixhawk committed
925
926
}

lm's avatar
lm committed
927
928
929
930
931
932
933
934
935
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;
}

936
937
void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired)
{
938
	Q_UNUSED(uasid);
939
940
941
942
943
    uiXSetCoordinate = xDesired;
    uiYSetCoordinate = yDesired;
    uiZSetCoordinate = zDesired;
    uiYawSet = yawDesired;
    userXYSetPointSet = true;
LM's avatar
LM committed
944
945
    userZSetPointSet = true;
    userYawSetPointSet = true;
946
947
948
    userSetPointSet = true;
}

lm's avatar
lm committed
949
void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec)
pixhawk's avatar
pixhawk committed
950
{
lm's avatar
lm committed
951
    Q_UNUSED(uasid);
952
    Q_UNUSED(usec);
953
954
955
956
    bodyXSetCoordinate = xDesired;
    bodyYSetCoordinate = yDesired;
    bodyZSetCoordinate = zDesired;
    bodyYawSet = yawDesired;
pixhawk's avatar
pixhawk committed
957
    mavInitialized = true;
958
    setPointKnown = true;
959
    positionSetPointKnown = true;
pixhawk's avatar
pixhawk committed
960

961
962
963
964
    if (!userSetPointSet && !dragStarted)
    {
        uiXSetCoordinate = bodyXSetCoordinate;
        uiYSetCoordinate = bodyYSetCoordinate;
965
        //        uiZSetCoordinate = bodyZSetCoordinate;
966
967
        uiYawSet= bodyYawSet;
    }
pixhawk's avatar
pixhawk committed
968
969
970
971
}

void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec)
{
lm's avatar
lm committed
972
973
974
975
    this->x = x;
    this->y = y;
    this->z = z;
    localAvailable = usec;
pixhawk's avatar
pixhawk committed
976
977
978
979
}

void HSIDisplay::updateGlobalPosition(UASInterface*, double lat, double lon, double alt, quint64 usec)
{
lm's avatar
lm committed
980
981
982
983
    this->lat = lat;
    this->lon = lon;
    this->alt = alt;
    globalAvailable = usec;
pixhawk's avatar
pixhawk committed
984
985
}

lm's avatar
lm committed
986
void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float azimuth, float snr, bool used)
lm's avatar
lm committed
987
988
{
    Q_UNUSED(uasid);
lm's avatar
lm committed
989
    //qDebug() << "UPDATED SATELLITE";
lm's avatar
lm committed
990
    // If slot is empty, insert object
991
    if (gpsSatellites.contains(satid)) {
lm's avatar
lm committed
992
        gpsSatellites.value(satid)->update(satid, elevation, azimuth, snr, used);
993
    } else {
lm's avatar
lm committed
994
        gpsSatellites.insert(satid, new GPSSatellite(satid, elevation, azimuth, snr, used));
lm's avatar
lm committed
995
996
997
    }
}

998
999
1000
void HSIDisplay::updatePositionYawControllerEnabled(bool enabled)
{
    yawControlEnabled = enabled;
For faster browsing, not all history is shown. View entire blame