Newer
Older
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "LogReplayLink.h"
#include "LinkManager.h"
const char* LogReplayLinkConfiguration::_logFilenameKey = "logFilename";
LogReplayLinkConfiguration::LogReplayLinkConfiguration(const QString& name)
LogReplayLinkConfiguration::LogReplayLinkConfiguration(LogReplayLinkConfiguration* copy)
{
_logFilename = copy->logFilename();
}
void LogReplayLinkConfiguration::copyFrom(LinkConfiguration *source)
{
LinkConfiguration::copyFrom(source);
auto* ssource = qobject_cast<LogReplayLinkConfiguration*>(source);
if (ssource) {
_logFilename = ssource->logFilename();
} else {
qWarning() << "Internal error";
}
}
void LogReplayLinkConfiguration::saveSettings(QSettings& settings, const QString& root)
{
settings.beginGroup(root);
settings.setValue(_logFilenameKey, _logFilename);
settings.endGroup();
}
void LogReplayLinkConfiguration::loadSettings(QSettings& settings, const QString& root)
{
settings.beginGroup(root);
_logFilename = settings.value(_logFilenameKey, "").toString();
settings.endGroup();
}
void LogReplayLinkConfiguration::updateSettings(void)
{
// Doesn't support changing filename on the fly is already connected
}
QString LogReplayLinkConfiguration::logFilenameShort(void)
{
QFileInfo fi(_logFilename);
return fi.fileName();
}
LogReplayLink::LogReplayLink(SharedLinkConfigurationPointer& config)
: LinkInterface (config)
, _logReplayConfig (qobject_cast<LogReplayLinkConfiguration*>(config.data()))
, _connected (false)
, _playbackSpeed (1)
if (!_logReplayConfig) {
qWarning() << "Internal error";
}
QObject::connect(&_readTickTimer, &QTimer::timeout, this, &LogReplayLink::_readNextLogEntry);
QObject::connect(this, &LogReplayLink::_playOnThread, this, &LogReplayLink::_play);
QObject::connect(this, &LogReplayLink::_pauseOnThread, this, &LogReplayLink::_pause);
QObject::connect(this, &LogReplayLink::_setPlaybackSpeedOnThread, this, &LogReplayLink::_setPlaybackSpeed);
moveToThread(this);
}
LogReplayLink::~LogReplayLink(void)
{
_disconnect();
}
bool LogReplayLink::_connect(void)
{
// Disallow replay when any links are connected
if (qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()) {
emit communicationError(_errorTitle, tr("You must close all connections prior to replaying a log."));
_mavlinkChannel = qgcApp()->toolbox()->linkManager()->_reserveMavlinkChannel();
if (_mavlinkChannel == 0) {
qWarning() << "No mavlink channels available";
return false;
}
if (isRunning()) {
quit();
wait();
}
start(HighPriority);
return true;
}
{
if (_connected) {
quit();
wait();
_connected = false;
if (_mavlinkChannel != 0) {
qgcApp()->toolbox()->linkManager()->_freeMavlinkChannel(_mavlinkChannel);
}
emit disconnected();
}
}
void LogReplayLink::run(void)
{
// Load the log file
if (!_loadLogFile()) {
return;
}
_connected = true;
emit connected();
// Start playback
_play();
// Run normal event loop until exit
exec();
}
void LogReplayLink::_replayError(const QString& errorMsg)
{
qDebug() << _errorTitle << errorMsg;
emit communicationError(_errorTitle, errorMsg);
}
/// Since this is log replay, we just drops writes on the floor
void LogReplayLink::_writeBytes(const QByteArray bytes)
{
Q_UNUSED(bytes);
}
/// Parses a BigEndian quint64 timestamp
/// @return A Unix timestamp in microseconds UTC for found message or 0 if parsing failed
quint64 LogReplayLink::_parseTimestamp(const QByteArray& bytes)
{
quint64 timestamp = qFromBigEndian(*((quint64*)(bytes.constData())));
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) {
180
181
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
209
/// Reads the next mavlink message from the log
/// @param bytes[output] Bytes for mavlink message
/// @return Unix timestamp in microseconds UTC for NEXT mavlink message or 0 if no message found
quint64 LogReplayLink::_readNextMavlinkMessage(QByteArray& bytes)
{
char nextByte;
mavlink_status_t status;
bytes.clear();
while (_logFile.getChar(&nextByte)) { // Loop over every byte
mavlink_message_t message;
bool messageFound = mavlink_parse_char(_mavlinkChannel, nextByte, &message, &status);
if (status.parse_state == MAVLINK_PARSE_STATE_GOT_STX) {
// This is the possible beginning of a mavlink message, clear any partial bytes
bytes.clear();
}
bytes.append(nextByte);
if (messageFound) {
// Return the timestamp for the next message
QByteArray rawTime = _logFile.read(cbTimestamp);
return _parseTimestamp(rawTime);
}
}
return 0;
}
/// Seeks to the beginning of the next successfully parsed mavlink message in the log file.
/// @param nextMsg[output] Parsed next message that was found
/// @return A Unix timestamp in microseconds UTC for found message or 0 if parsing failed
quint64 LogReplayLink::_seekToNextMavlinkMessage(mavlink_message_t* nextMsg)
{
char nextByte;
mavlink_status_t status;
qint64 messageStartPos = -1;
mavlink_reset_channel_status(_mavlinkChannel);
while (_logFile.getChar(&nextByte)) {
bool messageFound = mavlink_parse_char(_mavlinkChannel, nextByte, nextMsg, &status);
if (status.parse_state == MAVLINK_PARSE_STATE_GOT_STX) {
// This is the possible beginning of a mavlink message
messageStartPos = _logFile.pos() - 1;
}
// 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 && messageStartPos != -1) {
_logFile.seek(messageStartPos - cbTimestamp);
QByteArray rawTime = _logFile.read(cbTimestamp);
return _parseTimestamp(rawTime);
}
}
return 0;
}
quint64 LogReplayLink::_findLastTimestamp(void)
{
char nextByte;
mavlink_status_t status;
quint64 lastTimestamp = 0;
mavlink_message_t msg;
// We read through the entire file looking for the last good timestamp. This can be somewhat slow, but trying to work from the
// end of the file can be way slower due to all the seeking back and forth required. So instead we take the simple reliable approach.
_logFile.reset();
mavlink_reset_channel_status(_mavlinkChannel);
while (_logFile.bytesAvailable() > cbTimestamp) {
lastTimestamp = _parseTimestamp(_logFile.read(cbTimestamp));
bool endOfMessage = false;
while (!endOfMessage && _logFile.getChar(&nextByte)) {
endOfMessage = mavlink_parse_char(_mavlinkChannel, nextByte, &msg, &status);
}
}
return lastTimestamp;
}
bool LogReplayLink::_loadLogFile(void)
{
QString errorMsg;
QString logFilename = _logReplayConfig->logFilename();
QFileInfo logFileInfo;
int logDurationSecondsTotal;
errorMsg = tr("Attempt to load new log while log being played");
goto Error;
}
_logFile.setFileName(logFilename);
if (!_logFile.open(QFile::ReadOnly)) {
errorMsg = tr("Unable to open log file: '%1', error: %2").arg(logFilename).arg(_logFile.errorString());
goto Error;
}
logFileInfo.setFile(logFilename);
_logFileSize = logFileInfo.size();
startTimeUSecs = _parseTimestamp(_logFile.read(cbTimestamp));
endTimeUSecs = _findLastTimestamp();
if (endTimeUSecs <= startTimeUSecs) {
errorMsg = tr("The log file '%1' is corrupt or empty.").arg(logFilename);
goto Error;
// Remember the start and end time so we can move around this _logFile with the slider.
_logEndTimeUSecs = endTimeUSecs;
_logStartTimeUSecs = startTimeUSecs;
_logDurationUSecs = endTimeUSecs - startTimeUSecs;
_logCurrentTimeUSecs = startTimeUSecs;
// Reset our log file so when we go to read it for the first time, we start at the beginning.
_logFile.reset();
logDurationSecondsTotal = (_logDurationUSecs) / 1000000;
return true;
Error:
if (_logFile.isOpen()) {
_logFile.close();
}
_replayError(errorMsg);
return false;
}
/// This function will read the next available log entry. It will then start
/// the _readTickTimer timer to read the new log entry at the appropriate 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.
void LogReplayLink::_readNextLogEntry(void)
{
// Now parse MAVLink messages, grabbing their timestamps as we go. We stop once we
// 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.
int timeToNextExecutionMSecs = 0;
while (timeToNextExecutionMSecs < 3) {
// Read the next mavlink message from the log
qint64 nextTimeUSecs = _readNextMavlinkMessage(bytes);
emit bytesReceived(this, bytes);
emit playbackPercentCompleteChanged(((float)(_logCurrentTimeUSecs - _logStartTimeUSecs) / (float)_logDurationUSecs) * 100);
if (_logFile.atEnd()) {
_logCurrentTimeUSecs = nextTimeUSecs;
// 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())
quint64 currentTimeMSecs = (quint64)QDateTime::currentMSecsSinceEpoch();
quint64 desiredPlayheadMovementTimeMSecs = ((_logCurrentTimeUSecs - _playbackStartLogTimeUSecs) / 1000) / _playbackSpeed;
quint64 desiredCurrentTimeMSecs = _playbackStartTimeMSecs + desiredPlayheadMovementTimeMSecs;
timeToNextExecutionMSecs = desiredCurrentTimeMSecs - currentTimeMSecs;
_signalCurrentLogTimeSecs();
// And schedule the next execution of this function.
_readTickTimer.start(timeToNextExecutionMSecs);
qgcApp()->toolbox()->linkManager()->setConnectionsSuspended(tr("Connect not allowed during Flight Data replay."));
qgcApp()->toolbox()->mavlinkProtocol()->suspendLogForReplay(true);
// 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()) {
_resetPlaybackToBeginning();
}
_playbackStartTimeMSecs = (quint64)QDateTime::currentMSecsSinceEpoch();
_playbackStartLogTimeUSecs = _logCurrentTimeUSecs;
_readTickTimer.start(1);
emit playbackStarted();
}
void LogReplayLink::_pause(void)
{
qgcApp()->toolbox()->linkManager()->setConnectionsAllowed();
qgcApp()->toolbox()->mavlinkProtocol()->suspendLogForReplay(false);
_readTickTimer.stop();
emit playbackPaused();
}
void LogReplayLink::_resetPlaybackToBeginning(void)
{
if (_logFile.isOpen()) {
_logFile.reset();
}
// And since we haven't starting playback, clear the time of initial playback and the current timestamp.
_playbackStartTimeMSecs = 0;
_logCurrentTimeUSecs = _logStartTimeUSecs;
}
if (percentComplete < 0) {
percentComplete = 0;
}
if (percentComplete > 100) {
percentComplete = 100;
// 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)(percentCompleteMult * (qreal)_logFile.size());
// Now seek to the appropriate position, failing gracefully if we can't.
if (!_logFile.seek(newFilePos)) {
_replayError(tr("Unable to seek to new position"));
return;
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
// But we do align to the next MAVLink message for consistency.
mavlink_message_t dummy;
_logCurrentTimeUSecs = _seekToNextMavlinkMessage(&dummy);
// Now calculate the current file location based on time.
qreal newRelativeTimeUSecs = (qreal)(_logCurrentTimeUSecs - _logStartTimeUSecs);
// Calculate the effective baud rate of the file in bytes/s.
qreal baudRate = _logFile.size() / (qreal)_logDurationUSecs / 1e6;
// And the desired time is:
qreal desiredTimeUSecs = percentCompleteMult * _logDurationUSecs;
// And now jump the necessary number of bytes in the proper direction
qint64 offset = (newRelativeTimeUSecs - desiredTimeUSecs) * baudRate;
if (!_logFile.seek(_logFile.pos() + offset)) {
_replayError(tr("Unable to seek to new position"));
return;
}
// And scan until we reach the start of a MAVLink message. We make sure to record this timestamp for
// smooth jumping around the file.
_logCurrentTimeUSecs = _seekToNextMavlinkMessage(&dummy);
_signalCurrentLogTimeSecs();
// Now update the UI with our actual final position.
newRelativeTimeUSecs = (qreal)(_logCurrentTimeUSecs - _logStartTimeUSecs);
percentComplete = (newRelativeTimeUSecs / _logDurationUSecs) * 100;
emit playbackPercentCompleteChanged(percentComplete);
// Let _readNextLogEntry update to correct speed
_playbackStartTimeMSecs = (quint64)QDateTime::currentMSecsSinceEpoch();
_playbackStartLogTimeUSecs = _logCurrentTimeUSecs;
_readTickTimer.start(1);
}
/// @brief Called when playback is complete
void LogReplayLink::_finishPlayback(void)
{
_pause();
emit playbackAtEnd();
}
emit currentLogTimeSecs((_logCurrentTimeUSecs - _logStartTimeUSecs) / 1000000);
LogReplayLinkController::LogReplayLinkController(void)
: _link (nullptr)
, _isPlaying (false)
, _percentComplete (0)
, _playheadSecs (0)
}
void LogReplayLinkController::setLink(LogReplayLink* link)
{
if (_link) {
disconnect(_link);
disconnect(this, &LogReplayLinkController::playbackSpeedChanged, _link, &LogReplayLink::setPlaybackSpeed);
_isPlaying = false;
_percentComplete = 0;
_playheadTime.clear();
_totalTime.clear();
_link = nullptr;
emit isPlayingChanged(false);
emit percentCompleteChanged(0);
emit playheadTimeChanged(QString());
emit totalTimeChanged(QString());
emit linkChanged(nullptr);
}
if (link) {
_link = link;
connect(_link, &LogReplayLink::logFileStats, this, &LogReplayLinkController::_logFileStats);
connect(_link, &LogReplayLink::playbackStarted, this, &LogReplayLinkController::_playbackStarted);
connect(_link, &LogReplayLink::playbackPaused, this, &LogReplayLinkController::_playbackPaused);
connect(_link, &LogReplayLink::playbackPercentCompleteChanged, this, &LogReplayLinkController::_playbackPercentCompleteChanged);
connect(_link, &LogReplayLink::currentLogTimeSecs, this, &LogReplayLinkController::_currentLogTimeSecs);
connect(_link, &LogReplayLink::disconnected, this, &LogReplayLinkController::_linkDisconnected);
connect(this, &LogReplayLinkController::playbackSpeedChanged, _link, &LogReplayLink::setPlaybackSpeed);
emit linkChanged(_link);
}
}
void LogReplayLinkController::setIsPlaying(bool isPlaying)
{
if (isPlaying) {
_link->play();
} else {
_link->pause();
}
}
void LogReplayLinkController::setPercentComplete(qreal percentComplete)
{
_link->movePlayhead(percentComplete);
}
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
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
590
591
592
593
594
595
596
597
598
{
_totalTime = _secondsToHMS(logDurationSecs);
emit totalTimeChanged(_totalTime);
}
void LogReplayLinkController::_playbackStarted(void)
{
_isPlaying = true;
emit isPlayingChanged(true);
}
void LogReplayLinkController::_playbackPaused(void)
{
_isPlaying = false;
emit isPlayingChanged(true);
}
void LogReplayLinkController::_playbackAtEnd(void)
{
_isPlaying = false;
emit isPlayingChanged(true);
}
void LogReplayLinkController::_playbackPercentCompleteChanged(qreal percentComplete)
{
_percentComplete = percentComplete;
emit percentCompleteChanged(_percentComplete);
}
void LogReplayLinkController::_currentLogTimeSecs(int secs)
{
if (_playheadSecs != secs) {
_playheadSecs = secs;
_playheadTime = _secondsToHMS(secs);
emit playheadTimeChanged(_playheadTime);
}
}
void LogReplayLinkController::_linkDisconnected(void)
{
setLink(nullptr);
}
QString LogReplayLinkController::_secondsToHMS(int seconds)
{
int secondsPart = seconds;
int minutesPart = secondsPart / 60;
int hoursPart = minutesPart / 60;
secondsPart -= 60 * minutesPart;
minutesPart -= 60 * hoursPart;
if (hoursPart == 0) {
return tr("%2m:%3s").arg(minutesPart, 2, 10, QLatin1Char('0')).arg(secondsPart, 2, 10, QLatin1Char('0'));
} else {
return tr("%1h:%2m:%3s").arg(hoursPart, 2, 10, QLatin1Char('0')).arg(minutesPart, 2, 10, QLatin1Char('0')).arg(secondsPart, 2, 10, QLatin1Char('0'));
}