UASView.cc 15.7 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 34
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <cmath>
#include <QDateTime>
#include <QDebug>

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

UASView::UASView(UASInterface* uas, QWidget *parent) :
        QWidget(parent),
44
        startTime(0),
pixhawk's avatar
pixhawk committed
45
        timeRemaining(0),
46 47 48
        chargeLevel(0),
        uas(uas),
        load(0),
pixhawk's avatar
pixhawk committed
49 50
        state("UNKNOWN"),
        stateDesc(tr("Unknown system state")),
pixhawk's avatar
pixhawk committed
51 52
        mode("MAV_MODE_UNKNOWN"),
        thrust(0),
53
        isActive(false),
54 55 56 57 58 59 60 61
        x(0),
        y(0),
        z(0),
        totalSpeed(0),
        lat(0),
        lon(0),
        alt(0),
        groundDistance(0),
pixhawk's avatar
pixhawk committed
62 63 64
        m_ui(new Ui::UASView)
{
    m_ui->setupUi(this);
65
    
pixhawk's avatar
pixhawk committed
66
    // Setup communication
pixhawk's avatar
pixhawk committed
67
    //connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(receiveValue(int,QString,double,quint64)));
pixhawk's avatar
pixhawk committed
68 69 70 71 72 73 74
    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
75
    connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
pixhawk's avatar
pixhawk committed
76 77 78
    connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double)));
    //connect(uas, SIGNAL(waypointUpdated(int,int,double,double,double,double,bool,bool)), this, SLOT(setWaypoint(int,int,double,double,double,double,bool,bool)));
    connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int)));
79
    connect(&(uas->getWaypointManager()), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16)));
pixhawk's avatar
pixhawk committed
80
    connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
81
    connect(UASManager::instance(), SIGNAL(activeUASStatusChanged(UASInterface*,bool)), this, SLOT(updateActiveUAS(UASInterface*,bool)));
82
    
pixhawk's avatar
pixhawk committed
83
    // Setup UAS selection
84
    connect(m_ui->uasViewFrame, SIGNAL(clicked(bool)), this, SLOT(setUASasActive(bool)));
85
    
pixhawk's avatar
pixhawk committed
86 87 88 89
    // 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
90 91
    connect(m_ui->landButton, SIGNAL(clicked()), uas, SLOT(home()));
    connect(m_ui->abortButton, SIGNAL(clicked()), uas, SLOT(emergencySTOP()));
pixhawk's avatar
pixhawk committed
92 93
    connect(m_ui->killButton, SIGNAL(clicked()), uas, SLOT(emergencyKILL()));
    connect(m_ui->shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
94
    
pixhawk's avatar
pixhawk committed
95
    // Set static values
96
    
pixhawk's avatar
pixhawk committed
97 98 99 100 101 102 103 104 105
    // Name
    if (uas->getUASName() == "")
    {
        m_ui->nameLabel->setText(tr("UAS") + QString::number(uas->getUASID()));
    }
    else
    {
        m_ui->nameLabel->setText(uas->getUASName());
    }
106
    
107
    setBackgroundColor();
108
    
pixhawk's avatar
pixhawk committed
109 110 111 112 113 114 115 116 117 118
    // Heartbeat fade
    refreshTimer = new QTimer(this);
    connect(refreshTimer, SIGNAL(timeout()), this, SLOT(refresh()));
}

UASView::~UASView()
{
    delete m_ui;
}

