QGCMAVLinkLogPlayer.cc 22.8 KB
Newer Older
1
#include <QStandardPaths>
2
#include <QtEndian>
3

4
#include "MainWindow.h"
5
#include "SerialLink.h"
6
#include "QGCMAVLinkLogPlayer.h"
7
#include "QGC.h"
8
#include "ui_QGCMAVLinkLogPlayer.h"
Don Gagne's avatar
Don Gagne committed
9
#include "QGCApplication.h"
Don Gagne's avatar
Don Gagne committed
10
#include "LinkManager.h"
Don Gagne's avatar
Don Gagne committed
11
#include "QGCFileDialog.h"
12
#include "QGCMessageBox.h"
13 14

QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *parent) :
15
    QWidget(parent),
16 17 18
    playbackStartTime(0),
    logStartTime(0),
    logEndTime(0),
19 20 21 22 23
    accelerationFactor(1.0f),
    mavlink(mavlink),
    logLink(NULL),
    loopCounter(0),
    mavlinkLogFormat(true),
24
    binaryBaudRate(defaultBinaryBaudRate),
lm's avatar
lm committed
25
    isPlaying(false),
26
    currPacketCount(0),
27
    ui(new Ui::QGCMAVLinkLogPlayer)
28
{
Don Gagne's avatar
Don Gagne committed
29 30
    Q_ASSERT(mavlink);
    
31
    ui->setupUi(this);
32
    ui->horizontalLayout->setAlignment(Qt::AlignTop);
33

34 35 36
    // Connect protocol
    connect(this, SIGNAL(bytesReady(LinkInterface*,QByteArray)), mavlink, SLOT(receiveBytes(LinkInterface*,QByteArray)));

37
    // Setup timer
Don Gagne's avatar
Don Gagne committed
38
    connect(&loopTimer, &QTimer::timeout, this, &QGCMAVLinkLogPlayer::logLoop);
39

40
    // Setup buttons
Don Gagne's avatar
Don Gagne committed
41 42 43 44 45 46
    connect(ui->selectFileButton, &QPushButton::clicked, this, &QGCMAVLinkLogPlayer::_selectLogFileForPlayback);
    connect(ui->playButton, &QPushButton::clicked, this, &QGCMAVLinkLogPlayer::playPauseToggle);
    connect(ui->speedSlider, &QSlider::valueChanged, this, &QGCMAVLinkLogPlayer::setAccelerationFactorInt);
    connect(ui->positionSlider, &QSlider::valueChanged, this, &QGCMAVLinkLogPlayer::jumpToSliderVal);
    connect(ui->positionSlider, &QSlider::sliderPressed, this, &QGCMAVLinkLogPlayer::pause);
    
47 48
    setAccelerationFactorInt(49);
    ui->speedSlider->setValue(49);
49
    updatePositionSliderUi(0.0);
Lorenz Meier's avatar
Lorenz Meier committed
50 51 52 53

    ui->playButton->setEnabled(false);
    ui->speedSlider->setEnabled(false);
    ui->positionSlider->setEnabled(false);
54 55 56
    ui->speedLabel->setEnabled(false);
    ui->logFileNameLabel->setEnabled(false);
    ui->logStatsLabel->setEnabled(false);
Lorenz Meier's avatar
Lorenz Meier committed
57

58
    // Monitor for when the end of the log file is reached. This is done using signals because the main work is in a timer.
Don Gagne's avatar
Don Gagne committed
59
    connect(this,  &QGCMAVLinkLogPlayer::logFileEndReached, &loopTimer, &QTimer::stop);
60 61 62 63 64 65 66
}

QGCMAVLinkLogPlayer::~QGCMAVLinkLogPlayer()
{
    delete ui;
}

lm's avatar
lm committed
67 68 69 70 71 72 73 74 75 76 77 78
void QGCMAVLinkLogPlayer::playPauseToggle()
{
    if (isPlaying)
    {
        pause();
    }
    else
    {
        play();
    }
}

