QGCMAVLinkLogPlayer.cc 17.4 KB
Newer Older
1 2 3 4
#include <QFileDialog>
#include <QMessageBox>
#include <QDesktopServices>

5
#include "MainWindow.h"
6
#include "SerialLink.h"
7
#include "QGCMAVLinkLogPlayer.h"
8
#include "QGC.h"
9 10 11
#include "ui_QGCMAVLinkLogPlayer.h"

QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *parent) :
12 13 14 15 16 17 18 19 20 21 22 23
    QWidget(parent),
    lineCounter(0),
    totalLines(0),
    startTime(0),
    endTime(0),
    currentStartTime(0),
    accelerationFactor(1.0f),
    mavlink(mavlink),
    logLink(NULL),
    loopCounter(0),
    mavlinkLogFormat(true),
    binaryBaudRate(57600),
lm's avatar
lm committed
24
    isPlaying(false),
25
    currPacketCount(0),
Lorenz Meier's avatar
Lorenz Meier committed
26
    lastLogDirectory(QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)),
27
    ui(new Ui::QGCMAVLinkLogPlayer)
28 29
{
    ui->setupUi(this);
30
    ui->horizontalLayout->setAlignment(Qt::AlignTop);
31

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

35 36 37
    // Setup timer
    connect(&loopTimer, SIGNAL(timeout()), this, SLOT(logLoop()));

38 39
    // Setup buttons
    connect(ui->selectFileButton, SIGNAL(clicked()), this, SLOT(selectLogFile()));
lm's avatar
lm committed
40
    connect(ui->playButton, SIGNAL(clicked()), this, SLOT(playPauseToggle()));
41
    connect(ui->speedSlider, SIGNAL(valueChanged(int)), this, SLOT(setAccelerationFactorInt(int)));
42 43
    connect(ui->positionSlider, SIGNAL(valueChanged(int)), this, SLOT(jumpToSliderVal(int)));
    connect(ui->positionSlider, SIGNAL(sliderPressed()), this, SLOT(pause()));
44

45 46
    setAccelerationFactorInt(49);
    ui->speedSlider->setValue(49);
47
    ui->positionSlider->setValue(ui->positionSlider->minimum());
Lorenz Meier's avatar
Lorenz Meier committed
48 49 50 51

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

Lorenz Meier's avatar
Lorenz Meier committed
56
    loadSettings();
57 58 59 60
}

QGCMAVLinkLogPlayer::~QGCMAVLinkLogPlayer()
{
Lorenz Meier's avatar
Lorenz Meier committed
61
    storeSettings();
62 63 64
    delete ui;
}

lm's avatar
lm committed
65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88
void QGCMAVLinkLogPlayer::playPause(bool enabled)
{
    if (enabled)
    {
        play();
    }
    else
    {
        pause();
    }
}

void QGCMAVLinkLogPlayer::playPauseToggle()
{
    if (isPlaying)
    {
        pause();
    }
    else
    {
        play();
    }
}

89 90
void QGCMAVLinkLogPlayer::play()
{
lm's avatar
lm committed
91 92
    if (logFile.isOpen())
    {
93
        ui->selectFileButton->setEnabled(false);
lm's avatar
lm committed
94 95
        if (logLink)
        {
96 97
            logLink->disconnect();
            LinkManager::instance()->removeLink(logLink);
98 99 100 101 102
            delete logLink;
        }
        logLink = new MAVLinkSimulationLink("");

        // Start timer
lm's avatar
lm committed
103 104
        if (mavlinkLogFormat)
        {
105
            loopTimer.start(1);
lm's avatar
lm committed
106 107 108
        }
        else
        {
109 110 111 112 113 114
            // 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);
115
            loopTimer.start(interval*accelerationFactor);
116
        }
lm's avatar
lm committed
117
        isPlaying = true;
118
        ui->logStatsLabel->setText(tr("Started playing.."));
119
        ui->playButton->setIcon(QIcon(":files/images/actions/media-playback-pause.svg"));
lm's avatar
lm committed
120 121 122
    }
    else
    {
123 124 125 126 127 128 129 130 131 132 133 134 135
        ui->playButton->setChecked(false);
        QMessageBox msgBox;
        msgBox.setIcon(QMessageBox::Information);
        msgBox.setText(tr("No logfile selected"));
        msgBox.setInformativeText(tr("Please select first a MAVLink log file before playing it."));
        msgBox.setStandardButtons(QMessageBox::Ok);
        msgBox.setDefaultButton(QMessageBox::Ok);
        msgBox.exec();
    }
}

