HDDisplay.cc 39.9 KB
Newer Older
pixhawk's avatar
pixhawk committed
1
2
3
4
5
6
7
8
9
10
11
12
13
/*=====================================================================
======================================================================*/

/**
 * @file
 *   @brief Implementation of Head Down Display (HDD)
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QFile>
#include <QGLWidget>
pixhawk's avatar
pixhawk committed
14
#include <QStringList>
15
#include <QGraphicsTextItem>
16
17
#include <QDockWidget>
#include <QInputDialog>
18
#include <QMouseEvent>
19
20
#include <QMenu>
#include <QSettings>
21
#include <qmath.h>
pixhawk's avatar
pixhawk committed
22
23
24
#include "UASManager.h"
#include "HDDisplay.h"
#include "ui_HDDisplay.h"
25
#include "MG.h"
26
#include "QGC.h"
pixhawk's avatar
pixhawk committed
27
28
#include <QDebug>

29
HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) :
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
    QGraphicsView(parent),
    uas(NULL),
    xCenterOffset(0.0f),
    yCenterOffset(0.0f),
    vwidth(80.0f),
    vheight(80.0f),
    backgroundColor(QColor(0, 0, 0)),
    defaultColor(QColor(70, 200, 70)),
    setPointColor(QColor(200, 20, 200)),
    warningColor(Qt::yellow),
    criticalColor(Qt::red),
    infoColor(QColor(20, 200, 20)),
    fuelColor(criticalColor),
    warningBlinkRate(5),
    refreshTimer(new QTimer(this)),
    hardwareAcceleration(true),
    strongStrokeWidth(1.5f),
    normalStrokeWidth(1.0f),
    fineStrokeWidth(0.5f),
    acceptList(new QStringList()),
    acceptUnitList(new QStringList()),
    lastPaintTime(0),
    columns(3),
53
54
    valuesChanged(true),
    m_ui(NULL)
pixhawk's avatar
pixhawk committed
55
{
56
    setWindowTitle(title);
pixhawk's avatar
pixhawk committed
57
58
    //m_ui->setupUi(this);

59
60
    setAutoFillBackground(true);

61
    // Add all items in accept list to gauge
62
63
    if (plotList) {
        for(int i = 0; i < plotList->length(); ++i) {
64
65
            addGauge(plotList->at(i));
        }
66
67
    }

68
    restoreState();
69
70
71
    // Set preferred size
    setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);

72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
    createActions();

    //    setBackgroundBrush(QBrush(backgroundColor));
    //    setDragMode(QGraphicsView::ScrollHandDrag);
    //    setCacheMode(QGraphicsView::CacheBackground);
    //    // FIXME Handle full update with care - ressource intensive
    //    setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
    //
    //    setRenderHints(QPainter::Antialiasing | QPainter::SmoothPixmapTransform);
    //
    //    //Set-up the scene
    //    QGraphicsScene* Scene = new QGraphicsScene(this);
    //    setScene(Scene);
    //
    //    //Populate the scene
    //    for(int x = 0; x < 1000; x = x + 25) {
    //        for(int y = 0; y < 1000; y = y + 25) {
    //
    //            if(x % 100 == 0 && y % 100 == 0) {
    //                Scene->addRect(x, y, 2, 2);
    //
    //                QString pointString;
    //                QTextStream stream(&pointString);
    //                stream << "(" << x << "," << y << ")";
    //                QGraphicsTextItem* item = Scene->addText(pointString);
    //                item->setPos(x, y);
    //            } else {
    //                Scene->addRect(x, y, 1, 1);
    //            }
    //        }
    //    }
    //
    //    //Set-up the view
    //    setSceneRect(0, 0, 1000, 1000);
    //    setCenter(QPointF(500.0, 500.0)); //A modified version of centerOn(), handles special cases
    //    setCursor(Qt::OpenHandCursor);
108
109


110
    // Set minimum size
111
112
    this->setMinimumHeight(125);
    this->setMinimumWidth(100);
pixhawk's avatar
pixhawk committed
113

LM's avatar
LM committed
114
115
    scalingFactor = this->width()/vwidth;

pixhawk's avatar
pixhawk committed
116
    // Refresh timer
117
    refreshTimer->setInterval(180); //
118
    connect(refreshTimer, SIGNAL(timeout()), this, SLOT(triggerUpdate()));
pixhawk's avatar
pixhawk committed
119
120
121
122
123
124
125
126
    //connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintGL()));

    fontDatabase = QFontDatabase();
    const QString fontFileName = ":/general/vera.ttf"; ///< Font file is part of the QRC file and compiled into the app
    const QString fontFamilyName = "Bitstream Vera Sans";
    if(!QFile::exists(fontFileName)) qDebug() << "ERROR! font file: " << fontFileName << " DOES NOT EXIST!";

    fontDatabase.addApplicationFont(fontFileName);
lm's avatar
lm committed
127
    font = fontDatabase.font(fontFamilyName, "Roman", qMax(5, (int)(10*scalingFactor*1.2f+0.5f)));
pixhawk's avatar
pixhawk committed
128
129
130
    if (font.family() != fontFamilyName) qDebug() << "ERROR! Font not loaded: " << fontFamilyName;

    // Connect with UAS
pixhawk's avatar
pixhawk committed
131
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
pixhawk's avatar
pixhawk committed
132
133
134
135
136
    //start();
}

HDDisplay::~HDDisplay()
{
137
    saveState();
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
	if(this->refreshTimer)
	{
		delete this->refreshTimer;
	}
	if(this->acceptList)
	{
		delete this->acceptList;
	}
	if(this->acceptUnitList)
	{
		delete this->acceptUnitList;
	}
	if(this->m_ui)
	{
		delete m_ui;
	}
pixhawk's avatar
pixhawk committed
154
155
}

156
157
QSize HDDisplay::sizeHint() const
{
pixhawk's avatar
pixhawk committed
158
    return QSize(400, 400.0f*(vwidth/vheight)*1.2f);
159
160
}

161
162
void HDDisplay::enableGLRendering(bool enable)
{
163
    Q_UNUSED(enable);
164
165
}

166
167
168
void HDDisplay::triggerUpdate()
{
    // Only repaint the regions necessary
169
    update(this->geometry());
170
171
}

172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
//void HDDisplay::updateValue(UASInterface* uas, const QString& name, const QString& unit, double value, quint64 msec)
//{
//    // UAS is not needed
//    Q_UNUSED(uas);

//    if (!isnan(value) && !isinf(value))
//    {
//        // Update mean
//        const float oldMean = valuesMean.value(name, 0.0f);
//        const int meanCount = valuesCount.value(name, 0);
//        double mean = (oldMean * meanCount +  value) / (meanCount + 1);
//        if (isnan(mean) || isinf(mean)) mean = 0.0;
//        valuesMean.insert(name, mean);
//        valuesCount.insert(name, meanCount + 1);
//        // Two-value sliding average
//        double dot = (valuesDot.value(name) + (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f))/2.0f;
//        if (isnan(dot) || isinf(dot))
//        {
//            dot = 0.0;
//        }
//        valuesDot.insert(name, dot);
//        values.insert(name, value);
//        lastUpdate.insert(name, msec);
//        //}

//        //qDebug() << __FILE__ << __LINE__ << "VALUE:" << value << "MEAN:" << mean << "DOT:" << dot << "COUNT:" << meanCount;
//    }
//}

pixhawk's avatar
pixhawk committed
201
202
void HDDisplay::paintEvent(QPaintEvent * event)
{
203
    Q_UNUSED(event);
204
    quint64 interval = 0;
lm's avatar
lm committed
205
    //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
206
    interval = QGC::groundTimeMilliseconds();
207
    renderOverlay();
208
209
}

210
211
212
213
214
215
216
void HDDisplay::contextMenuEvent (QContextMenuEvent* event)
{
    QMenu menu(this);
    menu.addAction(addGaugeAction);
    menu.addActions(getItemRemoveActions());
    menu.addSeparator();
    menu.addAction(setColumnsAction);
217
218
219
220
221
222
    // Title change would ruin settings
    // this can only be allowed once
    // HDDisplays are instantiated
    // by a factory method based on
    // QSettings
    //menu.addAction(setTitleAction);
223
224
225
226
227
228
229
230
231
    menu.exec(event->globalPos());
}

void HDDisplay::saveState()
{
    QSettings settings;

    QString instruments;
    // Restore instrument settings
232
    for (int i = 0; i < acceptList->count(); i++) {
233
        QString key = acceptList->at(i);
234
        instruments += "|" + QString::number(minValues.value(key, -1.0))+","+key+","+acceptUnitList->at(i)+","+QString::number(maxValues.value(key, +1.0))+","+customNames.value(key, "")+","+((symmetric.value(key, false)) ? "s" : "");
235
236
    }

237
    // qDebug() << "Saving" << instruments;
238
239
240
241
242
243
244
245
246
247

    settings.setValue(windowTitle()+"_gauges", instruments);
    settings.sync();
}

void HDDisplay::restoreState()
{
    QSettings settings;
    settings.sync();

248
249
    acceptList->clear();

250
    QStringList instruments = settings.value(windowTitle()+"_gauges").toString().split('|');
251
    for (int i = 0; i < instruments.count(); i++) {
252
253
254
255
256
257
258
        addGauge(instruments.at(i));
    }
}

QList<QAction*> HDDisplay::getItemRemoveActions()
{
    QList<QAction*> actions;
259
    for(int i = 0; i < acceptList->length(); ++i) {
260
261
262
263
264
265
266
267
268
269
270
271
272
        QString gauge = acceptList->at(i);
        QAction* remove = new QAction(tr("Remove %1 gauge").arg(gauge), this);
        remove->setStatusTip(tr("Removes the %1 gauge from the view.").arg(gauge));
        remove->setData(gauge);
        connect(remove, SIGNAL(triggered()), this, SLOT(removeItemByAction()));
        actions.append(remove);
    }
    return actions;
}

void HDDisplay::removeItemByAction()
{
    QAction* trigger = qobject_cast<QAction*>(QObject::sender());
273
    if (trigger) {
274
275
276
277
278
        QString item = trigger->data().toString();
        int index = acceptList->indexOf(item);
        acceptList->removeAt(index);
        minValues.remove(item);
        maxValues.remove(item);
pixhawk's avatar
pixhawk committed
279
280
        symmetric.remove(item);
        adjustGaugeAspectRatio();
281
282
283
284
285
286
    }
}

void HDDisplay::addGauge()
{
    QStringList items;
287
    for (int i = 0; i < values.count(); ++i) {
288
        QString key = values.keys().at(i);
289
290
291
292
293
294
295
        QString label = key;
        QStringList keySplit = key.split(".");
        if (keySplit.size() > 1)
        {
            keySplit.removeFirst();
            label = keySplit.join(".");
        }
296
        QString unit = units.value(key);
297
        if (unit.contains("deg") || unit.contains("rad")) {
298
            items.append(QString("%1,%2,%3,%4,%5,s").arg("-180").arg(key).arg(unit).arg("+180").arg(label));
299
        } else {
300
            items.append(QString("%1,%2,%3,%4,%5").arg("0").arg(key).arg(unit).arg("+100").arg(label));
301
        }
302
303
304
    }
    bool ok;
    QString item = QInputDialog::getItem(this, tr("Add Gauge Instrument"),
305
                                         tr("Format: min, data name, unit, max, label [,s]"), items, 0, true, &ok);
306
    if (ok && !item.isEmpty()) {
307
308
309
310
311
312
        addGauge(item);
    }
}

void HDDisplay::addGauge(const QString& gauge)
{
313
    if (gauge.length() > 0) {
314
        QStringList parts = gauge.split(',');
315
        if (parts.count() > 2) {
316
317
            double val;
            bool ok;
lm's avatar
lm committed
318
            bool success = true;
319
320

            QString key = parts.at(1);
lm's avatar
lm committed
321
            QString unit = parts.at(2);
322

323
            if (!acceptList->contains(key)) {
324
325
                // Convert min to double number
                val = parts.first().toDouble(&ok);
lm's avatar
lm committed
326
                success &= ok;
327
328
                if (ok) minValues.insert(key, val);
                // Convert max to double number
lm's avatar
lm committed
329
330
                val = parts.at(3).toDouble(&ok);
                success &= ok;
331
                if (ok) maxValues.insert(key, val);
332
333
334
335
336
337
338
339
                // Convert name
                if (parts.length() >= 5)
                {
                    if (parts.at(4).length() > 0)
                    {
                        customNames.insert(key, parts.at(4));
                    }
                }
pixhawk's avatar
pixhawk committed
340
                // Convert symmetric flag
341
342
343
344
                if (parts.length() >= 6)
                {
                    if (parts.at(5).contains("s"))
                    {
pixhawk's avatar
pixhawk committed
345
346
347
                        symmetric.insert(key, true);
                    }
                }
348
                if (success) {
lm's avatar
lm committed
349
350
351
352
                    // Add value to acceptlist
                    acceptList->append(key);
                    acceptUnitList->append(unit);
                }
353
            }
354
355
        } else if (parts.count() > 1) {
            if (!acceptList->contains(gauge)) {
lm's avatar
lm committed
356
357
                acceptList->append(parts.at(0));
                acceptUnitList->append(parts.at(1));
358
359
360
            }
        }
    }
361
    adjustGaugeAspectRatio();
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
}

void HDDisplay::createActions()
{
    addGaugeAction = new QAction(tr("New &Gauge"), this);
    addGaugeAction->setStatusTip(tr("Add a new gauge to the view by adding its name from the linechart"));
    connect(addGaugeAction, SIGNAL(triggered()), this, SLOT(addGauge()));

    setTitleAction = new QAction(tr("Set Widget Title"), this);
    setTitleAction->setStatusTip(tr("Set the title caption of this tool widget"));
    connect(setTitleAction, SIGNAL(triggered()), this, SLOT(setTitle()));

    setColumnsAction = new QAction(tr("Set Number of Instrument Columns"), this);
    setColumnsAction->setStatusTip(tr("Set number of columns to draw"));
    connect(setColumnsAction, SIGNAL(triggered()), this, SLOT(setColumns()));
}


void HDDisplay::setColumns()
{
    bool ok;
    int i = QInputDialog::getInt(this, tr("Number of Instrument Columns"),
                                 tr("Columns:"), columns, 1, 15, 1, &ok);
385
    if (ok) {
386
387
388
389
390
391
392
        columns = i;
    }
}

void HDDisplay::setColumns(int cols)
{
    columns = cols;
393
394
395
396
397
398
399
400
401
402
    adjustGaugeAspectRatio();
}

void HDDisplay::adjustGaugeAspectRatio()
{
    // Adjust vheight dynamically according to the number of rows
    float vColWidth = vwidth / columns;
    int vRows = ceil(acceptList->length()/(float)columns);
    // Assuming square instruments, vheight is column width*row count
    vheight = vColWidth * vRows;
403
404
405
406
407
}

void HDDisplay::setTitle()
{
    QDockWidget* parent = dynamic_cast<QDockWidget*>(this->parentWidget());
408
    if (parent) {
409
410
411
412
413
414
415
416
417
418
        bool ok;
        QString text = QInputDialog::getText(this, tr("New title"),
                                             tr("Widget title:"), QLineEdit::Normal,
                                             parent->windowTitle(), &ok);
        if (ok && !text.isEmpty())
            parent->setWindowTitle(text);
        this->setWindowTitle(text);
    }
}

419
void HDDisplay::renderOverlay()
420
{
421
422
    if (!valuesChanged || !isVisible()) return;

423
424
425
#if (QGC_EVENTLOOP_DEBUG)
    qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif
426
    quint64 refreshInterval = 100;
427
    quint64 currTime = MG::TIME::getGroundTimeNow();
428
    if (currTime - lastPaintTime < refreshInterval) {
429
430
431
432
        // FIXME Need to find the source of the spurious paint events
        //return;
    }
    lastPaintTime = currTime;
pixhawk's avatar
pixhawk committed
433
434
435
436
437
    // 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;
438

pixhawk's avatar
pixhawk committed
439
440
441
    double scalingFactorH = this->height()/vheight;
    if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH;

442
    QPainter painter(viewport());
pixhawk's avatar
pixhawk committed
443
444
    painter.setRenderHint(QPainter::Antialiasing, true);
    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
445
    //painter.fillRect(QRect(0, 0, width(), height()), backgroundColor);
pixhawk's avatar
pixhawk committed
446
    const float spacing = 0.4f; // 40% of width
447
    const float gaugeWidth = vwidth / (((float)columns) + (((float)columns+1) * spacing + spacing * 0.5f));
pixhawk's avatar
pixhawk committed
448
    const QColor gaugeColor = QColor(200, 200, 200);
pixhawk's avatar
pixhawk committed
449
450
451
452
453
454
455
456
457
458
459
    //drawSystemIndicator(10.0f-gaugeWidth/2.0f, 20.0f, 10.0f, 40.0f, 15.0f, &painter);
    //drawGauge(15.0f, 15.0f, gaugeWidth/2.0f, 0, 1.0f, "thrust", values.value("thrust", 0.0f), gaugeColor, &painter, qMakePair(0.45f, 0.8f), qMakePair(0.8f, 1.0f), true);
    //drawGauge(15.0f+gaugeWidth*1.7f, 15.0f, gaugeWidth/2.0f, 0, 10.0f, "altitude", values.value("altitude", 0.0f), gaugeColor, &painter, qMakePair(1.0f, 2.5f), qMakePair(0.0f, 0.5f), true);

    // Left spacing from border / other gauges, measured from left edge to center
    float leftSpacing = gaugeWidth * spacing;
    float xCoord = leftSpacing + gaugeWidth/2.0f;

    float topSpacing = leftSpacing;
    float yCoord = topSpacing + gaugeWidth/2.0f;

460
461
    for (int i = 0; i < acceptList->size(); ++i)
    {
pixhawk's avatar
pixhawk committed
462
        QString value = acceptList->at(i);
463
464
        QString label = customNames.value(value);
        drawGauge(xCoord, yCoord, gaugeWidth/2.0f, minValues.value(value, -1.0f), maxValues.value(value, 1.0f), label, values.value(value, minValues.value(value, 0.0f)), gaugeColor, &painter, symmetric.value(value, false), goodRanges.value(value, qMakePair(0.0f, 0.5f)), critRanges.value(value, qMakePair(0.7f, 1.0f)), true);
pixhawk's avatar
pixhawk committed
465
466
        xCoord += gaugeWidth + leftSpacing;
        // Move one row down if necessary
467
468
        if (xCoord + gaugeWidth*0.9f > vwidth)
        {
pixhawk's avatar
pixhawk committed
469
470
471
472
            yCoord += topSpacing + gaugeWidth;
            xCoord = leftSpacing + gaugeWidth/2.0f;
        }
    }
pixhawk's avatar
pixhawk committed
473
474
475
476
477
478
479
480
}

/**
 *
 * @param uas the UAS/MAV to monitor/display with the HUD
 */
