UASView.cc 21.8 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2 3 4 5 6 7 8 9 10 11
/*=====================================================================
PIXHAWK Micro Air Vehicle Flying Robotics Toolkit

(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
12

pixhawk's avatar
pixhawk committed
13 14 15 16
    PIXHAWK is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
17

pixhawk's avatar
pixhawk committed
18 19
    You should have received a copy of the GNU General Public License
    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
20

pixhawk's avatar
pixhawk committed
21 22 23 24
======================================================================*/

/**
 * @file
pixhawk's avatar
pixhawk committed
25
 *   @brief Implementation of one airstrip
pixhawk's avatar
pixhawk committed
26 27 28 29 30 31 32 33
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <cmath>
#include <QDateTime>
#include <QDebug>
34
#include <QMenu>
35
#include <QInputDialog>
pixhawk's avatar
pixhawk committed
36

37
#include "QGC.h"
pixhawk's avatar
pixhawk committed
38 39
#include "UASManager.h"
#include "UASView.h"
40
#include "UASWaypointManager.h"
41
#include "MainWindow.h"
pixhawk's avatar
pixhawk committed
42 43 44
#include "ui_UASView.h"

UASView::UASView(UASInterface* uas, QWidget *parent) :
45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66
        QWidget(parent),
        startTime(0),
        timeout(false),
        iconIsRed(true),
        timeRemaining(0),
        chargeLevel(0),
        uas(uas),
        load(0),
        state("UNKNOWN"),
        stateDesc(tr("Unknown state")),
        mode("MAV_MODE_UNKNOWN"),
        thrust(0),
        isActive(false),
        x(0),
        y(0),
        z(0),
        totalSpeed(0),
        lat(0),
        lon(0),
        alt(0),
        groundDistance(0),
        localFrame(false),
67
        globalFrameKnown(false),
68 69 70
        removeAction(new QAction("Delete this system", this)),
        renameAction(new QAction("Rename..", this)),
        selectAction(new QAction("Control this system", this )),
lm's avatar
lm committed
71
        hilAction(new QAction("Enable Hardware-in-the-Loop Simulation", this )),
72 73
        selectAirframeAction(new QAction("Choose Airframe", this)),
        setBatterySpecsAction(new QAction("Set Battery Options", this)),
74
        lowPowerModeEnabled(true),
75 76
        generalUpdateCount(0),
        filterTime(0),
77 78 79 80 81
        m_ui(new Ui::UASView)
{
    // FIXME XXX
    lowPowerModeEnabled = MainWindow::instance()->lowPowerModeEnabled();

lm's avatar
lm committed
82
    hilAction->setCheckable(true);
83

pixhawk's avatar
pixhawk committed
84
    m_ui->setupUi(this);
85

pixhawk's avatar
pixhawk committed
86
    // Setup communication
pixhawk's avatar
pixhawk committed
87
    //connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(receiveValue(int,QString,double,quint64)));
pixhawk's avatar
pixhawk committed
88 89 90 91 92 93 94
    connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int)));
    connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
    connect(uas, SIGNAL(thrustChanged(UASInterface*, double)), this, SLOT(updateThrust(UASInterface*, double)));
    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(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
    connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString,QString)));
pixhawk's avatar
pixhawk committed
95
    connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
pixhawk's avatar
pixhawk committed
96
    connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double)));
97
    connect(uas, SIGNAL(heartbeatTimeout()), this, SLOT(heartbeatTimeout()));
pixhawk's avatar
pixhawk committed
98
    connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int)));
pixhawk's avatar
pixhawk committed
99
    connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16)));
pixhawk's avatar
pixhawk committed
100
    connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
101
    connect(UASManager::instance(), SIGNAL(activeUASStatusChanged(UASInterface*,bool)), this, SLOT(updateActiveUAS(UASInterface*,bool)));
102 103 104
    connect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(showStatusText(int, int, int, QString)));
    connect(uas, SIGNAL(navModeChanged(int, int, QString)), this, SLOT(updateNavMode(int, int, QString)));

pixhawk's avatar
pixhawk committed
105
    // Setup UAS selection
106
    connect(m_ui->uasViewFrame, SIGNAL(clicked(bool)), this, SLOT(setUASasActive(bool)));
107

pixhawk's avatar
pixhawk committed
108 109 110 111
    // Setup user interaction
    connect(m_ui->liftoffButton, SIGNAL(clicked()), uas, SLOT(launch()));
    connect(m_ui->haltButton, SIGNAL(clicked()), uas, SLOT(halt()));
    connect(m_ui->continueButton, SIGNAL(clicked()), uas, SLOT(go()));
pixhawk's avatar
pixhawk committed
112 113
    connect(m_ui->landButton, SIGNAL(clicked()), uas, SLOT(home()));
    connect(m_ui->abortButton, SIGNAL(clicked()), uas, SLOT(emergencySTOP()));
pixhawk's avatar
pixhawk committed
114 115
    connect(m_ui->killButton, SIGNAL(clicked()), uas, SLOT(emergencyKILL()));
    connect(m_ui->shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
116 117 118

    // Allow to delete this widget
    connect(removeAction, SIGNAL(triggered()), this, SLOT(deleteLater()));
119
    connect(renameAction, SIGNAL(triggered()), this, SLOT(rename()));
120
    connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected()));
lm's avatar
lm committed
121
    connect(hilAction, SIGNAL(triggered(bool)), uas, SLOT(enableHil(bool)));
122
    connect(selectAirframeAction, SIGNAL(triggered()), this, SLOT(selectAirframe()));
123
    connect(setBatterySpecsAction, SIGNAL(triggered()), this, SLOT(setBatterySpecs()));
124
    connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater()));
125 126 127

    // Name changes
    connect(uas, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString)));
128

pixhawk's avatar
pixhawk committed
129
    // Set static values
130

pixhawk's avatar
pixhawk committed
131
    // Name
132 133
    if (uas->getUASName() == "")
    {
pixhawk's avatar
pixhawk committed
134
        m_ui->nameLabel->setText(tr("UAS") + QString::number(uas->getUASID()));
135 136 137
    }
    else
    {
pixhawk's avatar
pixhawk committed
138 139
        m_ui->nameLabel->setText(uas->getUASName());
    }
140

141
    setBackgroundColor();
142

pixhawk's avatar
pixhawk committed
143 144 145
    // Heartbeat fade
    refreshTimer = new QTimer(this);
    connect(refreshTimer, SIGNAL(timeout()), this, SLOT(refresh()));
146 147
    if (lowPowerModeEnabled)
    {
148
        refreshTimer->start(updateInterval*3);
149 150 151
    }
    else
    {
152 153
        refreshTimer->start(updateInterval);
    }
154 155 156 157 158

    // Hide kill and shutdown buttons per default
    m_ui->killButton->hide();
    m_ui->shutdownButton->hide();

159 160 161
    // Set state and mode
    updateMode(uas->getUASID(), uas->getShortMode(), "");
    updateState(uas, uas->getShortState(), "");
162
    setSystemType(uas, uas->getSystemType());
pixhawk's avatar
pixhawk committed
163 164 165 166 167
}

UASView::~UASView()
{
    delete m_ui;
168 169 170
    delete removeAction;
    delete renameAction;
    delete selectAction;
pixhawk's avatar
pixhawk committed
171 172
}

173 174 175 176
void UASView::heartbeatTimeout()
{
    timeout = true;
}
177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192

void UASView::updateNavMode(int uasid, int mode, const QString& text)
{
    Q_UNUSED(uasid);
    Q_UNUSED(mode);
    m_ui->navLabel->setText(text);
}

void UASView::showStatusText(int uasid, int componentid, int severity, QString text)
{
    Q_UNUSED(uasid);
    Q_UNUSED(componentid);
    Q_UNUSED(severity);
    //m_ui->statusTextLabel->setText(text);
    stateDesc = text;
}
193

194 195 196 197 198 199 200 201 202 203
/**
 * Set the background color based on the MAV color. If the MAV is selected as the
 * currently actively controlled system, the frame color is highlighted
 */