79 80
void QGCMAVLinkLogPlayer::play()
{
Don Gagne's avatar
Don Gagne committed
81 82 83
    Q_ASSERT(logFile.isOpen());
    
    LinkManager::instance()->setConnectionsSuspended(tr("Connect not allowed during Flight Data replay."));
84
    mavlink->suspendLogForReplay(true);
Don Gagne's avatar
Don Gagne committed
85 86 87 88 89 90
    
    // Disable the log file selector button
    ui->selectFileButton->setEnabled(false);

    // Make sure we aren't at the end of the file, if we are, reset to the beginning and play from there.
    if (logFile.atEnd())
lm's avatar
lm committed
91
    {
Don Gagne's avatar
Don Gagne committed
92 93
        reset();
    }
94

Don Gagne's avatar
Don Gagne committed
95 96 97
    // Always correct the current start time such that the next message will play immediately at playback.
    // We do this by subtracting the current file playback offset  from now()
    playbackStartTime = (quint64)QDateTime::currentMSecsSinceEpoch() - (logCurrentTime - logStartTime) / 1000;
98

Don Gagne's avatar
Don Gagne committed
99 100 101 102
    // Start timer
    if (mavlinkLogFormat)
    {
        loopTimer.start(1);
lm's avatar
lm committed
103 104 105
    }
    else
    {
Don Gagne's avatar
Don Gagne committed
106 107 108 109 110 111 112
        // Read len bytes at a time
        int len = 100;
        // Calculate the number of times to read 100 bytes per second
        // to guarantee the baud rate, then divide 1000 by the number of read
        // operations to obtain the interval in milliseconds
        int interval = 1000 / ((binaryBaudRate / 10) / len);
        loopTimer.start(interval / accelerationFactor);
113
    }
Don Gagne's avatar
Don Gagne committed
114 115
    isPlaying = true;
    ui->playButton->setChecked(true);
Don Gagne's avatar
Don Gagne committed
116
    ui->playButton->setIcon(QIcon(":/res/Pause"));
117 118 119 120
}

void QGCMAVLinkLogPlayer::pause()
{
Don Gagne's avatar
Don Gagne committed
121
    LinkManager::instance()->setConnectionsAllowed();
122
    mavlink->suspendLogForReplay(false);
Don Gagne's avatar
Don Gagne committed
123

124
    loopTimer.stop();
125
    isPlaying = false;
Don Gagne's avatar
Don Gagne committed
126
    ui->playButton->setIcon(QIcon(":/res/Play"));
127
    ui->playButton->setChecked(false);
128 129 130
    ui->selectFileButton->setEnabled(true);
}

131 132 133 134
void QGCMAVLinkLogPlayer::reset()
{
    pause();
    loopCounter = 0;
Don Gagne's avatar
Don Gagne committed
135 136 137
    if (logFile.isOpen()) {
        logFile.reset();
    }
138 139 140 141 142 143 144 145 146 147

    // Now update the position slider to its default location
    updatePositionSliderUi(0.0);

    // And since we haven't starting playback, clear the time of initial playback and the current timestamp.
    playbackStartTime = 0;
    logCurrentTime = logStartTime;
}