void HDDisplay::setActiveUAS(UASInterface* uas)
{
481
    // Disconnect any previously connected active UAS
482
    if (this->uas != NULL) {
483
        removeSource(this->uas);
pixhawk's avatar
pixhawk committed
484
485
486
    }

    // Now connect the new UAS
487
	addSource(uas);
pixhawk's avatar
pixhawk committed
488
    this->uas = uas;
pixhawk's avatar
pixhawk committed
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
}

/**
 * Rotate a polygon around a point
 *
 * @param p polygon to rotate
 * @param origin the rotation center
 * @param angle rotation angle, in radians
 * @return p Polygon p rotated by angle around the origin point
 */
void HDDisplay::rotatePolygonClockWiseRad(QPolygonF& p, float angle, QPointF origin)
{
    // Standard 2x2 rotation matrix, counter-clockwise
    //
    //   |  cos(phi)   sin(phi) |
    //   | -sin(phi)   cos(phi) |
    //
506
    for (int i = 0; i < p.size(); i++) {
pixhawk's avatar
pixhawk committed
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
        QPointF curr = p.at(i);

        const float x = curr.x();
        const float y = curr.y();

        curr.setX(((cos(angle) * (x-origin.x())) + (-sin(angle) * (y-origin.y()))) + origin.x());
        curr.setY(((sin(angle) * (x-origin.x())) + (cos(angle) * (y-origin.y()))) + origin.y());
        p.replace(i, curr);
    }
}