void UASView::setBackgroundColor()
{
    // UAS color
    QColor uasColor = uas->getColor();
    QString colorstyle;
    QString borderColor = "#4A4A4F";
204 205
    if (isActive)
    {
206 207
        borderColor = "#FA4A4F";
        uasColor = uasColor.darker(475);
208 209 210
    }
    else
    {
211 212
        uasColor = uasColor.darker(675);
    }
213
    colorstyle = colorstyle.sprintf("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 2px solid %s; }",
214 215 216 217 218
                                    uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str());
    m_ui->uasViewFrame->setStyleSheet(colorstyle);
}

void UASView::setUASasActive(bool active)
pixhawk's avatar
pixhawk committed
219
{
220 221
    if (active)
    {
222 223 224 225 226 227
        UASManager::instance()->setActiveUAS(this->uas);
    }
}

void UASView::updateActiveUAS(UASInterface* uas, bool active)
{
228 229
    if (uas == this->uas)
    {
230 231 232
        this->isActive = active;
        setBackgroundColor();
    }
pixhawk's avatar
pixhawk committed
233 234
}

pixhawk's avatar
pixhawk committed
235 236
void UASView::updateMode(int sysId, QString status, QString description)
{
237
    Q_UNUSED(description);
238 239

    //int aa=this->uas->getUASID();
pixhawk's avatar
pixhawk committed
240
    if (sysId == this->uas->getUASID()) m_ui->modeLabel->setText(status);
241 242

    m_ui->modeLabel->setText(status);
pixhawk's avatar
pixhawk committed
243 244
}