void QGCMAVLinkLogPlayer::pause()
{
lm's avatar
lm committed
136
    isPlaying = false;
137
    loopTimer.stop();
138
    ui->playButton->setIcon(QIcon(":files/images/actions/media-playback-start.svg"));
139
    ui->selectFileButton->setEnabled(true);
lm's avatar
lm committed
140 141
    if (logLink)
    {
142 143 144 145 146
        logLink->disconnect();
        LinkManager::instance()->removeLink(logLink);
        delete logLink;
        logLink = NULL;
    }
147 148
}

149
bool QGCMAVLinkLogPlayer::reset(int packetIndex)
150
{
151
    // Reset only for valid values
lm's avatar
lm committed
152 153 154
    const unsigned int packetSize = timeLen + packetLen;
    if (packetIndex >= 0 && packetIndex*packetSize <= logFile.size() - packetSize)
    {
155 156 157 158
        bool result = true;
        pause();
        loopCounter = 0;
        logFile.reset();
lm's avatar
lm committed
159 160 161

        if (!logFile.seek(packetIndex*packetSize))
        {
162 163 164 165 166 167
            // Fallback: Start from scratch
            logFile.reset();
            ui->logStatsLabel->setText(tr("Changing packet index failed, back to start."));
            result = false;
        }

168
        ui->playButton->setIcon(QIcon(":files/images/actions/media-playback-start.svg"));
169
        ui->positionSlider->blockSignals(true);
lm's avatar
lm committed
170
        int sliderVal = (packetIndex / (double)(logFile.size()/packetSize)) * (ui->positionSlider->maximum() - ui->positionSlider->minimum());
171 172 173 174
        ui->positionSlider->setValue(sliderVal);
        ui->positionSlider->blockSignals(false);
        startTime = 0;
        return result;
lm's avatar
lm committed
175 176 177
    }
    else
    {
178 179
        return false;
    }
180 181
}

Lorenz Meier's avatar
Lorenz Meier committed
182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208
void QGCMAVLinkLogPlayer::loadSettings()
{
    QSettings settings;
    settings.beginGroup("QGC_MAVLINKLOGPLAYER");
    lastLogDirectory = settings.value("LAST_LOG_DIRECTORY", lastLogDirectory).toString();
    settings.endGroup();
}

void QGCMAVLinkLogPlayer::storeSettings()
{
    QSettings settings;
    settings.beginGroup("QGC_MAVLINKLOGPLAYER");
    settings.setValue("LAST_LOG_DIRECTORY", lastLogDirectory);
    settings.endGroup();
    settings.sync();
}

/**
 * @brief Select a log file
 * @param startDirectory Directory where the file dialog will be opened
 * @return filename of the logFile
 */
bool QGCMAVLinkLogPlayer::selectLogFile()
{
    return selectLogFile(lastLogDirectory);
}

209
/**
Lorenz Meier's avatar
Lorenz Meier committed
210
 * @brief Select a log file
211 212 213 214
 * @param startDirectory Directory where the file dialog will be opened
 * @return filename of the logFile
 */
bool QGCMAVLinkLogPlayer::selectLogFile(const QString startDirectory)
215
{
216
    QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name to replay"), startDirectory, tr("MAVLink or Binary Logfile (*.mavlink *.bin *.log)"));
217

218 219 220 221 222 223
    if (fileName == "")
    {
        return false;
    }
    else
    {
Lorenz Meier's avatar
Lorenz Meier committed
224
        lastLogDirectory = fileName;
225 226
        return loadLogFile(fileName);
    }
227 228
}