void HDDisplay::drawPolygon(QPolygonF refPolygon, QPainter* painter)
{
    // Scale coordinates
    QPolygonF draw(refPolygon.size());
522
    for (int i = 0; i < refPolygon.size(); i++) {
pixhawk's avatar
pixhawk committed
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
        QPointF curr;
        curr.setX(refToScreenX(refPolygon.at(i).x()));
        curr.setY(refToScreenY(refPolygon.at(i).y()));
        draw.replace(i, curr);
    }
    painter->drawPolygon(draw);
}

void HDDisplay::drawChangeRateStrip(float xRef, float yRef, float height, float minRate, float maxRate, float value, QPainter* painter)
{
    QBrush brush(defaultColor, Qt::NoBrush);
    painter->setBrush(brush);
    QPen rectPen(Qt::SolidLine);
    rectPen.setWidth(0);
    rectPen.setColor(defaultColor);
    painter->setPen(rectPen);

    float scaledValue = value;

    // Saturate value
    if (value > maxRate) scaledValue = maxRate;
    if (value < minRate) scaledValue = minRate;

    //           x (Origin: xRef, yRef)
    //           -
    //           |
    //           |
    //           |
    //           =
    //           |
    //   -0.005 >|
    //           |
    //           -

    const float width = height / 8.0f;
    const float lineWidth = 0.5f;

    // Indicator lines
    // Top horizontal line
    drawLine(xRef, yRef, xRef+width, yRef, lineWidth, defaultColor, painter);
    // Vertical main line
    drawLine(xRef+width/2.0f, yRef, xRef+width/2.0f, yRef+height, lineWidth, defaultColor, painter);
    // Zero mark
    drawLine(xRef, yRef+height/2.0f, xRef+width, yRef+height/2.0f, lineWidth, defaultColor, painter);
    // Horizontal bottom line
    drawLine(xRef, yRef+height, xRef+width, yRef+height, lineWidth, defaultColor, painter);

    // Text
    QString label;
    label.sprintf("< %06.2f", value);
    paintText(label, defaultColor, 3.0f, xRef+width/2.0f, yRef+height-((scaledValue - minRate)/(maxRate-minRate))*height - 1.6f, painter);
}