pixhawk's avatar
pixhawk committed
245 246
void UASView::mouseDoubleClickEvent (QMouseEvent * event)
{
247
    Q_UNUSED(event);
pixhawk's avatar
pixhawk committed
248
    UASManager::instance()->setActiveUAS(uas);
249
    // qDebug() << __FILE__ << __LINE__ << "DOUBLECLICKED";
pixhawk's avatar
pixhawk committed
250 251 252 253
}

void UASView::enterEvent(QEvent* event)
{
254 255
    if (event->type() == QEvent::MouseMove)
    {
256
        emit uasInFocus(uas);
257 258
        if (uas != UASManager::instance()->getActiveUAS())
        {
259 260 261
            grabMouse(QCursor(Qt::PointingHandCursor));
        }
    }
262

263 264
    if (event->type() == QEvent::MouseButtonDblClick)
    {
265
        // qDebug() << __FILE__ << __LINE__ << "UAS CLICKED!";
266
    }
pixhawk's avatar
pixhawk committed
267 268 269 270
}

void UASView::leaveEvent(QEvent* event)
{
271 272
    if (event->type() == QEvent::MouseMove)
    {
273 274 275 276 277 278 279 280 281
        emit uasOutFocus(uas);
        releaseMouse();
    }
}

void UASView::showEvent(QShowEvent* event)
{
    // React only to internal (pre-display)
    // events
282
    Q_UNUSED(event);
283
    refreshTimer->start(updateInterval*10);
284 285 286 287 288 289 290 291
}

void UASView::hideEvent(QHideEvent* event)
{
    // React only to internal (pre-display)
    // events
    Q_UNUSED(event);
    refreshTimer->stop();
pixhawk's avatar
pixhawk committed
292 293 294 295
}

void UASView::receiveHeartbeat(UASInterface* uas)
{
296
    Q_UNUSED(uas);
297
    heartbeatColor = QColor(20, 200, 20);
lm's avatar
lm committed
298
    QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 0px; border: 0px; background-color: %1; }");
299
    m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name()));
300 301 302 303 304 305
    if (timeout) setBackgroundColor();
    timeout = false;
}

void UASView::updateName(const QString& name)
{
306
    if (uas) m_ui->nameLabel->setText(name);
pixhawk's avatar
pixhawk committed
307 308 309 310 311 312 313 314 315 316 317
}

/**
 * The current system type is represented through the system icon.
 *
 * @param uas Source system, has to be the same as this->uas
 * @param systemType type ID, following the MAVLink system type conventions
 * @see http://pixhawk.ethz.ch/software/mavlink
 */
