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

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

QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *parent) :
11 12 13 14 15 16 17 18 19 20 21 22
    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
23
    isPlaying(false),
24
    currPacketCount(0),
25
    ui(new Ui::QGCMAVLinkLogPlayer)
26 27 28 29
{
    ui->setupUi(this);
    ui->gridLayout->setAlignment(Qt::AlignTop);

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

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

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

43 44
    setAccelerationFactorInt(49);
    ui->speedSlider->setValue(49);
45
    ui->positionSlider->setValue(ui->positionSlider->minimum());
46 47 48 49 50 51 52
}

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

lm's avatar
lm committed
53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76
void QGCMAVLinkLogPlayer::playPause(bool enabled)
{
    if (enabled)
    {
        play();
    }
    else
    {
        pause();
    }
}

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

77 78
void QGCMAVLinkLogPlayer::play()
{
lm's avatar
lm committed
79 80
    if (logFile.isOpen())
    {
81
        ui->selectFileButton->setEnabled(false);
lm's avatar
lm committed
82 83
        if (logLink)
        {
84 85
            logLink->disconnect();
            LinkManager::instance()->removeLink(logLink);
86 87 88 89 90
            delete logLink;
        }
        logLink = new MAVLinkSimulationLink("");

        // Start timer
lm's avatar
lm committed
91 92
        if (mavlinkLogFormat)
        {
93
            loopTimer.start(1);
lm's avatar
lm committed
94 95 96
        }
        else
        {
97 98 99 100 101 102
            // 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);
103
            loopTimer.start(interval*accelerationFactor);
104
        }
lm's avatar
lm committed
105
        isPlaying = true;
106
        ui->logStatsLabel->setText(tr("Started playing.."));
107
        ui->playButton->setIcon(QIcon(":files/images/actions/media-playback-pause.svg"));
lm's avatar
lm committed
108 109 110
    }
    else
    {
111 112 113 114 115 116 117 118 119 120 121 122 123
        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
124
    isPlaying = false;
125
    loopTimer.stop();
126
    ui->playButton->setIcon(QIcon(":files/images/actions/media-playback-start.svg"));
127
    ui->selectFileButton->setEnabled(true);
lm's avatar
lm committed
128 129
    if (logLink)
    {
130 131 132 133 134
        logLink->disconnect();
        LinkManager::instance()->removeLink(logLink);
        delete logLink;
        logLink = NULL;
    }
135 136
}

137
bool QGCMAVLinkLogPlayer::reset(int packetIndex)
138
{
139
    // Reset only for valid values
lm's avatar
lm committed
140 141 142
    const unsigned int packetSize = timeLen + packetLen;
    if (packetIndex >= 0 && packetIndex*packetSize <= logFile.size() - packetSize)
    {
143 144 145 146
        bool result = true;
        pause();
        loopCounter = 0;
        logFile.reset();
lm's avatar
lm committed
147 148 149

        if (!logFile.seek(packetIndex*packetSize))
        {
150 151 152 153 154 155
            // Fallback: Start from scratch
            logFile.reset();
            ui->logStatsLabel->setText(tr("Changing packet index failed, back to start."));
            result = false;
        }

156
        ui->playButton->setIcon(QIcon(":files/images/actions/media-playback-start.svg"));
157
        ui->positionSlider->blockSignals(true);
lm's avatar
lm committed
158
        int sliderVal = (packetIndex / (double)(logFile.size()/packetSize)) * (ui->positionSlider->maximum() - ui->positionSlider->minimum());
159 160 161 162
        ui->positionSlider->setValue(sliderVal);
        ui->positionSlider->blockSignals(false);
        startTime = 0;
        return result;
lm's avatar
lm committed
163 164 165
    }
    else
    {
166 167
        return false;
    }
168 169
}

170
bool QGCMAVLinkLogPlayer::selectLogFile()
171
{
172
    QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name to replay"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink or Binary Logfile (*.mavlink *.bin *.log)"));
173

174 175 176 177 178 179 180 181
    if (fileName == "")
    {
        return false;
    }
    else
    {
        return loadLogFile(fileName);
    }
182 183
}

184 185 186 187 188 189 190 191
/**
 * @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
192 193
    if (f < 0.0f)
    {
194
        accelerationFactor = 1.0f / (-f/2.0f);
lm's avatar
lm committed
195 196 197
    }
    else
    {
198 199 200
        accelerationFactor = 1+(f/2.0f);
    }

201
    // Update timer interval
lm's avatar
lm committed
202 203
    if (!mavlinkLogFormat)
    {
204 205 206 207 208 209 210 211 212 213
        // 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);
    }

214 215
    //qDebug() << "FACTOR:" << accelerationFactor;

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

219
bool QGCMAVLinkLogPlayer::loadLogFile(const QString& file)
220
{
lm's avatar
lm committed
221
    // Check if logging is still enabled
lm's avatar
lm committed
222 223
    if (mavlink->loggingEnabled())
    {
lm's avatar
lm committed
224 225 226 227
        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."));
    }

228
    // Ensure that the playback process is stopped
lm's avatar
lm committed
229 230
    if (logFile.isOpen())
    {
231
        pause();
232 233
        logFile.close();
    }
234 235
    logFile.setFileName(file);

lm's avatar
lm committed
236 237
    if (!logFile.open(QFile::ReadOnly))
    {
238
        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));
239
        logFile.setFileName("");
240
        return false;
lm's avatar
lm committed
241 242 243
    }
    else
    {
244 245 246 247
        QFileInfo logFileInfo(file);
        logFile.reset();
        startTime = 0;
        ui->logFileNameLabel->setText(tr("%1").arg(logFileInfo.baseName()));
248

249 250 251
        // Select if binary or MAVLink log format is used
        mavlinkLogFormat = file.endsWith(".mavlink");

lm's avatar
lm committed
252 253
        if (mavlinkLogFormat)
        {
254 255
            // Get the time interval from the logfile
            QByteArray timestamp = logFile.read(timeLen);
256

257 258
            // First timestamp
            quint64 starttime = *((quint64*)(timestamp.constData()));
259

260 261 262 263 264 265
            // Last timestamp
            logFile.seek(logFile.size()-packetLen-timeLen);
            QByteArray timestamp2 = logFile.read(timeLen);
            quint64 endtime = *((quint64*)(timestamp2.constData()));
            // Reset everything
            logFile.reset();
266

267
            qDebug() << "Starttime:" << starttime << "End:" << endtime;
268

269 270 271 272 273 274
            // 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;
275

276
            QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2);
277 278
            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
279 280 281
        }
        else
        {
282 283 284 285
            // Load in binary mode

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

lm's avatar
lm committed
287 288
            if (parts.count() > 1)
            {
289 290 291
                bool ok;
                int rate = parts.last().toInt(&ok);
                // 9600 baud to 100 MBit
lm's avatar
lm committed
292 293
                if (ok && (rate > 9600 && rate < 100000000))
                {
294 295 296 297 298 299 300 301 302 303 304 305
                    // 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);
306
            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));
307
        }
308 309 310 311

        // Reset current state
        reset(0);

312
        return true;
313
    }
314 315
}

316 317 318 319
/**
 * Jumps to the current percentage of the position slider
 */