pixhawk's avatar
pixhawk committed
576
void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float max, QString name, float value, const QColor& color, QPainter* painter, bool symmetric, QPair<float, float> goodRange, QPair<float, float> criticalRange, bool solid)
pixhawk's avatar
pixhawk committed
577
578
579
580
581
{
    // Draw the circle
    QPen circlePen(Qt::SolidLine);

    // Rotate the whole gauge with this angle (in radians) for the zero position
pixhawk's avatar
pixhawk committed
582
    float zeroRotation;
583
    if (symmetric) {
pixhawk's avatar
pixhawk committed
584
        zeroRotation = 1.35f;
585
    } else {
pixhawk's avatar
pixhawk committed
586
587
        zeroRotation = 0.49f;
    }
pixhawk's avatar
pixhawk committed
588
589
590

    // Scale the rotation so that the gauge does one revolution
    // per max. change
pixhawk's avatar
pixhawk committed
591
    float rangeScale;
592
    if (symmetric) {
pixhawk's avatar
pixhawk committed
593
        rangeScale = ((2.0f * M_PI) / (max - min)) * 0.57f;
594
    } else {
pixhawk's avatar
pixhawk committed
595
596
        rangeScale = ((2.0f * M_PI) / (max - min)) * 0.72f;
    }
pixhawk's avatar
pixhawk committed
597

598
599
600
    const float scaledValue = (value-min)*rangeScale;

    float nameHeight = radius / 2.6f;
pixhawk's avatar
pixhawk committed
601
602
    paintText(name.toUpper(), color, nameHeight*0.7f, xRef-radius, yRef-radius, painter);

603
604
605
    // Ensure some space
    nameHeight *= 1.2f;

606
    if (!solid) {
pixhawk's avatar
pixhawk committed
607
608
609
610
        circlePen.setStyle(Qt::DotLine);
    }
    circlePen.setWidth(refLineWidthToPen(radius/12.0f));
    circlePen.setColor(color);
611

612
    if (symmetric) {
613
614
        circlePen.setStyle(Qt::DashLine);
    }
pixhawk's avatar
pixhawk committed
615
616
    painter->setBrush(Qt::NoBrush);
    painter->setPen(circlePen);
617
618
    drawCircle(xRef, yRef+nameHeight, radius, 0.0f, color, painter);
    //drawCircle(xRef, yRef+nameHeight, radius, 0.0f, 170.0f, 1.0f, color, painter);
pixhawk's avatar
pixhawk committed
619
620

    QString label;
621
622

    // Show integer values without decimal places
623
    if (intValues.contains(name)) {
624
        label.sprintf("% 05d", (int)value);
625
    } else {
626
627
        label.sprintf("% 06.1f", value);
    }
pixhawk's avatar
pixhawk committed
628
629
630
631


    // Text
    // height
632
633
    const float textHeight = radius/2.1f;
    const float textX = xRef-radius/3.0f;
pixhawk's avatar
pixhawk committed
634
635
636
    const float textY = yRef+radius/2.0f;

    // Draw background rectangle
637
    QBrush brush(QGC::colorBackground, Qt::SolidPattern);
pixhawk's avatar
pixhawk committed
638
639
    painter->setBrush(brush);
    painter->setPen(Qt::NoPen);
pixhawk's avatar
pixhawk committed
640

641
    if (symmetric) {
pixhawk's avatar
pixhawk committed
642
        painter->drawRect(refToScreenX(xRef-radius), refToScreenY(yRef+nameHeight+radius/4.0f), refToScreenX(radius+radius), refToScreenY((radius - radius/4.0f)*1.2f));
643
    } else {
pixhawk's avatar
pixhawk committed
644
645
        painter->drawRect(refToScreenX(xRef-radius/2.5f), refToScreenY(yRef+nameHeight+radius/4.0f), refToScreenX(radius+radius/2.0f), refToScreenY((radius - radius/4.0f)*1.2f));
    }
pixhawk's avatar
pixhawk committed
646
647

    // Draw good value and crit. value markers
648
    if (goodRange.first != goodRange.second) {
pixhawk's avatar
pixhawk committed
649
650
651
652
653
654
655
        QRectF rectangle(refToScreenX(xRef-radius/2.0f), refToScreenY(yRef+nameHeight-radius/2.0f), refToScreenX(radius*2.0f), refToScreenX(radius*2.0f));
        painter->setPen(Qt::green);
        //int start = ((goodRange.first*rangeScale+zeroRotation)/M_PI)*180.0f * 16.0f;// + 16.0f * 60.0f;
        //int span  = start - ((goodRange.second*rangeScale+zeroRotation)/M_PI)*180.0f * 16.0f;
        //painter->drawArc(rectangle, start, span);
    }

656
    if (criticalRange.first != criticalRange.second) {
pixhawk's avatar
pixhawk committed
657
658
659
660
661
662
663
664
665
        QRectF rectangle(refToScreenX(xRef-radius/2.0f-3.0f), refToScreenY(yRef+nameHeight-radius/2.0f-3.0f), refToScreenX(radius*2.0f), refToScreenX(radius*2.0f));
        painter->setPen(Qt::yellow);
        //int start = ((criticalRange.first*rangeScale+zeroRotation)/M_PI)*180.0f * 16.0f - 180.0f*16.0f;// + 16.0f * 60.0f;
        //int span  = start - ((criticalRange.second*rangeScale+zeroRotation)/M_PI)*180.0f * 16.0f + 180.0f*16.0f;
        //painter->drawArc(rectangle, start, span);
    }

    // Draw the value
    //painter->setPen(textColor);
666
    paintText(label, QGC::colorCyan, textHeight, textX, textY+nameHeight, painter);
pixhawk's avatar
pixhawk committed
667
668
669
670
671
672
673
674
675
676
    //paintText(label, color, ((radius - radius/3.0f)*1.1f), xRef-radius/2.5f, yRef+radius/3.0f, painter);

    // Draw the needle

    const float maxWidth = radius / 6.0f;
    const float minWidth = maxWidth * 0.3f;

    QPolygonF p(6);

    p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef+nameHeight+radius * 0.05f));