229 230 231 232 233 234 235 236
/**
 * @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
237 238
    if (f < 0.0f)
    {
239
        accelerationFactor = 1.0f / (-f/2.0f);
lm's avatar
lm committed
240 241 242
    }
    else
    {
243 244 245
        accelerationFactor = 1+(f/2.0f);
    }

246
    // Update timer interval
lm's avatar
lm committed
247 248
    if (!mavlinkLogFormat)
    {
249 250 251 252 253 254 255 256 257 258
        // 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();
        loopTimer.start(interval/accelerationFactor);
    }

259 260
    //qDebug() << "FACTOR:" << accelerationFactor;

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

264
bool QGCMAVLinkLogPlayer::loadLogFile(const QString& file)
265
{
Lorenz Meier's avatar
Lorenz Meier committed
266 267 268 269
    // Enable controls
    ui->playButton->setEnabled(true);
    ui->speedSlider->setEnabled(true);
    ui->positionSlider->setEnabled(true);
270 271 272
    ui->speedLabel->setEnabled(true);
    ui->logFileNameLabel->setEnabled(true);
    ui->logStatsLabel->setEnabled(true);
Lorenz Meier's avatar
Lorenz Meier committed
273

lm's avatar
lm committed
274
    // Check if logging is still enabled
lm's avatar
lm committed
275 276
    if (mavlink->loggingEnabled())
    {
lm's avatar
lm committed
277 278 279 280
        mavlink->enableLogging(false);
        MainWindow::instance()->showInfoMessage(tr("MAVLink Logging Stopped during Replay"), tr("MAVLink logging has been stopped during the log replay. To re-enable logging, use the link properties in the communication menu."));
    }

281
    // Ensure that the playback process is stopped
lm's avatar
lm committed
282 283
    if (logFile.isOpen())
    {
284
        pause();
285 286
        logFile.close();
    }
287 288
    logFile.setFileName(file);

lm's avatar
lm committed
289 290
    if (!logFile.open(QFile::ReadOnly))
    {
291
        MainWindow::instance()->showCriticalMessage(tr("The selected logfile is unreadable"), tr("Please make sure that the file %1 is readable or select a different file").arg(file));
292
        logFile.setFileName("");
293
        return false;
lm's avatar
lm committed
294 295 296
    }
    else
    {
297 298 299 300
        QFileInfo logFileInfo(file);
        logFile.reset();
        startTime = 0;
        ui->logFileNameLabel->setText(tr("%1").arg(logFileInfo.baseName()));
301

302 303 304
        // Select if binary or MAVLink log format is used
        mavlinkLogFormat = file.endsWith(".mavlink");

lm's avatar
lm committed
305 306
        if (mavlinkLogFormat)
        {
307 308
            // Get the time interval from the logfile
            QByteArray timestamp = logFile.read(timeLen);
309

310 311
            // First timestamp
            quint64 starttime = *((quint64*)(timestamp.constData()));
312

313 314 315 316 317 318
            // Last timestamp
            logFile.seek(logFile.size()-packetLen-timeLen);
            QByteArray timestamp2 = logFile.read(timeLen);
            quint64 endtime = *((quint64*)(timestamp2.constData()));
            // Reset everything
            logFile.reset();
319

320
            qDebug() << "Starttime:" << starttime << "End:" << endtime;
321

322 323 324 325 326 327
            // WARNING: Order matters in this computation
            int seconds = (endtime - starttime)/1000000;
            int minutes = seconds / 60;
            int hours = minutes / 60;
            seconds -= 60*minutes;
            minutes -= 60*hours;
328

329
            QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2);
330 331
            currPacketCount = logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64));
            ui->logStatsLabel->setText(tr("%2 MB, %3 packets, %4").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(currPacketCount).arg(timelabel));
lm's avatar
lm committed
332 333 334
        }
        else
        {
335 336 337 338
            // Load in binary mode

            // Set baud rate if any present
            QStringList parts = logFileInfo.baseName().split("_");
339

lm's avatar
lm committed
340 341
            if (parts.count() > 1)
            {
342 343 344
                bool ok;
                int rate = parts.last().toInt(&ok);
                // 9600 baud to 100 MBit
lm's avatar
lm committed
345 346
                if (ok && (rate > 9600 && rate < 100000000))
                {
347 348 349 350 351 352 353 354 355 356 357 358
                    // Accept this as valid baudrate
                    binaryBaudRate = rate;
                }
            }

            int seconds = logFileInfo.size() / (binaryBaudRate / 10);
            int minutes = seconds / 60;
            int hours = minutes / 60;
            seconds -= 60*minutes;
            minutes -= 60*hours;

            QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2);
359
            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));
360
        }
361 362 363 364

        // Reset current state
        reset(0);

365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380
        // Check if a serial link is connected

        bool linkWarning = false;
        foreach (LinkInterface* link, LinkManager::instance()->getLinks())
        {
            SerialLink* s = dynamic_cast<SerialLink*>(link);

            if (s && s->isConnected())
                linkWarning = true;
        }

        if (linkWarning)
            MainWindow::instance()->showInfoMessage(tr("Active MAVLink links found"), tr("Currently other links are connected. It is recommended to disconnect any active link before replaying a log."));

        play();

381
        return true;
382
    }
383 384
}

385 386 387 388
/**
 * Jumps to the current percentage of the position slider
 */