void QGCMAVLinkLogPlayer::jumpToSliderVal(int slidervalue)
320
{
321
    loopTimer.stop();
322 323 324 325 326 327
    // 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
328 329 330 331
    if (reset(packetIndex))
    {
        if (mavlinkLogFormat)
        {
332
            ui->logStatsLabel->setText(tr("Jumped to packet %1").arg(packetIndex));
333
        }
334
    }
335
}
336

337 338 339 340 341 342 343 344
/**
 * 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.
 */
345
void QGCMAVLinkLogPlayer::logLoop()
346
{
lm's avatar
lm committed
347 348
    if (mavlinkLogFormat)
    {
349
        bool ok;
350

351
        // First check initialization
lm's avatar
lm committed
352 353
        if (startTime == 0)
        {
354
            QByteArray startBytes = logFile.read(timeLen);
355

356
            // Check if the correct number of bytes could be read
lm's avatar
lm committed
357 358
            if (startBytes.length() != timeLen)
            {
359 360 361 362 363
                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;
            }
364

365 366 367 368
            // Convert data to timestamp
            startTime = *((quint64*)(startBytes.constData()));
            currentStartTime = QGC::groundTimeUsecs();
            ok = true;
369

370
            //qDebug() << "START TIME: " << startTime;
371

372
            // Check if these bytes could be correctly decoded
lm's avatar
lm committed
373 374
            if (!ok)
            {
375 376 377 378 379 380
                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;
            }
        }
381 382


383 384 385
        // Initialization seems fine, load next chunk
        QByteArray chunk = logFile.read(timeLen+packetLen);
        QByteArray packet = chunk.mid(0, packetLen);
386

387 388
        // Emit this packet
        emit bytesReady(logLink, packet);
389

390
        // Check if reached end of file before reading next timestamp
lm's avatar
lm committed
391 392
        if (chunk.length() < timeLen+packetLen || logFile.atEnd())
        {
393 394
            // Reached end of file
            reset();
395

396 397 398 399 400
            QString status = tr("Reached end of MAVLink log file.");
            ui->logStatsLabel->setText(status);
            MainWindow::instance()->showStatusMessage(status);
            return;
        }
401

402 403
        // End of file not reached, read next timestamp
        QByteArray rawTime = chunk.mid(packetLen);
404

405 406 407
        // This is the timestamp of the next packet
        quint64 time = *((quint64*)(rawTime.constData()));
        ok = true;
lm's avatar
lm committed
408 409
        if (!ok)
        {
410 411 412 413
            // 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
414 415 416
        }
        else
        {
417 418 419
            // Normal processing, passed all checks
            // start timer to match time offset between
            // this and next packet
420 421


422 423
            // Offset in ms
            qint64 timediff = (time - startTime)/accelerationFactor;
424

425 426
            // Immediately load any data within
            // a 3 ms interval
427

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

430
            //qDebug() << "nextExecutionTime:" << nextExecutionTime << "QGC START TIME:" << currentStartTime << "LOG START TIME:" << startTime;
431

lm's avatar
lm committed
432 433
            if (nextExecutionTime < 2)
            {
434
                logLoop();
lm's avatar
lm committed
435 436 437
            }
            else
            {
438 439
                loopTimer.start(nextExecutionTime);
            }
440
        }
lm's avatar
lm committed
441 442 443
    }
    else
    {
444 445 446 447 448 449 450 451
        // 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
452 453
        if (chunk.length() < len || logFile.atEnd())
        {
454 455 456 457 458 459 460 461
            // Reached end of file
            reset();

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

466 467
    // Update status label
    // Update progress bar
468
    if (loopCounter % 40 == 0 || currPacketCount < 500)
lm's avatar
lm committed
469
    {
470 471 472 473 474 475
        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);
476
    }
477
    loopCounter++;
478
}
479 480 481 482

void QGCMAVLinkLogPlayer::changeEvent(QEvent *e)
{
    QWidget::changeEvent(e);
lm's avatar
lm committed
483 484
    switch (e->type())
    {
485 486 487 488 489 490 491
    case QEvent::LanguageChange:
        ui->retranslateUi(this);
        break;
    default:
        break;
    }
}