677
678
    p.replace(1, QPointF(xRef-minWidth/2.0f, yRef+nameHeight+radius * 0.89f));
    p.replace(2, QPointF(xRef+minWidth/2.0f, yRef+nameHeight+radius * 0.89f));
pixhawk's avatar
pixhawk committed
679
680
681
682
683
    p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef+nameHeight+radius * 0.05f));
    p.replace(4, QPointF(xRef,               yRef+nameHeight+radius * 0.0f));
    p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef+nameHeight+radius * 0.05f));


684
    rotatePolygonClockWiseRad(p, scaledValue+zeroRotation, QPointF(xRef, yRef+nameHeight));
pixhawk's avatar
pixhawk committed
685
686
687
688
689
690
691
692
693
694
695
696

    QBrush indexBrush;
    indexBrush.setColor(color);
    indexBrush.setStyle(Qt::SolidPattern);
    painter->setPen(Qt::NoPen);
    painter->setBrush(indexBrush);
    drawPolygon(p, painter);
}


void HDDisplay::drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter)
{
697
    if (values.size() > 0) {
pixhawk's avatar
pixhawk committed
698
699
700
701
702
703
        QString selectedKey = values.begin().key();
        //   | | | | | |
        //   | | | | | |
        //   x speed: 2.54

        // One column per value
704
        QMapIterator<QString, double> value(values);
pixhawk's avatar
pixhawk committed
705
706
707
708
709
710
711
712
713
714

        float x = xRef;
        float y = yRef;

        const float vspacing = 1.0f;
        float width = 1.5f;
        float height = 1.5f;
        const float hspacing = 0.6f;

        int i = 0;
715
        while (value.hasNext() && i < maxNum && x < maxWidth && y < maxHeight) {
pixhawk's avatar
pixhawk committed
716
717
718
719
            value.next();
            QBrush brush(Qt::SolidPattern);


720
            if (value.value() < 0.01f && value.value() > -0.01f) {
pixhawk's avatar
pixhawk committed
721
                brush.setColor(Qt::gray);
722
            } else if (value.value() > 0.01f) {
pixhawk's avatar
pixhawk committed
723
                brush.setColor(Qt::blue);
724
            } else {
pixhawk's avatar
pixhawk committed
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
                brush.setColor(Qt::yellow);
            }

            painter->setBrush(brush);
            painter->setPen(Qt::NoPen);

            // Draw current value colormap
            painter->drawRect(refToScreenX(x), refToScreenY(y), refToScreenX(width), refToScreenY(height));

            // Draw change rate colormap
            painter->drawRect(refToScreenX(x), refToScreenY(y+height+hspacing), refToScreenX(width), refToScreenY(height));

            // Draw mean value colormap
            painter->drawRect(refToScreenX(x), refToScreenY(y+2.0f*(height+hspacing)), refToScreenX(width), refToScreenY(height));

            // Add spacing
            x += width+vspacing;

            // Iterate
            i++;
        }

        // Draw detail label
        QString detail = "NO DATA AVAILABLE";

750
        if (values.contains(selectedKey)) {
pixhawk's avatar
pixhawk committed
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
            detail = values.find(selectedKey).key();
            detail.append(": ");
            detail.append(QString::number(values.find(selectedKey).value()));
        }
        paintText(detail, QColor(255, 255, 255), 3.0f, xRef, yRef+3.0f*(height+hspacing)+1.0f, painter);
    }
}