void QGCMAVLinkLogPlayer::jumpToSliderVal(int slidervalue)
389
{
390
    loopTimer.stop();
391 392 393 394 395 396
    // Set the logfile to the correct percentage and
    // align to the timestamp values
    int packetCount = logFile.size() / (packetLen + timeLen);
    int packetIndex = (packetCount - 1) * (slidervalue / (double)(ui->positionSlider->maximum() - ui->positionSlider->minimum()));

    // Do only accept valid jumps
lm's avatar
lm committed
397 398 399 400
    if (reset(packetIndex))
    {
        if (mavlinkLogFormat)
        {
401
            ui->logStatsLabel->setText(tr("Jumped to packet %1").arg(packetIndex));
402
        }
403
    }
404
}
405

406 407 408 409 410 411 412 413
/**
 * 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.
 */
414
void QGCMAVLinkLogPlayer::logLoop()
415
{
lm's avatar
lm committed
416 417
    if (mavlinkLogFormat)
    {
418
        bool ok;
419

420
        // First check initialization
lm's avatar
lm committed
421 422
        if (startTime == 0)
        {
423
            QByteArray startBytes = logFile.read(timeLen);
424

425
            // Check if the correct number of bytes could be read
lm's avatar
lm committed
426 427
            if (startBytes.length() != timeLen)
            {
428 429 430 431 432
                ui->logStatsLabel->setText(tr("Error reading first %1 bytes").arg(timeLen));
                MainWindow::instance()->showCriticalMessage(tr("Failed loading MAVLink Logfile"), tr("Error reading first %1 bytes from logfile. Got %2 instead of %1 bytes. Is the logfile readable?").arg(timeLen).arg(startBytes.length()));
                reset();
                return;
            }
433

434 435 436 437
            // Convert data to timestamp
            startTime = *((quint64*)(startBytes.constData()));
            currentStartTime = QGC::groundTimeUsecs();
            ok = true;
438

439
            //qDebug() << "START TIME: " << startTime;
440

441
            // Check if these bytes could be correctly decoded
442
            // TODO
lm's avatar
lm committed
443 444
            if (!ok)
            {
445 446 447 448 449 450
                ui->logStatsLabel->setText(tr("Error decoding first timestamp, aborting."));
                MainWindow::instance()->showCriticalMessage(tr("Failed loading MAVLink Logfile"), tr("Could not load initial timestamp from file %1. Is the file corrupted?").arg(logFile.fileName()));
                reset();
                return;
            }
        }
451 452


453
        // Initialization seems fine, load next chunk
454
        //this is ok because before we already read the timestamp of this paket before
455
        QByteArray chunk = logFile.read(timeLen+packetLen);
456
        QByteArray packet = chunk.left(packetLen);
457

458 459
        // Emit this packet
        emit bytesReady(logLink, packet);
460

461
        // Check if reached end of file before reading next timestamp
462
        if (chunk.length() < (timeLen + packetLen) || logFile.atEnd())
lm's avatar
lm committed
463
        {
464 465
            // Reached end of file
            reset();
466

467 468 469 470 471
            QString status = tr("Reached end of MAVLink log file.");
            ui->logStatsLabel->setText(status);
            MainWindow::instance()->showStatusMessage(status);
            return;
        }
472

473
        // End of file not reached, read next timestamp
474
        // which is located after current packet
475
        QByteArray rawTime = chunk.mid(packetLen);
476

477 478 479
        // This is the timestamp of the next packet
        quint64 time = *((quint64*)(rawTime.constData()));
        ok = true;
lm's avatar
lm committed
480 481
        if (!ok)
        {
482 483 484 485
            // Convert it to 64bit number
            QString status = tr("Time conversion error during log replay. Continuing..");
            ui->logStatsLabel->setText(status);
            MainWindow::instance()->showStatusMessage(status);
lm's avatar
lm committed
486 487 488
        }
        else
        {
489 490 491
            // Normal processing, passed all checks
            // start timer to match time offset between
            // this and next packet
492 493


494 495
            // Offset in ms
            qint64 timediff = (time - startTime)/accelerationFactor;
496

497 498
            // Immediately load any data within
            // a 3 ms interval
499

500
            int nextExecutionTime = (((qint64)currentStartTime + (qint64)timediff) - (qint64)QGC::groundTimeUsecs())/1000;
501

502
            //qDebug() << "nextExecutionTime:" << nextExecutionTime << "QGC START TIME:" << currentStartTime << "LOG START TIME:" << startTime;
503

lm's avatar
lm committed
504 505
            if (nextExecutionTime < 2)
            {
506
                logLoop();
lm's avatar
lm committed
507 508 509
            }
            else
            {
510 511
                loopTimer.start(nextExecutionTime);
            }
512
        }
lm's avatar
lm committed
513 514 515
    }
    else
    {
516 517 518 519 520 521 522 523
        // 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
524 525
        if (chunk.length() < len || logFile.atEnd())
        {
526 527 528 529 530 531 532 533
            // Reached end of file
            reset();

            QString status = tr("Reached end of binary log file.");
            ui->logStatsLabel->setText(status);
            MainWindow::instance()->showStatusMessage(status);
            return;
        }
534 535 536
    }
    // Ui update: Only every 20 messages
    // to prevent flickering and high CPU load
537

538 539
    // Update status label
    // Update progress bar
540
    if (loopCounter % 40 == 0 || currPacketCount < 500)
lm's avatar
lm committed
541
    {
542 543 544 545 546 547
        QFileInfo logFileInfo(logFile);
        int progress = (ui->positionSlider->maximum()-ui->positionSlider->minimum())*(logFile.pos()/static_cast<float>(logFileInfo.size()));
        //qDebug() << "Progress:" << progress;
        ui->positionSlider->blockSignals(true);
        ui->positionSlider->setValue(progress);
        ui->positionSlider->blockSignals(false);
548
    }
549
    loopCounter++;
550
}
551 552 553 554

void QGCMAVLinkLogPlayer::changeEvent(QEvent *e)
{
    QWidget::changeEvent(e);
lm's avatar
lm committed
555 556
    switch (e->type())
    {
557 558 559 560 561 562 563
    case QEvent::LanguageChange:
        ui->retranslateUi(this);
        break;
    default:
        break;
    }
}