void UASView::setSystemType(UASInterface* uas, unsigned int systemType)
{
318 319
    if (uas == this->uas)
    {
pixhawk's avatar
pixhawk committed
320
        // Set matching icon
321 322
        switch (systemType)
        {
pixhawk's avatar
pixhawk committed
323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338
        case 0:
            m_ui->typeButton->setIcon(QIcon(":/images/mavs/generic.svg"));
            break;
        case 1:
            m_ui->typeButton->setIcon(QIcon(":/images/mavs/fixed-wing.svg"));
            break;
        case 2:
            m_ui->typeButton->setIcon(QIcon(":/images/mavs/quadrotor.svg"));
            break;
        case 3:
            m_ui->typeButton->setIcon(QIcon(":/images/mavs/coaxial.svg"));
            break;
        case 4:
            m_ui->typeButton->setIcon(QIcon(":/images/mavs/helicopter.svg"));
            break;
        case 5:
lm's avatar
lm committed
339 340
            m_ui->typeButton->setIcon(QIcon(":/images/mavs/unknown.svg"));
            break;
341
        case 6: {
342 343
                // A groundstation is a special system type, update widget
                QString result;
lm's avatar
lm committed
344
                m_ui->nameLabel->setText(tr("GCS ") + result.sprintf("%03d", uas->getUASID()));
345 346 347 348 349 350 351 352 353 354 355 356 357 358 359
                m_ui->waypointLabel->setText("");
                m_ui->timeRemainingLabel->setText("Online:");
                m_ui->batteryBar->hide();
                m_ui->thrustBar->hide();
                m_ui->stateLabel->hide();
                m_ui->statusTextLabel->hide();
                m_ui->waypointLabel->hide();
                m_ui->liftoffButton->hide();
                m_ui->haltButton->hide();
                m_ui->landButton->hide();
                m_ui->shutdownButton->hide();
                m_ui->abortButton->hide();
                m_ui->typeButton->setIcon(QIcon(":/images/mavs/groundstation.svg"));
            }
            break;
pixhawk's avatar
pixhawk committed
360 361 362 363 364 365 366 367 368
        default:
            m_ui->typeButton->setIcon(QIcon(":/images/mavs/unknown.svg"));
            break;
        }
    }
}

void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec)
{
369
    Q_UNUSED(usec);
370 371 372 373
    Q_UNUSED(uas);
    this->x = x;
    this->y = y;
    this->z = z;
374
    localFrame = true;
pixhawk's avatar
pixhawk committed
375 376
}

377
void UASView::updateGlobalPosition(UASInterface* uas, double lon, double lat, double alt, quint64 usec)
pixhawk's avatar
pixhawk committed
378
{
379 380
    Q_UNUSED(uas);
    Q_UNUSED(usec);
pixhawk's avatar
pixhawk committed
381 382 383
    this->lon = lon;
    this->lat = lat;
    this->alt = alt;
384
    globalFrameKnown = true;
pixhawk's avatar
pixhawk committed
385 386 387 388
}

void UASView::updateSpeed(UASInterface*, double x, double y, double z, quint64 usec)
{
389
    Q_UNUSED(usec);
390
    totalSpeed = sqrt(x*x + y*y + z*z);
pixhawk's avatar
pixhawk committed
391 392
}

393 394 395 396 397
void UASView::currentWaypointUpdated(quint16 waypoint)
{
    m_ui->waypointLabel->setText(tr("WP") + QString::number(waypoint));
}

pixhawk's avatar
pixhawk committed
398 399
void UASView::setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current)
{
400 401 402 403 404
    Q_UNUSED(x);
    Q_UNUSED(y);
    Q_UNUSED(z);
    Q_UNUSED(yaw);
    Q_UNUSED(autocontinue);
405 406 407 408
    if (uasId == this->uas->getUASID())
    {
        if (current)
        {
pixhawk's avatar
pixhawk committed
409 410 411 412 413 414 415
            m_ui->waypointLabel->setText(tr("WP") + QString::number(id));
        }
    }
}

void UASView::selectWaypoint(int uasId, int id)
{
416 417
    if (uasId == this->uas->getUASID())
    {
pixhawk's avatar
pixhawk committed
418 419 420 421 422 423
        m_ui->waypointLabel->setText(tr("WP") + QString::number(id));
    }
}

void UASView::updateThrust(UASInterface* uas, double thrust)
{
424 425
    if (this->uas == uas)
    {
pixhawk's avatar
pixhawk committed
426
        this->thrust = thrust;
pixhawk's avatar
pixhawk committed
427 428 429 430 431
    }
}

void UASView::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
{
432
    Q_UNUSED(voltage);
433 434
    if (this->uas == uas)
    {
pixhawk's avatar
pixhawk committed
435 436 437 438 439 440 441
        timeRemaining = seconds;
        chargeLevel = percent;
    }
}

void UASView::updateState(UASInterface* uas, QString uasState, QString stateDescription)
{
442 443
    if (this->uas == uas)
    {
pixhawk's avatar
pixhawk committed
444 445 446 447 448 449 450
        state = uasState;
        stateDesc = stateDescription;
    }
}

void UASView::updateLoad(UASInterface* uas, double load)
{
451 452
    if (this->uas == uas)
    {
pixhawk's avatar
pixhawk committed
453 454 455 456
        this->load = load;
    }
}