void HDDisplay::drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid)
{
    // Draw the circle
    QPen circlePen(Qt::SolidLine);
    if (!solid) circlePen.setStyle(Qt::DotLine);
    circlePen.setWidth(refLineWidthToPen(0.5f));
    circlePen.setColor(defaultColor);
    painter->setBrush(Qt::NoBrush);
    painter->setPen(circlePen);
768
769
    drawCircle(xRef, yRef, radius, 200.0f, color, painter);
    //drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.0f, color, painter);
pixhawk's avatar
pixhawk committed
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818

    QString label;
    label.sprintf("%05.1f", value);

    // Draw the value
    paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter);

    // Draw the needle
    // Scale the rotation so that the gauge does one revolution
    // per max. change
    const float rangeScale = (2.0f * M_PI) / expectedMaxChange;
    const float maxWidth = radius / 10.0f;
    const float minWidth = maxWidth * 0.3f;

    QPolygonF p(6);

    p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f));
    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.5f));
    p.replace(4, QPointF(xRef,               yRef-radius * 0.46f));
    p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f));

    rotatePolygonClockWiseRad(p, value*rangeScale, QPointF(xRef, yRef));

    QBrush indexBrush;
    indexBrush.setColor(defaultColor);
    indexBrush.setStyle(Qt::SolidPattern);
    painter->setPen(Qt::SolidLine);
    painter->setPen(defaultColor);
    painter->setBrush(indexBrush);
    drawPolygon(p, painter);
}