bool QGCMAVLinkLogPlayer::jumpToPlaybackLocation(float percentage)
148
{
149
    // Reset only for valid values
150
    if (percentage <= 100.0f && percentage >= 0.0f)
lm's avatar
lm committed
151
    {
152
        bool result = true;
153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170
        if (mavlinkLogFormat)
        {
            // But if we have a timestamped MAVLink log, then actually aim to hit that percentage in terms of
            // time through the file.
            qint64 newFilePos = (qint64)(percentage * (float)logFile.size());

            // Now seek to the appropriate position, failing gracefully if we can't.
            if (!logFile.seek(newFilePos))
            {
                // Fallback: Start from scratch
                logFile.reset();
                ui->logStatsLabel->setText(tr("Changing packet index failed, back to start."));
                result = false;
            }

            // But we do align to the next MAVLink message for consistency.
            mavlink_message_t dummy;
            logCurrentTime = findNextMavlinkMessage(&dummy);
lm's avatar
lm committed
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
            // Now calculate the current file location based on time.
            float newRelativeTime = (float)(logCurrentTime - logStartTime);

            // Calculate the effective baud rate of the file in bytes/s.
            float logDuration = (logEndTime - logStartTime);
            float baudRate = logFile.size() / logDuration / 1e6;

            // And the desired time is:
            float desiredTime = percentage * logDuration;

            // And now jump the necessary number of bytes in the proper direction
            qint64 offset = (newRelativeTime - desiredTime) * baudRate;
            logFile.seek(logFile.pos() + offset);

            // And scan until we reach the start of a MAVLink message. We make sure to record this timestamp for
            // smooth jumping around the file.
            logCurrentTime = findNextMavlinkMessage(&dummy);

            // Now update the UI with our actual final position.
            newRelativeTime = (float)(logCurrentTime - logStartTime);
            percentage = newRelativeTime / logDuration;
            updatePositionSliderUi(percentage);
        }
        else
lm's avatar
lm committed
196
        {
197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212
            // If we're working with a non-timestamped file, we just jump to that percentage of the file,
            // align to the next MAVLink message and roll with it. No reason to do anything more complicated.
            qint64 newFilePos = (qint64)(percentage * (float)logFile.size());

            // Now seek to the appropriate position, failing gracefully if we can't.
            if (!logFile.seek(newFilePos))
            {
                // Fallback: Start from scratch
                logFile.reset();
                ui->logStatsLabel->setText(tr("Changing packet index failed, back to start."));
                result = false;
            }

            // But we do align to the next MAVLink message for consistency.
            mavlink_message_t dummy;
            findNextMavlinkMessage(&dummy);
213 214
        }

215 216
        // Now update the UI. This is necessary because stop() is called when loading a new logfile

217
        return result;
lm's avatar
lm committed
218 219 220
    }
    else
    {
221 222
        return false;
    }
223 224
}

225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244
void QGCMAVLinkLogPlayer::updatePositionSliderUi(float percent)
{
    ui->positionSlider->blockSignals(true);
    int sliderVal = ui->positionSlider->minimum() + (int)(percent * (float)(ui->positionSlider->maximum() - ui->positionSlider->minimum()));
    ui->positionSlider->setValue(sliderVal);

    // Calculate the runtime in hours:minutes:seconds
    // WARNING: Order matters in this computation
    quint32 seconds = percent * (logEndTime - logStartTime) / 1e6;
    quint32 minutes = seconds / 60;
    quint32 hours = minutes / 60;
    seconds -= 60*minutes;
    minutes -= 60*hours;

    // And show the user the details we found about this file.
    QString timeLabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2);
    ui->positionSlider->setToolTip(timeLabel);
    ui->positionSlider->blockSignals(false);
}

Don Gagne's avatar
Don Gagne committed
245 246
/// @brief Displays a file dialog to allow the user to select a log file to play back.
void QGCMAVLinkLogPlayer::_selectLogFileForPlayback(void)
Lorenz Meier's avatar
Lorenz Meier committed
247
{
Don Gagne's avatar
Don Gagne committed
248
    // Disallow replay when any links are connected
249
    if (LinkManager::instance()->anyConnectedLinks()) {
250
        QGCMessageBox::information(tr("Log Replay"), tr("You must close all connections prior to replaying a log."));
Don Gagne's avatar
Don Gagne committed
251 252 253
        return;
    }
    
254 255 256 257
    QString logFile = QGCFileDialog::getOpenFileName(
        this,
        tr("Load MAVLink Log File"),
        qgcApp()->mavlinkLogFilesLocation(),
258
        tr("MAVLink Log Files (*.mavlink);;All Files (*)"));
259

Don Gagne's avatar
Don Gagne committed
260 261
    if (!logFile.isEmpty()) {
        loadLogFile(logFile);
262
    }
263 264
}

265 266 267 268 269 270 271 272
/**
 * @param factor 0: 0.01X, 50: 1.0X, 100: 100.0X
 */