457 458
void UASView::contextMenuEvent (QContextMenuEvent* event)
{
459
    QMenu menu(this);
lm's avatar
lm committed
460 461
    menu.addAction(selectAction);
    menu.addSeparator();
462
    menu.addAction(renameAction);
463 464
    if (timeout)
    {
465
        menu.addAction(removeAction);
466
    }
lm's avatar
lm committed
467
    menu.addAction(hilAction);
468
    menu.addAction(selectAirframeAction);
469
    menu.addAction(setBatterySpecsAction);
470 471 472
    menu.exec(event->globalPos());
}

473 474
void UASView::setBatterySpecs()
{
475 476
    if (uas)
    {
477 478
        bool ok;
        QString newName = QInputDialog::getText(this, tr("Set Battery Specifications for %1").arg(uas->getUASName()),
479
                                                tr("Specs: (empty,warn,full), e.g. (9V,9.5V,12.6V) or just warn level in percent (e.g. 15%) to use estimate from MAV"), QLineEdit::Normal,
480 481 482 483 484 485
                                                uas->getBatterySpecs(), &ok);

        if (ok && !newName.isEmpty()) uas->setBatterySpecs(newName);
    }
}

486 487
void UASView::rename()
{
488 489
    if (uas)
    {
490 491 492 493 494 495
        bool ok;
        QString newName = QInputDialog::getText(this, tr("Rename System %1").arg(uas->getUASName()),
                                                tr("System Name:"), QLineEdit::Normal,
                                                uas->getUASName(), &ok);

        if (ok && !newName.isEmpty()) uas->setUASName(newName);
496 497 498
    }
}

499 500
void UASView::selectAirframe()
{
501 502
    if (uas)
    {
503 504 505
        // Get list of airframes from UAS
        QStringList airframes;
        airframes << "Generic"
506 507 508 509 510 511 512 513
                << "Multiplex Easystar"
                << "Multiplex Twinstar"
                << "Multiplex Merlin"
                << "Pixhawk Cheetah"
                << "Mikrokopter"
                << "Reaper"
                << "Predator"
                << "Coaxial"
514 515
                << "Pteryx"
                << "Asctec Firefly";
516 517 518 519

        bool ok;
        QString item = QInputDialog::getItem(this, tr("Select Airframe for %1").arg(uas->getUASName()),
                                             tr("Airframe"), airframes, uas->getAirframe(), false, &ok);
520 521
        if (ok && !item.isEmpty())
        {
522 523 524 525 526 527
            // Set this airframe as UAS airframe
            uas->setAirframe(airframes.indexOf(item));
        }
    }
}

pixhawk's avatar
pixhawk committed
528 529
void UASView::refresh()
{
530
    //setUpdatesEnabled(false);
531 532
    //setUpdatesEnabled(true);
    //repaint();
533
    //qDebug() << "UPDATING UAS WIDGET!" << uas->getUASName();
534 535


536 537 538
    quint64 lastupdate = 0;
    //// qDebug() << "UASVIEW update diff: " << MG::TIME::getGroundTimeNow() - lastupdate;
    lastupdate = QGC::groundTimeMilliseconds();
539

540 541
    if (generalUpdateCount == 4)
    {
542
#if (QGC_EVENTLOOP_DEBUG)
543
        // qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
544
#endif
545
        generalUpdateCount = 0;
546
        //// qDebug() << "UPDATING EVERYTHING";
547 548 549 550 551 552 553 554 555 556
        // State
        m_ui->stateLabel->setText(state);
        m_ui->statusTextLabel->setText(stateDesc);

        // Battery
        m_ui->batteryBar->setValue(static_cast<int>(this->chargeLevel));
        //m_ui->loadBar->setValue(static_cast<int>(this->load));
        m_ui->thrustBar->setValue(this->thrust);

        // Position
557 558 559 560 561 562 563
        // If global position is known, prefer it over local coordinates

        if (!globalFrameKnown && localFrame)
        {
            QString position;
            position = position.sprintf("%05.1f %05.1f %06.1f m", x, y, z);
            m_ui->positionLabel->setText(position);
564
        }
565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589

        if (globalFrameKnown)
        {
            QString globalPosition;
            QString latIndicator;
            if (lat > 0)
            {
                latIndicator = "N";
            }
            else
            {
                latIndicator = "S";
            }
            QString lonIndicator;
            if (lon > 0)
            {
                lonIndicator = "E";
            }
            else
            {
                lonIndicator = "W";
            }

            globalPosition = globalPosition.sprintf("%05.1f%s %05.1f%s %06.1f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt);
            m_ui->positionLabel->setText(globalPosition);
590
        }
pixhawk's avatar
pixhawk committed
591

592
        // Altitude
593 594
        if (groundDistance == 0 && alt != 0)
        {
595
            m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt, 6, 'f', 1, '0'));
596 597 598
        }
        else
        {
599
            m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance, 6, 'f', 1, '0'));