/**
 * Paint text on top of the image and OpenGL drawings
 *
 * @param text chars to write
 * @param color text color
 * @param fontSize text size in mm
 * @param refX position in reference units (mm of the real instrument). This is relative to the measurement unit position, NOT in pixels.
 * @param refY position in reference units (mm of the real instrument). This is relative to the measurement unit position, NOT in pixels.
 */
void HDDisplay::paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter)
{
    QPen prevPen = painter->pen();
    float pPositionX = refToScreenX(refX) - (fontSize*scalingFactor*0.072f);
    float pPositionY = refToScreenY(refY) - (fontSize*scalingFactor*0.212f);

819
820
821
822
    QFont font("Bitstream Vera Sans");
    // Enforce minimum font size of 5 pixels
    int fSize = qMax(5, (int)(fontSize*scalingFactor*1.26f));
    font.setPixelSize(fSize);
pixhawk's avatar
pixhawk committed
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841

    QFontMetrics metrics = QFontMetrics(font);
    int border = qMax(4, metrics.leading());
    QRect rect = metrics.boundingRect(0, 0, width() - 2*border, int(height()*0.125),
                                      Qt::AlignLeft | Qt::TextWordWrap, text);
    painter->setPen(color);
    painter->setFont(font);
    painter->setRenderHint(QPainter::TextAntialiasing);
    painter->drawText(pPositionX, pPositionY,
                      rect.width(), rect.height(),
                      Qt::AlignCenter | Qt::TextWordWrap, text);
    painter->setPen(prevPen);
}

float HDDisplay::refLineWidthToPen(float line)
{
    return line * 2.50f;
}

842
// Connect a generic source
843
844
845
846
847
848
void HDDisplay::addSource(QObject* obj)
{
    //genericSources.append(obj);
    // FIXME XXX HACK
//    if (plots.size() > 0)
//    {
849
850
851
852
853
854
        connect(obj, SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), this, SLOT(updateValue(int,QString,QString,qint8,quint64)));
        connect(obj, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), this, SLOT(updateValue(int,QString,QString,quint8,quint64)));
        connect(obj, SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), this, SLOT(updateValue(int,QString,QString,qint16,quint64)));
        connect(obj, SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), this, SLOT(updateValue(int,QString,QString,quint16,quint64)));
        connect(obj, SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), this, SLOT(updateValue(int,QString,QString,qint32,quint64)));
        connect(obj, SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), this, SLOT(updateValue(int,QString,QString,quint32,quint64)));
855
856
857
858
859
860
        connect(obj, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), this, SLOT(updateValue(int,QString,QString,quint64,quint64)));
        connect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), this, SLOT(updateValue(int,QString,QString,qint64,quint64)));
        connect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64)));
//    }
}