void QGCMAVLinkLogPlayer::setAccelerationFactorInt(int factor)
{
    float f = factor+1.0f;
    f -= 50.0f;

lm's avatar
lm committed
273 274
    if (f < 0.0f)
    {
275
        accelerationFactor = 1.0f / (-f/2.0f);
lm's avatar
lm committed
276 277 278
    }
    else
    {
279 280 281
        accelerationFactor = 1+(f/2.0f);
    }

282
    // Update timer interval
lm's avatar
lm committed
283 284
    if (!mavlinkLogFormat)
    {
285 286 287 288 289 290 291
        // Read len bytes at a time
        int len = 100;
        // Calculate the number of times to read 100 bytes per second
        // to guarantee the baud rate, then divide 1000 by the number of read
        // operations to obtain the interval in milliseconds
        int interval = 1000 / ((binaryBaudRate / 10) / len);
        loopTimer.stop();
292
        loopTimer.start(interval / accelerationFactor);
293 294
    }

295
    ui->speedLabel->setText(tr("Speed: %1X").arg(accelerationFactor, 5, 'f', 2, '0'));
296 297
}

298
bool QGCMAVLinkLogPlayer::loadLogFile(const QString& file)
299
{
300 301
    // Make sure to stop the logging process and reset everything.
    reset();
Don Gagne's avatar
Don Gagne committed
302
    logFile.close();
303

304 305
    // Now load the new file.
    logFile.setFileName(file);
Don Gagne's avatar
Don Gagne committed
306
    if (!logFile.open(QFile::ReadOnly)) {
307
        QGCMessageBox::critical(tr("Log Replay"), tr("The selected file is unreadable. Please make sure that the file %1 is readable or select a different file").arg(file));
Don Gagne's avatar
Don Gagne committed
308
        _playbackError();
309
        return false;
lm's avatar
lm committed
310
    }
Don Gagne's avatar
Don Gagne committed
311 312 313
    
    QFileInfo logFileInfo(file);
    ui->logFileNameLabel->setText(tr("File: %1").arg(logFileInfo.fileName()));
314

Don Gagne's avatar
Don Gagne committed
315 316
    // If there's an existing MAVLinkSimulationLink() being used for an old file,
    // we replace it.
317
    if (logLink) {
318
        LinkManager::instance()->_deleteLink(logLink);
Don Gagne's avatar
Don Gagne committed
319
    }
320 321
    logLink = new MockLink();
    LinkManager::instance()->_addLink(logLink);
322

Don Gagne's avatar
Don Gagne committed
323 324
    // Select if binary or MAVLink log format is used
    mavlinkLogFormat = file.endsWith(".mavlink");
325

Don Gagne's avatar
Don Gagne committed
326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346
    if (mavlinkLogFormat)
    {
        // Get the first timestamp from the logfile
        // This should be a big-endian uint64.
        QByteArray timestamp = logFile.read(timeLen);
        quint64 starttime = parseTimestamp(timestamp);

        // Now find the last timestamp by scanning for the last MAVLink packet and
        // find the timestamp before it. To do this we start searchin a little before
        // the end of the file, specifically the maximum MAVLink packet size + the
        // timestamp size. This guarantees that we will hit a MAVLink packet before
        // the end of the file. Unfortunately, it basically guarantees that we will
        // hit more than one. This is why we have to search for a bit.
        qint64 fileLoc = logFile.size() - MAVLINK_MAX_PACKET_LEN - timeLen;
        logFile.seek(fileLoc);
        quint64 endtime = starttime; // Set a sane default for the endtime
        mavlink_message_t msg;
        quint64 newTimestamp;
        while ((newTimestamp = findNextMavlinkMessage(&msg)) > endtime) {
            endtime = newTimestamp;
        }
347

Don Gagne's avatar
Don Gagne committed
348
        if (endtime == starttime) {
349
            QGCMessageBox::critical(tr("Log Replay"), tr("The selected file is corrupt. No valid timestamps were found at the end of the file.").arg(file));
Don Gagne's avatar
Don Gagne committed
350 351 352
            _playbackError();
            return false;
        }
353

Don Gagne's avatar
Don Gagne committed
354 355 356 357
        // Remember the start and end time so we can move around this logfile with the slider.
        logEndTime = endtime;
        logStartTime = starttime;
        logCurrentTime = logStartTime;
358

Don Gagne's avatar
Don Gagne committed
359 360
        // Reset our log file so when we go to read it for the first time, we start at the beginning.
        logFile.reset();
361

Don Gagne's avatar
Don Gagne committed
362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384
        // Calculate the runtime in hours:minutes:seconds
        // WARNING: Order matters in this computation
        quint32 seconds = (endtime - starttime)/1000000;
        quint32 minutes = seconds / 60;
        quint32 hours = minutes / 60;
        seconds -= 60*minutes;
        minutes -= 60*hours;

        // And show the user the details we found about this file.
        QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2);
        currPacketCount = logFileInfo.size()/(32 + MAVLINK_NUM_NON_PAYLOAD_BYTES + sizeof(quint64)); // Count packets by assuming an average payload size of 32 bytes
        ui->logStatsLabel->setText(tr("%2 MB, ~%3 packets, %4").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(currPacketCount).arg(timelabel));
    }
    else
    {
        // Load in binary mode. In this mode, files should be have a filename postfix
        // of the baud rate they were recorded at, like `test_run_115200.bin`. Then on
        // playback, the datarate is equal to set to this value.

        // Set baud rate if any present. Otherwise we default to 57600.
        QStringList parts = logFileInfo.baseName().split("_");
        binaryBaudRate = defaultBinaryBaudRate;
        if (parts.count() > 1)
lm's avatar
lm committed
385
        {
Don Gagne's avatar
Don Gagne committed
386 387 388 389
            bool ok;
            int rate = parts.last().toInt(&ok);
            // 9600 baud to 100 MBit
            if (ok && (rate > 9600 && rate < 100000000))
lm's avatar
lm committed
390
            {
Don Gagne's avatar
Don Gagne committed
391 392
                // Accept this as valid baudrate
                binaryBaudRate = rate;
393 394
            }
        }
395

Don Gagne's avatar
Don Gagne committed
396 397 398 399 400
        int seconds = logFileInfo.size() / (binaryBaudRate / 10);
        int minutes = seconds / 60;
        int hours = minutes / 60;
        seconds -= 60*minutes;
        minutes -= 60*hours;
401

Don Gagne's avatar
Don Gagne committed
402 403 404
        QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2);
        ui->logStatsLabel->setText(tr("%2 MB, %4 at %5 KB/s").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(timelabel).arg(binaryBaudRate/10.0f/1024.0f, 0, 'f', 2));
    }