600
        }
601

602
        // Speed
603 604
        QString speed("%1 m/s");
        m_ui->speedLabel->setText(speed.arg(totalSpeed, 4, 'f', 1, '0'));
pixhawk's avatar
pixhawk committed
605

606 607
        // Thrust
        m_ui->thrustBar->setValue(thrust * 100);
pixhawk's avatar
pixhawk committed
608

609 610
        if(this->timeRemaining > 1 && this->timeRemaining < QGC::MAX_FLIGHT_TIME)
        {
611
            // Filter output to get a higher stability
612
            filterTime = static_cast<int>(this->timeRemaining);
613 614 615 616 617 618 619 620
            filterTime = 0.8 * filterTime + 0.2 * static_cast<int>(this->timeRemaining);
            int sec = static_cast<int>(filterTime - static_cast<int>(filterTime / 60.0f) * 60);
            int min = static_cast<int>(filterTime / 60);
            int hours = static_cast<int>(filterTime - min * 60 - sec);

            QString timeText;
            timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec);
            m_ui->timeRemainingLabel->setText(timeText);
621 622 623
        }
        else
        {
624
            m_ui->timeRemainingLabel->setText(tr("Calc.."));
625 626 627 628 629 630 631
        }

        // Time Elapsed
        //QDateTime time = MG::TIME::msecToQDateTime(uas->getUptime());

        quint64 filterTime = uas->getUptime() / 1000;
        int sec = static_cast<int>(filterTime - static_cast<int>(filterTime / 60) * 60);
pixhawk's avatar
pixhawk committed
632 633 634 635
        int min = static_cast<int>(filterTime / 60);
        int hours = static_cast<int>(filterTime - min * 60 - sec);
        QString timeText;
        timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec);
636
        m_ui->timeElapsedLabel->setText(timeText);
pixhawk's avatar
pixhawk committed
637
    }
638
    generalUpdateCount++;
pixhawk's avatar
pixhawk committed
639

lm's avatar
lm committed
640
    QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 0px; border: 0px; background-color: %1; }");
641

642 643
    if (timeout)
    {
644 645
        // CRITICAL CONDITION, NO HEARTBEAT

646
        QString borderColor = "#FFFF00";
647 648
        if (isActive)
        {
649 650 651
            borderColor = "#FA4A4F";
        }

652 653
        if (iconIsRed)
        {
654 655
            QColor warnColor(Qt::red);
            m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name()));
656
            QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name());
657
            m_ui->uasViewFrame->setStyleSheet(style);
658 659 660
        }
        else
        {
661 662
            QColor warnColor(Qt::black);
            m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name()));
663
            QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name());
664
            m_ui->uasViewFrame->setStyleSheet(style);
LM's avatar
LM committed
665 666

            refreshTimer->setInterval(errorUpdateInterval);
667
            refreshTimer->start();
668 669
        }
        iconIsRed = !iconIsRed;
670 671 672
    }
    else
    {
673 674 675 676
        if (!lowPowerModeEnabled)
        {
            // Fade heartbeat icon
            // Make color darker
LM's avatar
LM committed
677
            heartbeatColor = heartbeatColor.darker(210);
678 679 680

            //m_ui->heartbeatIcon->setAutoFillBackground(true);
            m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name()));
LM's avatar
LM committed
681
            refreshTimer->setInterval(updateInterval);
682
            refreshTimer->start();
683
        }
684
    }
685
    //setUpdatesEnabled(true);
686 687

    //setUpdatesEnabled(false);
pixhawk's avatar
pixhawk committed
688 689 690 691 692
}

void UASView::changeEvent(QEvent *e)
{
    QWidget::changeEvent(e);
693 694
    switch (e->type())
    {
pixhawk's avatar
pixhawk committed
695 696 697 698 699 700 701
    case QEvent::LanguageChange:
        m_ui->retranslateUi(this);
        break;
    default:
        break;
    }
}