861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
// Disconnect a generic source
void HDDisplay::removeSource(QObject* obj)
{
    //genericSources.append(obj);
    // FIXME XXX HACK
//    if (plots.size() > 0)
//    {
        disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), this, SLOT(updateValue(int,QString,QString,qint8,quint64)));
        disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), this, SLOT(updateValue(int,QString,QString,quint8,quint64)));
        disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), this, SLOT(updateValue(int,QString,QString,qint16,quint64)));
        disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), this, SLOT(updateValue(int,QString,QString,quint16,quint64)));
        disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), this, SLOT(updateValue(int,QString,QString,qint32,quint64)));
        disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), this, SLOT(updateValue(int,QString,QString,quint32,quint64)));
        disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), this, SLOT(updateValue(int,QString,QString,quint64,quint64)));
        disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), this, SLOT(updateValue(int,QString,QString,qint64,quint64)));
        disconnect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64)));
//    }
}

void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint8 value, const quint64 msec)
{
    if (!intValues.contains(name)) intValues.insert(name, true);
    updateValue(uasId, name, unit, (double)value, msec);
}

void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint8 value, const quint64 msec)
{
    if (!intValues.contains(name)) intValues.insert(name, true);
    updateValue(uasId, name, unit, (double)value, msec);
}

void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint16 value, const quint64 msec)
{
    if (!intValues.contains(name)) intValues.insert(name, true);
    updateValue(uasId, name, unit, (double)value, msec);
}

void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint16 value, const quint64 msec)
{
    if (!intValues.contains(name)) intValues.insert(name, true);
    updateValue(uasId, name, unit, (double)value, msec);
}

void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint32 value, const quint64 msec)
905
906
907
908
909
{
    if (!intValues.contains(name)) intValues.insert(name, true);
    updateValue(uasId, name, unit, (double)value, msec);
}

910
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint32 value, const quint64 msec)
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
{
    if (!intValues.contains(name)) intValues.insert(name, true);
    updateValue(uasId, name, unit, (double)value, msec);
}

void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint64 value, const quint64 msec)
{
    if (!intValues.contains(name)) intValues.insert(name, true);
    updateValue(uasId, name, unit, (double)value, msec);
}

void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint64 value, const quint64 msec)
{
    if (!intValues.contains(name)) intValues.insert(name, true);
    updateValue(uasId, name, unit, (double)value, msec);
}

lm's avatar
lm committed
928
void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec)
pixhawk's avatar
pixhawk committed
929
{
930
    Q_UNUSED(uasId);
931
    Q_UNUSED(unit);
932
933
934
935
936
937
    // Update mean
    const float oldMean = valuesMean.value(name, 0.0f);
    const int meanCount = valuesCount.value(name, 0);
    valuesMean.insert(name, (oldMean * meanCount +  value) / (meanCount + 1));
    valuesCount.insert(name, meanCount + 1);
    valuesDot.insert(name, (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f));
938
    if (values.value(name, 0.0) != value) valuesChanged = true;
939
    values.insert(name, value);
940
    units.insert(name, unit);
941
    lastUpdate.insert(name, msec);
pixhawk's avatar
pixhawk committed
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
}

/**
 * @param y coordinate in pixels to be converted to reference mm units
 * @return the screen coordinate relative to the QGLWindow origin
 */
float HDDisplay::refToScreenX(float x)
{
    return (scalingFactor * x);
}

/**
 * @param x coordinate in pixels to be converted to reference mm units
 * @return the screen coordinate relative to the QGLWindow origin
 */
float HDDisplay::refToScreenY(float y)
{
    return (scalingFactor * y);
}

962
963
964
965
966
967
968
969
970
971
float HDDisplay::screenToRefX(float x)
{
    return x/scalingFactor;
}

float HDDisplay::screenToRefY(float y)
{
    return y/scalingFactor;
}

pixhawk's avatar
pixhawk committed
972
973
974
975
976
977
978
979
980
void HDDisplay::drawLine(float refX1, float refY1, float refX2, float refY2, float width, const QColor& color, QPainter* painter)
{
    QPen pen(Qt::SolidLine);
    pen.setWidth(refLineWidthToPen(width));
    pen.setColor(color);
    painter->setPen(pen);
    painter->drawLine(QPoint(refToScreenX(refX1), refToScreenY(refY1)), QPoint(refToScreenX(refX2), refToScreenY(refY2)));
}

981
void HDDisplay::drawEllipse(float refX, float refY, float radiusX, float radiusY, float lineWidth, const QColor& color, QPainter* painter)
pixhawk's avatar
pixhawk committed
982
983
984
985
986
987
988
989
{
    QPen pen(painter->pen().style());
    pen.setWidth(refLineWidthToPen(lineWidth));
    pen.setColor(color);
    painter->setPen(pen);
    painter->drawEllipse(QPointF(refToScreenX(refX), refToScreenY(refY)), refToScreenX(radiusX), refToScreenY(radiusY));
}

990
void HDDisplay::drawCircle(float refX, float refY, float radius, float lineWidth, const QColor& color, QPainter* painter)
pixhawk's avatar
pixhawk committed
991
{
992
    drawEllipse(refX, refY, radius, radius, lineWidth, color, painter);
pixhawk's avatar
pixhawk committed
993
994
995
996
997
998
999
1000
}

void HDDisplay::changeEvent(QEvent *e)
{
    QWidget::changeEvent(e);
    switch (e->type()) {
    case QEvent::LanguageChange:
        m_ui->retranslateUi(this);
For faster browsing, not all history is shown. View entire blame