405

Don Gagne's avatar
Don Gagne committed
406 407 408 409 410 411 412 413 414
    // Enable controls
    ui->playButton->setEnabled(true);
    ui->speedSlider->setEnabled(true);
    ui->positionSlider->setEnabled(true);
    ui->speedLabel->setEnabled(true);
    ui->logFileNameLabel->setEnabled(true);
    ui->logStatsLabel->setEnabled(true);
    
    play();
415

Don Gagne's avatar
Don Gagne committed
416
    return true;
417 418
}

419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435
quint64 QGCMAVLinkLogPlayer::parseTimestamp(const QByteArray &data)
{
    // Retrieve the timestamp from the ByteArray assuming a proper BigEndian quint64 timestamp in microseconds.
    quint64 timestamp = qFromBigEndian(*((quint64*)(data.constData())));

    // And get the current time in microseconds
    quint64 currentTimestamp = ((quint64)QDateTime::currentMSecsSinceEpoch()) * 1000;

    // Now if the parsed timestamp is in the future, it must be an old file where the timestamp was stored as
    // little endian, so switch it.
    if (timestamp > currentTimestamp) {
        timestamp = qbswap(timestamp);
    }

    return timestamp;
}

436
/**
437 438
 * Jumps to the current percentage of the position slider. When this is called, the LogPlayer should already
 * have been paused, so it just jumps to the proper location in the file and resumes playing.
439 440
 */