119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143
/**
 * 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";
    if (isActive)
    {
        borderColor = "#FA4A4F";
        uasColor = uasColor.darker(475);
    }
    else
    {
        uasColor = uasColor.darker(675);
    }
    colorstyle = colorstyle.sprintf("QGroupBox { border-radius: 5px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 2px solid %s; }",
                                    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
144
{
145 146 147 148 149 150 151 152 153 154 155 156 157
    if (active)
    {
        UASManager::instance()->setActiveUAS(this->uas);
    }
}

void UASView::updateActiveUAS(UASInterface* uas, bool active)
{
    if (uas == this->uas)
    {
        this->isActive = active;
        setBackgroundColor();
    }
pixhawk's avatar
pixhawk committed
158 159
}

pixhawk's avatar
pixhawk committed
160 161
void UASView::updateMode(int sysId, QString status, QString description)
{
162
    Q_UNUSED(description);
pixhawk's avatar
pixhawk committed
163 164 165
    if (sysId == this->uas->getUASID()) m_ui->modeLabel->setText(status);
}

pixhawk's avatar
pixhawk committed
166 167
void UASView::mouseDoubleClickEvent (QMouseEvent * event)
{
168
    Q_UNUSED(event);
pixhawk's avatar
pixhawk committed
169 170 171 172 173 174
    UASManager::instance()->setActiveUAS(uas);
    qDebug() << __FILE__ << __LINE__ << "DOUBLECLICKED";
}

void UASView::enterEvent(QEvent* event)
{
175 176 177 178 179 180 181 182
    if (event->type() == QEvent::MouseMove)
    {
        emit uasInFocus(uas);
        if (uas != UASManager::instance()->getActiveUAS())
        {
            grabMouse(QCursor(Qt::PointingHandCursor));
        }
    }
pixhawk's avatar
pixhawk committed
183
    qDebug() << __FILE__ << __LINE__ << "IN FOCUS";
184
    
185 186 187 188
    if (event->type() == QEvent::MouseButtonDblClick)
    {
        qDebug() << __FILE__ << __LINE__ << "UAS CLICKED!";
    }
pixhawk's avatar
pixhawk committed
189 190 191 192
}

void UASView::leaveEvent(QEvent* event)
{
193 194 195 196 197 198 199 200 201 202 203
    if (event->type() == QEvent::MouseMove)
    {
        emit uasOutFocus(uas);
        releaseMouse();
    }
}

void UASView::showEvent(QShowEvent* event)
{
    // React only to internal (pre-display)
    // events
204 205 206 207 208 209 210 211 212 213
    Q_UNUSED(event);
    refreshTimer->start(updateInterval);
}

void UASView::hideEvent(QHideEvent* event)
{
    // React only to internal (pre-display)
    // events
    Q_UNUSED(event);
    refreshTimer->stop();
pixhawk's avatar
pixhawk committed
214 215 216 217
}

void UASView::receiveHeartbeat(UASInterface* uas)
{
218 219 220 221 222 223
    QString colorstyle;
    heartbeatColor = QColor(20, 200, 20);
    colorstyle = colorstyle.sprintf("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X;}",
                                    heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue());
    m_ui->heartbeatIcon->setStyleSheet(colorstyle);
    m_ui->heartbeatIcon->setAutoFillBackground(true);
pixhawk's avatar
pixhawk committed
224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255
}

/**
 * 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)
{
    if (uas == this->uas)
    {
        // Set matching icon
        switch (systemType)
        {
        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
256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274
            m_ui->typeButton->setIcon(QIcon(":/images/mavs/unknown.svg"));
            break;
        case 6:
            {
                // A groundstation is a special system type, update widget
                QString result;
                m_ui->nameLabel->setText(tr("OCU ") + result.sprintf("%03d", uas->getUASID()));
                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();
275 276
                m_ui->typeButton->setIcon(QIcon(":/images/mavs/groundstation.svg"));
            }
pixhawk's avatar
pixhawk committed
277 278 279 280 281 282 283 284 285 286
            break;
        default:
            m_ui->typeButton->setIcon(QIcon(":/images/mavs/unknown.svg"));
            break;
        }
    }
}

void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec)
{
287
    Q_UNUSED(usec);
pixhawk's avatar
pixhawk committed
288 289
    if (uas == this->uas)
    {
pixhawk's avatar
pixhawk committed
290 291 292
        this->x = x;
        this->y = y;
        this->z = z;
pixhawk's avatar
pixhawk committed
293 294 295
    }
}

296
void UASView::updateGlobalPosition(UASInterface* uas, double lon, double lat, double alt, quint64 usec)
pixhawk's avatar
pixhawk committed
297
{
298 299
    Q_UNUSED(uas);
    Q_UNUSED(usec);
pixhawk's avatar
pixhawk committed
300 301 302
    this->lon = lon;
    this->lat = lat;
    this->alt = alt;
pixhawk's avatar
pixhawk committed
303 304 305 306
}

void UASView::updateSpeed(UASInterface*, double x, double y, double z, quint64 usec)
{
307
    Q_UNUSED(usec);
pixhawk's avatar
pixhawk committed
308
    totalSpeed = sqrt((pow(x, 2) + pow(y, 2) + pow(z, 2)));
pixhawk's avatar
pixhawk committed
309 310
}

311 312 313 314 315
void UASView::currentWaypointUpdated(quint16 waypoint)
{
    m_ui->waypointLabel->setText(tr("WP") + QString::number(waypoint));
}

pixhawk's avatar
pixhawk committed
316 317
void UASView::setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current)
{
318 319 320 321 322
    Q_UNUSED(x);
    Q_UNUSED(y);
    Q_UNUSED(z);
    Q_UNUSED(yaw);
    Q_UNUSED(autocontinue);
pixhawk's avatar
pixhawk committed
323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343
    if (uasId == this->uas->getUASID())
    {
        if (current)
        {
            m_ui->waypointLabel->setText(tr("WP") + QString::number(id));
        }
    }
}

void UASView::selectWaypoint(int uasId, int id)
{
    if (uasId == this->uas->getUASID())
    {
        m_ui->waypointLabel->setText(tr("WP") + QString::number(id));
    }
}

void UASView::updateThrust(UASInterface* uas, double thrust)
{
    if (this->uas == uas)
    {
pixhawk's avatar
pixhawk committed
344
        this->thrust = thrust;
pixhawk's avatar
pixhawk committed
345 346 347 348 349
    }
}

void UASView::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
{
350
    Q_UNUSED(voltage);
pixhawk's avatar
pixhawk committed
351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376
    if (this->uas == uas)
    {
        timeRemaining = seconds;
        chargeLevel = percent;
    }
}

void UASView::updateState(UASInterface* uas, QString uasState, QString stateDescription)
{
    if (this->uas == uas)
    {
        state = uasState;
        stateDesc = stateDescription;
    }
}

void UASView::updateLoad(UASInterface* uas, double load)
{
    if (this->uas == uas)
    {
        this->load = load;
    }
}

void UASView::refresh()
{
377
    //setUpdatesEnabled(false);
378 379
    //setUpdatesEnabled(true);
    //repaint();
380 381

    static quint64 lastupdate = 0;
pixhawk's avatar
pixhawk committed
382
    //qDebug() << "UASVIEW update diff: " << MG::TIME::getGroundTimeNow() - lastupdate;
383 384 385 386 387 388 389
    lastupdate = MG::TIME::getGroundTimeNow();

    // FIXME
    static int generalUpdateCount = 0;

    if (generalUpdateCount == 4)
    {
390 391 392
#if (QGC_EVENTLOOP_DEBUG)
        qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif
393
        generalUpdateCount = 0;
pixhawk's avatar
pixhawk committed
394
        //qDebug() << "UPDATING EVERYTHING";
395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428
        // 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
        QString position;
        position = position.sprintf("%02.2f %02.2f %02.2f m", x, y, z);
        m_ui->positionLabel->setText(position);
        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("%02.2f%s %02.2f%s %02.2f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt);
        m_ui->gpsLabel->setText(globalPosition);
pixhawk's avatar
pixhawk committed
429

430 431 432 433 434 435 436 437 438
        // Altitude
        if (groundDistance == 0 && alt != 0)
        {
            m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt));
        }
        else
        {
            m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance));
        }
439

440 441 442 443
        // Speed
        QString speed;
        speed = speed.sprintf("%02.2f m/s", totalSpeed);
        m_ui->speedLabel->setText(speed);
pixhawk's avatar
pixhawk committed
444

445 446
        // Thrust
        m_ui->thrustBar->setValue(thrust * 100);
pixhawk's avatar
pixhawk committed
447

448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470
        if(this->timeRemaining > 1 && this->timeRemaining < MG::MAX_FLIGHT_TIME)
        {
            // Filter output to get a higher stability
            static double filterTime = static_cast<int>(this->timeRemaining);
            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);
        }
        else
        {
            m_ui->timeRemainingLabel->setText(tr("Calculating"));
        }

        // 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
471 472 473 474
        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);
475
        m_ui->timeElapsedLabel->setText(timeText);
pixhawk's avatar
pixhawk committed
476
    }
477
    generalUpdateCount++;
pixhawk's avatar
pixhawk committed
478 479 480

    // Fade heartbeat icon
    // Make color darker
pixhawk's avatar
pixhawk committed
481
    heartbeatColor = heartbeatColor.darker(150);
pixhawk's avatar
pixhawk committed
482 483 484 485 486 487

    QString colorstyle;
    colorstyle = colorstyle.sprintf("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X;}",
                                    heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue());
    m_ui->heartbeatIcon->setStyleSheet(colorstyle);
    m_ui->heartbeatIcon->setAutoFillBackground(true);
488
    //setUpdatesEnabled(true);
489 490

    //setUpdatesEnabled(false);
pixhawk's avatar
pixhawk committed
491 492 493 494 495 496 497 498 499 500 501 502 503
}

void UASView::changeEvent(QEvent *e)
{
    QWidget::changeEvent(e);
    switch (e->type()) {
    case QEvent::LanguageChange:
        m_ui->retranslateUi(this);
        break;
    default:
        break;
    }
}