void QGCMAVLinkLogPlayer::jumpToSliderVal(int slidervalue)
441
{
442 443 444 445 446 447 448 449 450 451 452 453
    // Determine what percentage through the file we should be (time or packet number depending).
    float newLocation = slidervalue / (float)(ui->positionSlider->maximum() - ui->positionSlider->minimum());

    // And clamp our calculated values to the valid range of [0,100]
    if (newLocation > 100.0f)
    {
        newLocation = 100.0f;
    }
    if (newLocation < 0.0f)
    {
        newLocation = 0.0f;
    }
454

455 456
    // Do only valid jumps
    if (jumpToPlaybackLocation(newLocation))
lm's avatar
lm committed
457 458 459
    {
        if (mavlinkLogFormat)
        {
460 461 462 463 464 465 466 467 468 469 470 471
            // Grab the total seconds of this file (1e6 is due to microsecond -> second conversion)
            int seconds = newLocation * (logEndTime - logStartTime) / 1e6;
            int minutes = seconds / 60;
            int hours = minutes / 60;
            seconds -= 60*minutes;
            minutes -= 60*hours;

            ui->logStatsLabel->setText(tr("Jumped to time %1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2));
        }
        else
        {
            ui->logStatsLabel->setText(tr("Jumped to %1").arg(newLocation));
472
        }
473 474 475 476 477 478

        play();
    }
    else
    {
        reset();
479
    }
480
}
481

482 483 484 485 486 487 488 489
/**
 * This function is the "mainloop" of the log player, reading one line
 * and adjusting the mainloop timer to read the next line in time.
 * It might not perfectly match the timing of the log file,
 * but it will never induce a static drift into the log file replay.
 * For scientific logging, the use of onboard timestamps and the log
 * functionality of the line chart plot is recommended.
 */
490
void QGCMAVLinkLogPlayer::logLoop()
491
{
492 493
    // If we have a file with timestamps, try and pace this out following the time differences
    // between the timestamps and the current playback speed.
lm's avatar
lm committed
494 495
    if (mavlinkLogFormat)
    {
496
        // Now parse MAVLink messages, grabbing their timestamps as we go. We stop once we
Bryant Mairs's avatar
Bryant Mairs committed
497 498 499 500
        // have at least 3ms until the next one. 

        // We track what the next execution time should be in milliseconds, which we use to set
        // the next timer interrupt.
501
        int nextExecutionTime = 0;
Bryant Mairs's avatar
Bryant Mairs committed
502 503 504 505

        // We use the `findNextMavlinkMessage()` function to scan ahead for MAVLink messages. This
        // is necessary because we don't know how big each MAVLink message is until we finish parsing
        // one, and since we only output arrays of bytes, we need to know the size of that array.
506
        mavlink_message_t msg;
Bryant Mairs's avatar
Bryant Mairs committed
507 508
        findNextMavlinkMessage(&msg);

509
        while (nextExecutionTime < 3) {
510

511 512
            // Now we're sitting at the start of a MAVLink message, so read it all into a byte array for feeding to our parser.
            QByteArray message = logFile.read(msg.len + MAVLINK_NUM_NON_PAYLOAD_BYTES);
513

514 515
            // Emit this message to our MAVLink parser.
            emit bytesReady(logLink, message);
516

517
            // If we've reached the end of the of the file, make sure we handle that well
Don Gagne's avatar
Don Gagne committed
518 519
            if (logFile.atEnd()) {
                _finishPlayback();
520 521
                return;
            }
522

523 524
            // Run our parser to find the next timestamp and leave us at the start of the next MAVLink message.
            logCurrentTime = findNextMavlinkMessage(&msg);
525

526 527 528 529 530 531
            // Calculate how long we should wait in real time until parsing this message.
            // We pace ourselves relative to the start time of playback to fix any drift (initially set in play())
            qint64 timediff = (logCurrentTime - logStartTime) / accelerationFactor;
            quint64 desiredPacedTime = playbackStartTime + ((quint64)timediff) / 1000;
            quint64 currentTime = (quint64)QDateTime::currentMSecsSinceEpoch();
            nextExecutionTime = desiredPacedTime - currentTime;
532
        }
533

534 535
        // And schedule the next execution of this function.
        loopTimer.start(nextExecutionTime);
lm's avatar
lm committed
536 537 538
    }
    else
    {
539 540 541 542 543 544 545 546
        // Binary format - read at fixed rate
        const int len = 100;
        QByteArray chunk = logFile.read(len);

        // Emit this packet
        emit bytesReady(logLink, chunk);

        // Check if reached end of file before reading next timestamp
lm's avatar
lm committed
547 548
        if (chunk.length() < len || logFile.atEnd())
        {
Don Gagne's avatar
Don Gagne committed
549
            _finishPlayback();
550 551
            return;
        }
552
    }
553

554 555 556
    // Update the UI every 2^5=32 times, or when there isn't much data to be played back.
    // Reduces flickering and minimizes CPU load.
    if ((loopCounter & 0x1F) == 0 || currPacketCount < 2000)
lm's avatar
lm committed
557
    {
558
        QFileInfo logFileInfo(logFile);
559
        updatePositionSliderUi(logFile.pos() / static_cast<float>(logFileInfo.size()));
560
    }
561
    loopCounter++;
562
}
563

564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579
/**
 * This function parses out the next MAVLink message and its corresponding timestamp.
 *
 * It makes no assumptions about where in the file we currently are. It leaves the file right
 * at the beginning of the successfully parsed message. Note that this function will not attempt to
 * correct for any MAVLink parsing failures, so it always returns the next successfully-parsed
 * message.
 *
 * @param msg[output] Where the final parsed message output will go.
 * @return A Unix timestamp in microseconds UTC or 0 if parsing failed
 */
quint64 QGCMAVLinkLogPlayer::findNextMavlinkMessage(mavlink_message_t *msg)
{
    char nextByte;
    mavlink_status_t comm;
    while (logFile.getChar(&nextByte)) { // Loop over every byte
580
        bool messageFound = mavlink_parse_char(logLink->getMavlinkChannel(), nextByte, msg, &comm);
581 582 583 584 585 586 587 588 589 590 591 592 593 594

        // If we've found a message, jump back to the start of the message, grab the timestamp,
        // and go back to the end of this file.
        if (messageFound) {
            logFile.seek(logFile.pos() - (msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES + timeLen));
            QByteArray rawTime = logFile.read(timeLen);
            return parseTimestamp(rawTime);
        }
    }

    // Otherwise, if we never find a message, return a failure code of 0.
    return 0;
}

595 596 597
void QGCMAVLinkLogPlayer::changeEvent(QEvent *e)
{
    QWidget::changeEvent(e);
lm's avatar
lm committed
598 599
    switch (e->type())
    {
600 601 602 603 604 605 606
    case QEvent::LanguageChange:
        ui->retranslateUi(this);
        break;
    default:
        break;
    }
}
607 608 609 610 611 612 613 614 615 616

/**
 * Implement paintEvent() so that stylesheets work for our custom widget.
 */
void QGCMAVLinkLogPlayer::paintEvent(QPaintEvent *)
{
    QStyleOption opt;
    opt.init(this);
    QPainter p(this);
    style()->drawPrimitive(QStyle::PE_Widget, &opt, &p, this);
617
}
Don Gagne's avatar
Don Gagne committed
618 619 620 621 622 623 624 625 626 627 628 629 630 631

/// @brief Called when playback is complete
void QGCMAVLinkLogPlayer::_finishPlayback(void)
{
    pause();
    
    QString status = tr("Flight Data replay complete");
    ui->logStatsLabel->setText(status);
    
    // Note that we explicitly set the slider to 100%, as it may not hit that by itself depending on log file size.
    updatePositionSliderUi(100.0f);
    
    emit logFileEndReached();
    
632
    mavlink->suspendLogForReplay(false);
Don Gagne's avatar
Don Gagne committed
633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653
    LinkManager::instance()->setConnectionsAllowed();
}

/// @brief Called when an error occurs during playback to reset playback system state.
void QGCMAVLinkLogPlayer::_playbackError(void)
{
    pause();
    
    logFile.close();
    logFile.setFileName("");
    
    ui->logFileNameLabel->setText(tr("No Flight Data selected"));    
    
    // Disable playback controls
    ui->playButton->setEnabled(false);
    ui->speedSlider->setEnabled(false);
    ui->positionSlider->setEnabled(false);
    ui->speedLabel->setEnabled(false);
    ui->logFileNameLabel->setEnabled(false);
    ui->logStatsLabel->setEnabled(false);
}