diff --git a/src/Comms/CMakeLists.txt b/src/Comms/CMakeLists.txt index 0f33c5516ce..d647405df33 100644 --- a/src/Comms/CMakeLists.txt +++ b/src/Comms/CMakeLists.txt @@ -1,7 +1,7 @@ add_subdirectory(AirLink) add_subdirectory(MockLink) -find_package(Qt6 REQUIRED COMPONENTS Core Network Qml Test Widgets) +find_package(Qt6 REQUIRED COMPONENTS Core Network Qml) qt_add_library(Comms STATIC LinkConfiguration.cc @@ -12,6 +12,8 @@ qt_add_library(Comms STATIC LinkManager.h LogReplayLink.cc LogReplayLink.h + LogReplayLinkController.cc + LogReplayLinkController.h MAVLinkProtocol.cc MAVLinkProtocol.h TCPLink.cc @@ -23,7 +25,6 @@ qt_add_library(Comms STATIC target_link_libraries(Comms PRIVATE Qt6::Qml - Qt6::Test MockLink PositionManager QGC diff --git a/src/Comms/LinkConfiguration.cc b/src/Comms/LinkConfiguration.cc index f4ea48e122b..4604aa68769 100644 --- a/src/Comms/LinkConfiguration.cc +++ b/src/Comms/LinkConfiguration.cc @@ -82,7 +82,7 @@ LinkConfiguration *LinkConfiguration::createSettings(int type, const QString &na break; #endif case TypeLogReplay: - config = new LogReplayLinkConfiguration(name); + config = new LogReplayConfiguration(name); break; #ifdef QT_DEBUG case TypeMock: @@ -124,7 +124,7 @@ LinkConfiguration *LinkConfiguration::duplicateSettings(const LinkConfiguration break; #endif case TypeLogReplay: - dupe = new LogReplayLinkConfiguration(qobject_cast(source)); + dupe = new LogReplayConfiguration(qobject_cast(source)); break; #ifdef QT_DEBUG case TypeMock: diff --git a/src/Comms/LinkInterface.h b/src/Comms/LinkInterface.h index 340db7a44ee..7f2ea148dc7 100644 --- a/src/Comms/LinkInterface.h +++ b/src/Comms/LinkInterface.h @@ -31,7 +31,7 @@ class LinkInterface : public QThread Q_INVOKABLE virtual void disconnect() = 0; // FIXME: This gets called 3x when closing link virtual bool isConnected() const = 0; - virtual bool isLogReplay() { return false; } + virtual bool isLogReplay() const { return false; } virtual bool isSecureConnection() { return false; } ///< Returns true if the connection is secure (e.g. USB, wired ethernet) SharedLinkConfigurationPtr linkConfiguration() { return _config; } diff --git a/src/Comms/LinkManager.cc b/src/Comms/LinkManager.cc index 78b2782dffa..cec03d36e5d 100644 --- a/src/Comms/LinkManager.cc +++ b/src/Comms/LinkManager.cc @@ -9,7 +9,7 @@ #include "LinkManager.h" #include "DeviceInfo.h" -#include "LogReplayLink.h" +#include "LogReplayLinkController.h" #include "MAVLinkProtocol.h" #include "MultiVehicleManager.h" #include "QGCApplication.h" @@ -346,7 +346,7 @@ void LinkManager::loadLinkConfigurationList() break; #endif case LinkConfiguration::TypeLogReplay: - link = new LogReplayLinkConfiguration(name); + link = new LogReplayConfiguration(name); break; #ifdef QT_DEBUG case LinkConfiguration::TypeMock: @@ -705,7 +705,7 @@ void LinkManager::freeMavlinkChannel(uint8_t channel) LogReplayLink *LinkManager::startLogReplay(const QString &logFile) { - LogReplayLinkConfiguration* const linkConfig = new LogReplayLinkConfiguration(tr("Log Replay")); + LogReplayConfiguration* const linkConfig = new LogReplayConfiguration(tr("Log Replay")); linkConfig->setLogFilename(logFile); linkConfig->setName(linkConfig->logFilenameShort()); diff --git a/src/Comms/LogReplayLink.cc b/src/Comms/LogReplayLink.cc index 4ce220caabb..63766a3cefa 100644 --- a/src/Comms/LogReplayLink.cc +++ b/src/Comms/LogReplayLink.cc @@ -7,594 +7,474 @@ * ****************************************************************************/ - #include "LogReplayLink.h" #include "LinkManager.h" -#include "QGCApplication.h" -#include "MultiVehicleManager.h" -#ifndef __mobile__ #include "MAVLinkProtocol.h" -#endif -#include "MAVLinkLib.h" +#include "MultiVehicleManager.h" +#include "QGCLoggingCategory.h" #include #include -#include +#include + +QGC_LOGGING_CATEGORY(LogReplayLinkLog, "qgc.comms.logreplaylink") + +/*===========================================================================*/ + +LogReplayConfiguration::LogReplayConfiguration(const QString &name, QObject *parent) + : LinkConfiguration(name, parent) +{ + // qCDebug(LogReplayLinkLog) << Q_FUNC_INFO << this; +} -LogReplayLinkConfiguration::LogReplayLinkConfiguration(const QString& name) - : LinkConfiguration(name) +LogReplayConfiguration::LogReplayConfiguration(const LogReplayConfiguration *copy, QObject *parent) + : LinkConfiguration(copy, parent) + , _logFilename(copy->logFilename()) { - + // qCDebug(LogReplayLinkLog) << Q_FUNC_INFO << this; } -LogReplayLinkConfiguration::LogReplayLinkConfiguration(const LogReplayLinkConfiguration* copy) - : LinkConfiguration(copy) +LogReplayConfiguration::~LogReplayConfiguration() { - _logFilename = copy->logFilename(); + // qCDebug(LogReplayLinkLog) << Q_FUNC_INFO << this; } -void LogReplayLinkConfiguration::copyFrom(const LinkConfiguration *source) +void LogReplayConfiguration::copyFrom(const LinkConfiguration *source) { + Q_ASSERT(source); LinkConfiguration::copyFrom(source); - const LogReplayLinkConfiguration* ssource = qobject_cast(source); - if (ssource) { - _logFilename = ssource->logFilename(); - } else { - qWarning() << "Internal error"; - } + + const LogReplayConfiguration *const logReplaySource = qobject_cast(source); + Q_ASSERT(logReplaySource); + + setLogFilename(logReplaySource->logFilename()); } -void LogReplayLinkConfiguration::saveSettings(QSettings& settings, const QString& root) +void LogReplayConfiguration::loadSettings(QSettings &settings, const QString &root) { settings.beginGroup(root); - settings.setValue(_logFilenameKey, _logFilename); + + setLogFilename(settings.value("logFilename", "").toString()); + settings.endGroup(); } -void LogReplayLinkConfiguration::loadSettings(QSettings& settings, const QString& root) +void LogReplayConfiguration::saveSettings(QSettings &settings, const QString &root) { settings.beginGroup(root); - _logFilename = settings.value(_logFilenameKey, "").toString(); + + settings.setValue("logFilename", _logFilename); + settings.endGroup(); } -QString LogReplayLinkConfiguration::logFilenameShort(void) +QString LogReplayConfiguration::logFilenameShort() const { - QFileInfo fi(_logFilename); - return fi.fileName(); + return QFileInfo(_logFilename).fileName(); } -LogReplayLink::LogReplayLink(SharedLinkConfigurationPtr& config) - : LinkInterface (config) - , _logReplayConfig (qobject_cast(config.get())) - , _connected (false) - , _mavlinkChannel (0) - , _logCurrentTimeUSecs (0) - , _logStartTimeUSecs (0) - , _logEndTimeUSecs (0) - , _logDurationUSecs (0) - , _playbackSpeed (1) - , _playbackStartTimeMSecs (0) - , _playbackStartLogTimeUSecs (0) - , _mavlink (nullptr) - , _logFileSize (0) +void LogReplayConfiguration::setLogFilename(const QString &logFilename) { - if (!_logReplayConfig) { - qWarning() << "Internal error"; + if (logFilename != _logFilename) { + _logFilename = logFilename; + emit filenameChanged(); } - - _errorTitle = tr("Log Replay Error"); - - _readTickTimer.moveToThread(this); - - 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) +/*===========================================================================*/ + +LogReplayWorker::LogReplayWorker(const LogReplayConfiguration *config, QObject *parent) + : QObject(parent) + , _logReplayConfig(config) { - disconnect(); + // qCDebug(LogReplayLinkLog) << Q_FUNC_INFO << this; } -bool LogReplayLink::_connect(void) +LogReplayWorker::~LogReplayWorker() { - // Disallow replay when any links are connected - if (MultiVehicleManager::instance()->activeVehicle()) { - emit communicationError(_errorTitle, tr("You must close all connections prior to replaying a log.")); - return false; - } + disconnectFromLog(); - if (isRunning()) { - quit(); - wait(); - } - start(HighPriority); - return true; + // qCDebug(LogReplayLinkLog) << Q_FUNC_INFO << this; } -void LogReplayLink::disconnect(void) +void LogReplayWorker::setup() { - if (_connected) { - quit(); - wait(); - _connected = false; - emit disconnected(); - } + Q_ASSERT(!_readTickTimer); + _readTickTimer = new QTimer(this); + + (void) connect(_readTickTimer, &QTimer::timeout, this, &LogReplayWorker::_readNextLogEntry); } -void LogReplayLink::run(void) +void LogReplayWorker::connectToLog() { - // Load the log file + if (isConnected()) { + qCWarning(LogReplayLinkLog) << "Already connected"; + return; + } + + if (MultiVehicleManager::instance()->activeVehicle()) { + emit errorOccurred(tr("You must close all connections prior to replaying a log.")); + return; + } + if (!_loadLogFile()) { + disconnectFromLog(); return; } - - _connected = true; + + _isConnected = true; emit connected(); - - // Start playback - _play(); - // Run normal event loop until exit - exec(); - - _readTickTimer.stop(); + play(); } -void LogReplayLink::_replayError(const QString& errorMsg) +void LogReplayWorker::disconnectFromLog() { - qDebug() << _errorTitle << errorMsg; - emit communicationError(_errorTitle, errorMsg); + if (!isConnected()) { + qCDebug(LogReplayLinkLog) << "Already disconnected"; + return; + } + + _isConnected = false; + emit disconnected(); + + _readTickTimer->stop(); } -/// Since this is log replay, we just drops writes on the floor -void LogReplayLink::_writeBytes(const QByteArray &bytes) +bool LogReplayWorker::isPlaying() const { - Q_UNUSED(bytes); + return (_readTickTimer && _readTickTimer->isActive()); } -/// 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) +void LogReplayWorker::play() { - 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) { - timestamp = qbswap(timestamp); + LinkManager::instance()->setConnectionsSuspended(tr("Connect not allowed during Flight Data replay.")); + MAVLinkProtocol::instance()->suspendLogForReplay(true); + + if (_logFile.atEnd()) { + _resetPlaybackToBeginning(); } - - return timestamp; + + _playbackStartTimeMSecs = static_cast(QDateTime::currentMSecsSinceEpoch()); + _playbackStartLogTimeUSecs = _logCurrentTimeUSecs; + _readTickTimer->start(1); + + emit playbackStarted(); } -/// 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) +void LogReplayWorker::pause() { - char nextByte; - mavlink_status_t status; + LinkManager::instance()->setConnectionsAllowed(); + MAVLinkProtocol::instance()->suspendLogForReplay(false); - bytes.clear(); + _readTickTimer->stop(); - while (_logFile.getChar(&nextByte)) { // Loop over every byte - mavlink_message_t message; - bool messageFound = mavlink_parse_char(_mavlinkChannel, nextByte, &message, &status); + emit playbackPaused(); +} - 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); +void LogReplayWorker::setPlaybackSpeed(qreal playbackSpeed) +{ + _playbackSpeed = playbackSpeed; + _playbackStartTimeMSecs = static_cast(QDateTime::currentMSecsSinceEpoch()); + _playbackStartLogTimeUSecs = _logCurrentTimeUSecs; + _readTickTimer->start(1); +} - if (messageFound) { - // Return the timestamp for the next message - QByteArray rawTime = _logFile.read(cbTimestamp); - return _parseTimestamp(rawTime); +void LogReplayWorker::movePlayhead(qreal percentComplete) +{ + if (isPlaying()) { + pause(); + if (_readTickTimer->isActive()) { + return; } } - return 0; -} + percentComplete = qBound(0., percentComplete, 100.); + const qreal percentCompleteMult = percentComplete / 100.0; + const qint64 newFilePos = static_cast(percentCompleteMult * static_cast(_logFile.size())); + if (!_logFile.seek(newFilePos)) { + emit errorOccurred(tr("Unable to seek to new position")); + return; + } -/// 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_message_t dummy{}; + _logCurrentTimeUSecs = _seekToNextMavlinkMessage(dummy); - mavlink_reset_channel_status(_mavlinkChannel); + qreal newRelativeTimeUSecs = static_cast(_logCurrentTimeUSecs - _logStartTimeUSecs); + const qreal baudRate = _logFile.size() / static_cast(_logDurationUSecs) / 1e6; + const qreal desiredTimeUSecs = percentCompleteMult * _logDurationUSecs; + const qint64 offset = (newRelativeTimeUSecs - desiredTimeUSecs) * baudRate; + if (!_logFile.seek(_logFile.pos() + offset)) { + emit errorOccurred(tr("Unable to seek to new position")); + return; + } - while (_logFile.getChar(&nextByte)) { - bool messageFound = mavlink_parse_char(_mavlinkChannel, nextByte, nextMsg, &status); + _logCurrentTimeUSecs = _seekToNextMavlinkMessage(dummy); + _signalCurrentLogTimeSecs(); - 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); + newRelativeTimeUSecs = static_cast(_logCurrentTimeUSecs - _logStartTimeUSecs); + percentComplete = ((newRelativeTimeUSecs / _logDurationUSecs) * 100); + emit playbackPercentCompleteChanged(percentComplete); +} + +void LogReplayWorker::_resetPlaybackToBeginning() +{ + if (_logFile.isOpen()) { + if (!_logFile.reset()) { + qCWarning(LogReplayLinkLog) << "failed to reset log file:" << _logFile.error() << _logFile.errorString(); } } - - return 0; + + _playbackStartTimeMSecs = 0; + _playbackStartLogTimeUSecs = 0; + _logCurrentTimeUSecs = _logStartTimeUSecs; } -quint64 LogReplayLink::_findLastTimestamp(void) +void LogReplayWorker::_readNextLogEntry() { - 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. + int timeToNextExecutionMSecs = 0; + while (timeToNextExecutionMSecs < 3) { + QByteArray bytes; + bytes.reserve(_logFile.bytesAvailable()); + const qint64 nextTimeUSecs = _readNextMavlinkMessage(bytes); + emit dataReceived(bytes); + emit playbackPercentCompleteChanged((static_cast(_logCurrentTimeUSecs - _logStartTimeUSecs) / static_cast(_logDurationUSecs)) * 100); - _logFile.reset(); - mavlink_reset_channel_status(_mavlinkChannel); + if (_logFile.atEnd()) { + pause(); + emit playbackAtEnd(); + return; + } - while (_logFile.bytesAvailable() > cbTimestamp) { - lastTimestamp = _parseTimestamp(_logFile.read(cbTimestamp)); + _logCurrentTimeUSecs = nextTimeUSecs; - bool endOfMessage = false; - while (!endOfMessage && _logFile.getChar(&nextByte)) { - endOfMessage = mavlink_parse_char(_mavlinkChannel, nextByte, &msg, &status); - } + const quint64 currentTimeMSecs = static_cast(QDateTime::currentMSecsSinceEpoch()); + const quint64 desiredPlayheadMovementTimeMSecs = ((_logCurrentTimeUSecs - _playbackStartLogTimeUSecs) / 1000) / _playbackSpeed; + const quint64 desiredCurrentTimeMSecs = _playbackStartTimeMSecs + desiredPlayheadMovementTimeMSecs; + timeToNextExecutionMSecs = desiredCurrentTimeMSecs - currentTimeMSecs; } - return lastTimestamp; + _signalCurrentLogTimeSecs(); + + _readTickTimer->start(timeToNextExecutionMSecs); } -bool LogReplayLink::_loadLogFile(void) +void LogReplayWorker::_signalCurrentLogTimeSecs() { - QString errorMsg; - QString logFilename = _logReplayConfig->logFilename(); - QFileInfo logFileInfo; - int logDurationSecondsTotal; - quint64 startTimeUSecs; - quint64 endTimeUSecs; + emit currentLogTimeSecs((_logCurrentTimeUSecs - _logStartTimeUSecs) / 1000000); +} +bool LogReplayWorker::_loadLogFile() +{ if (_logFile.isOpen()) { - errorMsg = tr("Attempt to load new log while log being played"); - goto Error; + _logFile.close(); + emit errorOccurred(tr("Attempt to load new log while log being played")); + return false; } - + + const QString logFilename = _logReplayConfig->logFilename(); _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; + emit errorOccurred(tr("Unable to open log file: '%1', error: %2").arg(logFilename, _logFile.errorString())); + return false; } + + QFileInfo logFileInfo; logFileInfo.setFile(logFilename); _logFileSize = logFileInfo.size(); - - startTimeUSecs = _parseTimestamp(_logFile.read(cbTimestamp)); - endTimeUSecs = _findLastTimestamp(); + const quint64 startTimeUSecs = _parseTimestamp(_logFile.read(kTimestamp)); + const quint64 endTimeUSecs = _findLastTimestamp(); if (endTimeUSecs <= startTimeUSecs) { - errorMsg = tr("The log file '%1' is corrupt or empty.").arg(logFilename); - goto Error; + _logFile.close(); + emit errorOccurred(tr("The log file '%1' is corrupt or empty.").arg(logFilename)); + return false; } - // 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(); + if (!_logFile.reset()) { + qCWarning(LogReplayLinkLog) << "failed to reset log file:" << _logFile.error() << _logFile.errorString(); + } - logDurationSecondsTotal = (_logDurationUSecs) / 1000000; - + const quint64 logDurationSecondsTotal = _logDurationUSecs / 1000000; emit logFileStats(logDurationSecondsTotal); - + 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) +quint64 LogReplayWorker::_parseTimestamp(const QByteArray &bytes) { - QByteArray bytes; + const quint64 currentTimestamp = static_cast(QDateTime::currentMSecsSinceEpoch()) * 1000; + quint64 timestamp = qFromBigEndian(*reinterpret_cast(bytes.constData())); + if (timestamp > currentTimestamp) { + timestamp = qbswap(timestamp); + } - // Now parse MAVLink messages, grabbing their timestamps as we go. We stop once we - // have at least 3ms until the next one. + return timestamp; +} - // We track what the next execution time should be in milliseconds, which we use to set - // the next timer interrupt. - int timeToNextExecutionMSecs = 0; +quint64 LogReplayWorker::_readNextMavlinkMessage(QByteArray &bytes) +{ + bytes.clear(); - 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); + char nextByte; + while (_logFile.getChar(&nextByte)) { + mavlink_message_t message{}; + mavlink_status_t status{}; + const bool messageFound = mavlink_parse_char(_mavlinkChannel, nextByte, &message, &status); - if (_logFile.atEnd()) { - _finishPlayback(); - return; + if (status.parse_state == MAVLINK_PARSE_STATE_GOT_STX) { + bytes.clear(); } + (void) bytes.append(nextByte); - _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; + if (messageFound) { + const QByteArray rawTime = _logFile.read(kTimestamp); + return _parseTimestamp(rawTime); + } } - _signalCurrentLogTimeSecs(); - - // And schedule the next execution of this function. - _readTickTimer.start(timeToNextExecutionMSecs); + return 0; } -void LogReplayLink::_play(void) +quint64 LogReplayWorker::_seekToNextMavlinkMessage(mavlink_message_t &nextMsg) { - LinkManager::instance()->setConnectionsSuspended(tr("Connect not allowed during Flight Data replay.")); -#ifndef __mobile__ - MAVLinkProtocol::instance()->suspendLogForReplay(true); -#endif - - // 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(); -} + mavlink_reset_channel_status(_mavlinkChannel); -void LogReplayLink::_pause(void) -{ - LinkManager::instance()->setConnectionsAllowed(); -#ifndef __mobile__ - MAVLinkProtocol::instance()->suspendLogForReplay(false); -#endif - - _readTickTimer.stop(); - - emit playbackPaused(); -} + qint64 messageStartPos = -1; + char nextByte; + while (_logFile.getChar(&nextByte)) { + mavlink_status_t status{}; + const bool messageFound = mavlink_parse_char(_mavlinkChannel, nextByte, &nextMsg, &status); -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; - _playbackStartLogTimeUSecs = 0; - _logCurrentTimeUSecs = _logStartTimeUSecs; -} + if (status.parse_state == MAVLINK_PARSE_STATE_GOT_STX) { + messageStartPos = _logFile.pos() - 1; + } -void LogReplayLink::movePlayhead(qreal percentComplete) -{ - if (isPlaying()) { - _pauseOnThread(); - QSignalSpy waitForPause(this, SIGNAL(playbackPaused())); - waitForPause.wait(); - if (_readTickTimer.isActive()) { - return; + if (messageFound && (messageStartPos != -1)) { + if (!_logFile.seek(messageStartPos - kTimestamp)) { + qCWarning(LogReplayLinkLog) << "Failed to seek next message:" << _logFile.error() << _logFile.errorString(); + break; + } + + const QByteArray rawTime = _logFile.read(kTimestamp); + return _parseTimestamp(rawTime); } } - if (percentComplete < 0) { - percentComplete = 0; - } - if (percentComplete > 100) { - percentComplete = 100; - } - - qreal percentCompleteMult = percentComplete / 100.0; - - // 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; - } + return 0; +} - // But we do align to the next MAVLink message for consistency. - mavlink_message_t dummy; - _logCurrentTimeUSecs = _seekToNextMavlinkMessage(&dummy); +quint64 LogReplayWorker::_findLastTimestamp() +{ + if (!_logFile.reset()) { + qCWarning(LogReplayLinkLog) << "failed to reset log file:" << _logFile.error() << _logFile.errorString(); + } - // Now calculate the current file location based on time. - qreal newRelativeTimeUSecs = (qreal)(_logCurrentTimeUSecs - _logStartTimeUSecs); + mavlink_reset_channel_status(_mavlinkChannel); - // Calculate the effective baud rate of the file in bytes/s. - qreal baudRate = _logFile.size() / (qreal)_logDurationUSecs / 1e6; + quint64 lastTimestamp = 0; - // And the desired time is: - qreal desiredTimeUSecs = percentCompleteMult * _logDurationUSecs; + while (_logFile.bytesAvailable() > kTimestamp) { + lastTimestamp = _parseTimestamp(_logFile.read(kTimestamp)); - // 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; + bool endOfMessage = false; + char nextByte; + while (!endOfMessage && _logFile.getChar(&nextByte)) { + mavlink_message_t msg{}; + mavlink_status_t status{}; + endOfMessage = mavlink_parse_char(_mavlinkChannel, nextByte, &msg, &status); + } } - // 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); + return lastTimestamp; } -void LogReplayLink::_setPlaybackSpeed(qreal playbackSpeed) -{ - _playbackSpeed = playbackSpeed; - - // 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) +LogReplayLink::LogReplayLink(SharedLinkConfigurationPtr &config, QObject *parent) + : LinkInterface(config, parent) + , _logReplayConfig(qobject_cast(config.get())) + , _worker(new LogReplayWorker(_logReplayConfig)) + , _workerThread(new QThread(this)) { - _pause(); - - emit playbackAtEnd(); -} + // qCDebug(LogReplayLinkLog) << Q_FUNC_INFO << this; -void LogReplayLink::_signalCurrentLogTimeSecs(void) -{ - emit currentLogTimeSecs((_logCurrentTimeUSecs - _logStartTimeUSecs) / 1000000); -} + _workerThread->setObjectName(QStringLiteral("LogReplay_%1").arg(_logReplayConfig->name())); -LogReplayLinkController::LogReplayLinkController(void) - : _link (nullptr) - , _isPlaying (false) - , _percentComplete (0) - , _playheadSecs (0) - , _playbackSpeed (1) -{ -} - -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); - } + _worker->moveToThread(_workerThread); + (void) connect(_workerThread, &QThread::started, _worker, &LogReplayWorker::setup); + (void) connect(_workerThread, &QThread::finished, _worker, &QObject::deleteLater); - if (link) { - _link = link; + (void) connect(_worker, &LogReplayWorker::connected, this, &LogReplayLink::_onConnected, Qt::QueuedConnection); + (void) connect(_worker, &LogReplayWorker::disconnected, this, &LogReplayLink::_onDisconnected, Qt::QueuedConnection); + (void) connect(_worker, &LogReplayWorker::errorOccurred, this, &LogReplayLink::_onErrorOccurred, Qt::QueuedConnection); + (void) connect(_worker, &LogReplayWorker::dataReceived, this, &LogReplayLink::_onDataReceived, Qt::QueuedConnection); - 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); + (void) connect(_worker, &LogReplayWorker::logFileStats, this, &LogReplayLink::logFileStats, Qt::QueuedConnection); + (void) connect(_worker, &LogReplayWorker::playbackStarted, this, &LogReplayLink::playbackStarted, Qt::QueuedConnection); + (void) connect(_worker, &LogReplayWorker::playbackPaused, this, &LogReplayLink::playbackPaused, Qt::QueuedConnection); + (void) connect(_worker, &LogReplayWorker::playbackPercentCompleteChanged, this, &LogReplayLink::playbackPercentCompleteChanged, Qt::QueuedConnection); + (void) connect(_worker, &LogReplayWorker::currentLogTimeSecs, this, &LogReplayLink::currentLogTimeSecs, Qt::QueuedConnection); + (void) connect(_worker, &LogReplayWorker::disconnected, this, &LogReplayLink::disconnected, Qt::QueuedConnection); - connect(this, &LogReplayLinkController::playbackSpeedChanged, _link, &LogReplayLink::setPlaybackSpeed); - - emit linkChanged(_link); - } + _workerThread->start(); } -void LogReplayLinkController::setIsPlaying(bool isPlaying) +LogReplayLink::~LogReplayLink() { - if (isPlaying) { - _link->play(); - } else { - _link->pause(); + LogReplayLink::disconnect(); + + _workerThread->quit(); + if (!_workerThread->wait()) { + qCWarning(LogReplayLinkLog) << "Failed to wait for LogReplay Thread to close"; } -} -void LogReplayLinkController::setPercentComplete(qreal percentComplete) -{ - _link->movePlayhead(percentComplete); + // qCDebug(LogReplayLinkLog) << Q_FUNC_INFO << this; } -void LogReplayLinkController::_logFileStats(int logDurationSecs) +bool LogReplayLink::_connect() { - _totalTime = _secondsToHMS(logDurationSecs); - emit totalTimeChanged(_totalTime); + return QMetaObject::invokeMethod(_worker, "connectToLog", Qt::QueuedConnection); } -void LogReplayLinkController::_playbackStarted(void) +void LogReplayLink::disconnect() { - _isPlaying = true; - emit isPlayingChanged(true); + (void) QMetaObject::invokeMethod(_worker, "disconnectFromLog", Qt::QueuedConnection); } -void LogReplayLinkController::_playbackPaused(void) +void LogReplayLink::_onErrorOccurred(const QString &errorString) { - _isPlaying = false; - emit isPlayingChanged(true); + qCWarning(LogReplayLinkLog) << "Error:" << errorString; + emit communicationError(tr("Log Replay Link Error"), tr("Link: %1, %2.").arg(_logReplayConfig->name(), errorString)); } -void LogReplayLinkController::_playbackAtEnd(void) +void LogReplayLink::_onDataReceived(const QByteArray &data) { - _isPlaying = false; - emit isPlayingChanged(true); + emit bytesReceived(this, data); } -void LogReplayLinkController::_playbackPercentCompleteChanged(qreal percentComplete) +void LogReplayLink::play() { - _percentComplete = percentComplete; - emit percentCompleteChanged(_percentComplete); + (void) QMetaObject::invokeMethod(_worker, "play", Qt::QueuedConnection); } -void LogReplayLinkController::_currentLogTimeSecs(int secs) +void LogReplayLink::pause() { - if (_playheadSecs != secs) { - _playheadSecs = secs; - _playheadTime = _secondsToHMS(secs); - emit playheadTimeChanged(_playheadTime); - } + (void) QMetaObject::invokeMethod(_worker, "pause", Qt::QueuedConnection); } -void LogReplayLinkController::_linkDisconnected(void) +void LogReplayLink::setPlaybackSpeed(qreal playbackSpeed) { - setLink(nullptr); + (void) QMetaObject::invokeMethod(_worker, "setPlaybackSpeed", Qt::QueuedConnection, playbackSpeed); } -QString LogReplayLinkController::_secondsToHMS(int seconds) +void LogReplayLink::movePlayhead(qreal percentComplete) { - 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')); - } + (void) QMetaObject::invokeMethod(_worker, "movePlayhead", Qt::QueuedConnection, percentComplete); } diff --git a/src/Comms/LogReplayLink.h b/src/Comms/LogReplayLink.h index 45e1a27fbe4..0e3d5e8212b 100644 --- a/src/Comms/LogReplayLink.h +++ b/src/Comms/LogReplayLink.h @@ -9,185 +9,155 @@ #pragma once +#include +#include + #include "LinkConfiguration.h" #include "LinkInterface.h" -#include -#include - -class LinkManager; -class MAVLinkProtocol; +class QTimer; typedef struct __mavlink_message mavlink_message_t; -class LogReplayLinkConfiguration : public LinkConfiguration +Q_DECLARE_LOGGING_CATEGORY(LogReplayLinkLog) + +/*===========================================================================*/ + +class LogReplayConfiguration : public LinkConfiguration { Q_OBJECT -public: - Q_PROPERTY(QString fileName READ logFilename WRITE setLogFilename NOTIFY fileNameChanged) - - LogReplayLinkConfiguration(const QString& name); - LogReplayLinkConfiguration(const LogReplayLinkConfiguration* copy); + Q_PROPERTY(QString filename READ logFilename WRITE setLogFilename NOTIFY filenameChanged) - QString logFilename(void) const { return _logFilename; } - void setLogFilename(const QString logFilename) { _logFilename = logFilename; emit fileNameChanged(); } +public: + explicit LogReplayConfiguration(const QString &name, QObject *parent = nullptr); + explicit LogReplayConfiguration(const LogReplayConfiguration *copy, QObject *parent = nullptr); + virtual ~LogReplayConfiguration(); - QString logFilenameShort(void); + LinkType type() const override { return LinkConfiguration::TypeLogReplay; } + void copyFrom(const LinkConfiguration *source) override; + void loadSettings(QSettings &settings, const QString &root) override; + void saveSettings(QSettings &settings, const QString &root) override; + QString settingsURL() override { return QStringLiteral("LogReplaySettings.qml"); } + QString settingsTitle() override { return tr("Log Replay Link Settings"); } - // Virtuals from LinkConfiguration - LinkType type (void) const override { return LinkConfiguration::TypeLogReplay; } - void copyFrom (const LinkConfiguration* source) override; - void loadSettings (QSettings& settings, const QString& root) override; - void saveSettings (QSettings& settings, const QString& root) override; - QString settingsURL (void) override { return "LogReplaySettings.qml"; } - QString settingsTitle (void) override { return tr("Log Replay Link Settings"); } + QString logFilenameShort() const; + QString logFilename() const { return _logFilename; } + void setLogFilename(const QString &logFilename); signals: - void fileNameChanged(); + void filenameChanged(); private: - static constexpr const char* _logFilenameKey = "logFilename"; - QString _logFilename; + QString _logFilename; }; -/// Pseudo link that reads a telemetry log and feeds it into the application. -class LogReplayLink : public LinkInterface +/*===========================================================================*/ + +class LogReplayWorker : public QObject { Q_OBJECT public: - LogReplayLink(SharedLinkConfigurationPtr& config); - virtual ~LogReplayLink(); - - /// @return true: log is currently playing, false: log playback is paused - bool isPlaying(void) { return _readTickTimer.isActive(); } + explicit LogReplayWorker(const LogReplayConfiguration *config, QObject *parent = nullptr); + ~LogReplayWorker(); - void play (void) { emit _playOnThread(); } - void pause (void) { emit _pauseOnThread(); } - void movePlayhead (qreal percentComplete); + bool isConnected() const { return _isConnected; } + bool isPlaying() const; - // overrides from LinkInterface - bool isConnected(void) const override { return _connected; } - bool isLogReplay(void) override { return true; } - void disconnect (void) override; +signals: + void connected(); + void disconnected(); + void errorOccurred(const QString &errorString); + void dataReceived(const QByteArray &data); + void logFileStats(uint32_t logDurationSecs); + void playbackStarted(); + void playbackPaused(); + void playbackAtEnd(); + void playbackPercentCompleteChanged(qreal percentComplete); + void currentLogTimeSecs(uint32_t secs); public slots: - /// Sets the acceleration factor: -100: 0.01X, 0: 1.0X, 100: 100.0X - void setPlaybackSpeed(qreal playbackSpeed) { emit _setPlaybackSpeedOnThread(playbackSpeed); } - -signals: - void logFileStats (int logDurationSecs); - void playbackStarted (void); - void playbackPaused (void); - void playbackAtEnd (void); - void playbackPercentCompleteChanged (qreal percentComplete); - void currentLogTimeSecs (int secs); - - // Internal signals - void _playOnThread (void); - void _pauseOnThread (void); - void _setPlaybackSpeedOnThread (qreal playbackSpeed); + void setup(); + void connectToLog(); + void disconnectFromLog(); + void play(); + void pause(); + void setPlaybackSpeed(qreal playbackSpeed); + void movePlayhead(qreal percentComplete); private slots: - // LinkInterface overrides - void _writeBytes(const QByteArray &bytes) override; - - void _readNextLogEntry (void); - void _play (void); - void _pause (void); - void _setPlaybackSpeed (qreal playbackSpeed); + void _readNextLogEntry(); private: + quint64 _parseTimestamp(const QByteArray &bytes); + quint64 _seekToNextMavlinkMessage(mavlink_message_t &nextMsg); + quint64 _findLastTimestamp(); + quint64 _readNextMavlinkMessage(QByteArray &bytes); + bool _loadLogFile(); + void _resetPlaybackToBeginning(); + void _signalCurrentLogTimeSecs(); - // LinkInterface overrides - bool _connect(void) override; - - void _replayError (const QString& errorMsg); - quint64 _parseTimestamp (const QByteArray& bytes); - quint64 _seekToNextMavlinkMessage (mavlink_message_t* nextMsg); - quint64 _findLastTimestamp (void); - quint64 _readNextMavlinkMessage (QByteArray& bytes); - bool _loadLogFile (void); - void _finishPlayback (void); - void _resetPlaybackToBeginning (void); - void _signalCurrentLogTimeSecs (void); - - // QThread overrides - void run(void) override; - - LogReplayLinkConfiguration* _logReplayConfig; - - bool _connected; - uint8_t _mavlinkChannel; - QTimer _readTickTimer; ///< Timer which signals a read of next log record + const LogReplayConfiguration *_logReplayConfig = nullptr; + QTimer *_readTickTimer = nullptr; - QString _errorTitle; ///< Title for communicatorError signals + bool _isConnected = false; + uint8_t _mavlinkChannel = 0; - quint64 _logCurrentTimeUSecs; ///< The timestamp of the next message in the log file. - quint64 _logStartTimeUSecs; ///< The first timestamp in the current log file. - quint64 _logEndTimeUSecs; ///< The last timestamp in the current log file. - quint64 _logDurationUSecs; + quint64 _logCurrentTimeUSecs = 0; + quint64 _logStartTimeUSecs = 0; + quint64 _logEndTimeUSecs = 0; + quint64 _logDurationUSecs = 0; - qreal _playbackSpeed; - quint64 _playbackStartTimeMSecs; ///< The time when the logfile was first played back. This is used to pace out replaying the messages to fix long-term drift/skew. 0 indicates that the player hasn't initiated playback of this log file. - quint64 _playbackStartLogTimeUSecs; + qreal _playbackSpeed = 1; + quint64 _playbackStartTimeMSecs = 0; + quint64 _playbackStartLogTimeUSecs = 0; - MAVLinkProtocol* _mavlink; - QFile _logFile; - quint64 _logFileSize; + QFile _logFile; + quint64 _logFileSize = 0; - static const int cbTimestamp = sizeof(quint64); + static constexpr size_t kTimestamp = sizeof(quint64); }; -class LogReplayLinkController : public QObject +/*===========================================================================*/ + +class LogReplayLink : public LinkInterface { Q_OBJECT public: - Q_PROPERTY(LogReplayLink* link READ link WRITE setLink NOTIFY linkChanged) - Q_PROPERTY(bool isPlaying READ isPlaying WRITE setIsPlaying NOTIFY isPlayingChanged) - Q_PROPERTY(qreal percentComplete READ percentComplete WRITE setPercentComplete NOTIFY percentCompleteChanged) - Q_PROPERTY(QString totalTime MEMBER _totalTime NOTIFY totalTimeChanged) - Q_PROPERTY(QString playheadTime MEMBER _playheadTime NOTIFY playheadTimeChanged) - Q_PROPERTY(qreal playbackSpeed MEMBER _playbackSpeed NOTIFY playbackSpeedChanged) - - LogReplayLinkController(void); + explicit LogReplayLink(SharedLinkConfigurationPtr &config, QObject *parent = nullptr); + virtual ~LogReplayLink(); - LogReplayLink* link (void) { return _link; } - bool isPlaying (void) const{ return _isPlaying; } - qreal percentComplete (void) const{ return _percentComplete; } + bool isConnected() const override { return _worker->isConnected(); } + void disconnect() override; + bool isLogReplay() const final { return true; } - void setLink (LogReplayLink* link); - void setIsPlaying (bool isPlaying); - void setPercentComplete (qreal percentComplete); + bool isPlaying() const { return _worker->isPlaying(); } + void play(); + void pause(); + void setPlaybackSpeed(qreal playbackSpeed); + void movePlayhead(qreal percentComplete); signals: - void linkChanged (LogReplayLink* link); - void isPlayingChanged (bool isPlaying); - void percentCompleteChanged (qreal percentComplete); - void playheadTimeChanged (QString playheadTime); - void totalTimeChanged (QString totalTime); - void playbackSpeedChanged (qreal playbackSpeed); + void logFileStats(uint32_t logDurationSecs); + void playbackStarted(); + void playbackPaused(); + void playbackAtEnd(); + void playbackPercentCompleteChanged(qreal percentComplete); + void currentLogTimeSecs(uint32_t secs); private slots: - void _logFileStats (int logDurationSecs); - void _playbackStarted (void); - void _playbackPaused (void); - void _playbackAtEnd (void); - void _playbackPercentCompleteChanged (qreal percentComplete); - void _currentLogTimeSecs (int secs); - void _linkDisconnected (void); + void _writeBytes(const QByteArray &bytes) override { Q_UNUSED(bytes); } + void _onConnected() { emit connected(); } + void _onDisconnected() { emit disconnected(); } + void _onErrorOccurred(const QString &errorString); + void _onDataReceived(const QByteArray &data); private: - QString _secondsToHMS(int seconds); - - LogReplayLink* _link; - bool _isPlaying; - qreal _percentComplete; - int _playheadSecs; - QString _playheadTime; - QString _totalTime; - qreal _playbackSpeed; -}; + bool _connect() override; + const LogReplayConfiguration *_logReplayConfig = nullptr; + LogReplayWorker *_worker = nullptr; + QThread *_workerThread = nullptr; +}; diff --git a/src/Comms/LogReplayLinkController.cc b/src/Comms/LogReplayLinkController.cc new file mode 100644 index 00000000000..e89ee2c9201 --- /dev/null +++ b/src/Comms/LogReplayLinkController.cc @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "LogReplayLinkController.h" +#include "QGCLoggingCategory.h" + +QGC_LOGGING_CATEGORY(LogReplayLinkControllerLog, "qgc.comms.logreplaylink") + +LogReplayLinkController::LogReplayLinkController(QObject *parent) + : QObject(parent) +{ + // qCDebug(LogReplayLinkControllerLog) << Q_FUNC_INFO << this; +} + +LogReplayLinkController::~LogReplayLinkController() +{ + // qCDebug(LogReplayLinkControllerLog) << Q_FUNC_INFO << this; +} + +void LogReplayLinkController::setLink(LogReplayLink *link) +{ + if (_link) { + (void) disconnect(_link); + (void) disconnect(this, &LogReplayLinkController::playbackSpeedChanged, _link, &LogReplayLink::setPlaybackSpeed); + + _isPlaying = false; + emit isPlayingChanged(_isPlaying); + + _percentComplete = 0; + emit percentCompleteChanged(_percentComplete); + + _playheadTime.clear(); + emit playheadTimeChanged(_playheadTime); + + _totalTime.clear(); + emit totalTimeChanged(_totalTime); + + _link = nullptr; + emit linkChanged(_link); + } + + if (link) { + _link = link; + + (void) connect(_link, &LogReplayLink::logFileStats, this, &LogReplayLinkController::_logFileStats); + (void) connect(_link, &LogReplayLink::playbackStarted, this, &LogReplayLinkController::_playbackStarted); + (void) connect(_link, &LogReplayLink::playbackPaused, this, &LogReplayLinkController::_playbackPaused); + (void) connect(_link, &LogReplayLink::playbackPercentCompleteChanged, this, &LogReplayLinkController::_playbackPercentCompleteChanged); + (void) connect(_link, &LogReplayLink::currentLogTimeSecs, this, &LogReplayLinkController::_currentLogTimeSecs); + (void) connect(_link, &LogReplayLink::disconnected, this, &LogReplayLinkController::_linkDisconnected); + + (void) connect(this, &LogReplayLinkController::playbackSpeedChanged, _link, &LogReplayLink::setPlaybackSpeed); + + emit linkChanged(_link); + } +} + +void LogReplayLinkController::setIsPlaying(bool isPlaying) const +{ + if (isPlaying) { + _link->play(); + } else { + _link->pause(); + } +} + +void LogReplayLinkController::setPercentComplete(qreal percentComplete) const +{ + _link->movePlayhead(percentComplete); +} + +void LogReplayLinkController::_logFileStats(uint32_t logDurationSecs) +{ + const QString totalTime = _secondsToHMS(logDurationSecs); + if (totalTime != _totalTime) { + _totalTime = totalTime; + emit totalTimeChanged(_totalTime); + } +} + +void LogReplayLinkController::_playbackStarted() +{ + if (!_isPlaying) { + _isPlaying = true; + emit isPlayingChanged(_isPlaying); + } +} + +void LogReplayLinkController::_playbackPaused() +{ + if (_isPlaying) { + _isPlaying = false; + emit isPlayingChanged(_isPlaying); + } +} + +void LogReplayLinkController::_playbackAtEnd() +{ + if (_isPlaying) { + _isPlaying = false; + emit isPlayingChanged(_isPlaying); + } +} + +void LogReplayLinkController::_playbackPercentCompleteChanged(qreal percentComplete) +{ + if (percentComplete != _percentComplete) { + _percentComplete = percentComplete; + emit percentCompleteChanged(_percentComplete); + } +} + +void LogReplayLinkController::_currentLogTimeSecs(uint32_t secs) +{ + if (secs != _playheadSecs) { + _playheadSecs = secs; + _playheadTime = _secondsToHMS(secs); + emit playheadTimeChanged(_playheadTime); + } +} + +QString LogReplayLinkController::_secondsToHMS(uint32_t seconds) +{ + uint32_t secondsPart = seconds; + uint32_t minutesPart = secondsPart / 60; + const uint32_t hoursPart = minutesPart / 60; + secondsPart -= (60 * minutesPart); + minutesPart -= (60 * hoursPart); + + QString result = QStringLiteral("%2m:%3s").arg(minutesPart, 2, 10, QLatin1Char('0')).arg(secondsPart, 2, 10, QLatin1Char('0')); + if (hoursPart != 0) { + (void) result.prepend(QStringLiteral("%1h:").arg(hoursPart, 2, 10, QLatin1Char('0'))); + } + + return result; +} diff --git a/src/Comms/LogReplayLinkController.h b/src/Comms/LogReplayLinkController.h new file mode 100644 index 00000000000..8e9c5f3709c --- /dev/null +++ b/src/Comms/LogReplayLinkController.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include + +#include "LogReplayLink.h" + +Q_DECLARE_LOGGING_CATEGORY(LogReplayLinkControllerLog) + +class LogReplayLinkController : public QObject +{ + Q_OBJECT + + Q_PROPERTY(LogReplayLink *link READ link WRITE setLink NOTIFY linkChanged) + Q_PROPERTY(bool isPlaying READ isPlaying WRITE setIsPlaying NOTIFY isPlayingChanged) + Q_PROPERTY(qreal percentComplete READ percentComplete WRITE setPercentComplete NOTIFY percentCompleteChanged) + Q_PROPERTY(QString totalTime MEMBER _totalTime NOTIFY totalTimeChanged) + Q_PROPERTY(QString playheadTime MEMBER _playheadTime NOTIFY playheadTimeChanged) + Q_PROPERTY(qreal playbackSpeed MEMBER _playbackSpeed NOTIFY playbackSpeedChanged) + +public: + explicit LogReplayLinkController(QObject *parent = nullptr); + ~LogReplayLinkController(); + + LogReplayLink *link() const { return _link; } + void setLink(LogReplayLink *link); + + bool isPlaying() const { return _isPlaying; } + void setIsPlaying(bool isPlaying) const; + + qreal percentComplete() const { return _percentComplete; } + void setPercentComplete(qreal percentComplete) const; + +signals: + void isPlayingChanged(bool isPlaying); + void linkChanged(LogReplayLink *link); + void percentCompleteChanged(qreal percentComplete); + void playbackSpeedChanged(qreal playbackSpeed); + void playheadTimeChanged(const QString &playheadTime); + void totalTimeChanged(const QString &totalTime); + +private slots: + void _currentLogTimeSecs(uint32_t secs); + void _linkDisconnected() { setLink(nullptr); } + void _logFileStats(uint32_t logDurationSecs); + void _playbackAtEnd(); + void _playbackPaused(); + void _playbackPercentCompleteChanged(qreal percentComplete); + void _playbackStarted(); + +private: + static QString _secondsToHMS(uint32_t seconds); + + bool _isPlaying = false; + qreal _percentComplete = 0; + uint32_t _playheadSecs = 0; + qreal _playbackSpeed = 1; + QString _playheadTime; + QString _totalTime; + LogReplayLink *_link = nullptr; +}; diff --git a/src/QmlControls/LogReplayStatusBar.qml b/src/QmlControls/LogReplayStatusBar.qml index 80401ea4dae..962f86c763a 100644 --- a/src/QmlControls/LogReplayStatusBar.qml +++ b/src/QmlControls/LogReplayStatusBar.qml @@ -8,11 +8,11 @@ import QGroundControl.Palette import QGroundControl.ScreenTools Rectangle { - height: visible ? (rowLayout.height + (_margins * 2)) : 0 - color: qgcPal.window + height: visible ? (rowLayout.height + (_margins * 2)) : 0 + color: qgcPal.window - property real _margins: ScreenTools.defaultFontPixelHeight / 4 - property var _logReplayLink: null + property real _margins: ScreenTools.defaultFontPixelHeight / 4 + property var _logReplayLink: null function pickLogFile() { if (globals.activeVehicle) { @@ -26,10 +26,10 @@ Rectangle { QGCPalette { id: qgcPal } QGCFileDialog { - id: filePicker - title: qsTr("Select Telemetery Log") - nameFilters: [ qsTr("Telemetry Logs (*.%1)").arg(_logFileExtension), qsTr("All Files (*)") ] - folder: QGroundControl.settingsManager.appSettings.telemetrySavePath + id: filePicker + title: qsTr("Select Telemetery Log") + nameFilters: [ qsTr("Telemetry Logs (*.%1)").arg(_logFileExtension), qsTr("All Files (*)") ] + folder: QGroundControl.settingsManager.appSettings.telemetrySavePath onAcceptedForLoad: (file) => { controller.link = QGroundControl.linkManager.startLogReplay(file) close() @@ -41,25 +41,27 @@ Rectangle { LogReplayLinkController { id: controller - onPercentCompleteChanged: slider.updatePercentComplete(percentComplete) + onPercentCompleteChanged: (percentComplete) => slider.updatePercentComplete(percentComplete) } RowLayout { - id: rowLayout - anchors.margins: _margins - anchors.top: parent.top - anchors.left: parent.left - anchors.right: parent.right + id: rowLayout + anchors { + margins: _margins + top: parent.top + left: parent.left + right: parent.right + } QGCButton { - text: controller.isPlaying ? qsTr("Pause") : qsTr("Play") - enabled: controller.link - onClicked: controller.isPlaying = !controller.isPlaying + enabled: controller.link + text: controller.isPlaying ? qsTr("Pause") : qsTr("Play") + onClicked: controller.isPlaying = !controller.isPlaying } QGCComboBox { - textRole: "text" - currentIndex: 3 + textRole: "text" + currentIndex: 3 model: ListModel { ListElement { text: "0.1"; value: 0.1 } @@ -77,11 +79,11 @@ Rectangle { QGCLabel { text: controller.playheadTime } Slider { - id: slider - Layout.fillWidth: true - from: 0 - to: 100 - enabled: controller.link + id: slider + Layout.fillWidth: true + from: 0 + to: 100 + enabled: controller.link property bool manualUpdate: false @@ -101,13 +103,13 @@ Rectangle { QGCLabel { text: controller.totalTime } QGCButton { - text: qsTr("Load Telemetry Log") - onClicked: pickLogFile() - visible: !controller.link + text: qsTr("Load Telemetry Log") + onClicked: pickLogFile() + visible: !controller.link } QGCButton { - text: qsTr("Close") + text: qsTr("Close") onClicked: { var activeVehicle = QGroundControl.multiVehicleManager.activeVehicle if (activeVehicle) { diff --git a/src/UI/preferences/LogReplaySettings.qml b/src/UI/preferences/LogReplaySettings.qml index 61da20f3da6..052c1f69fcb 100644 --- a/src/UI/preferences/LogReplaySettings.qml +++ b/src/UI/preferences/LogReplaySettings.qml @@ -13,39 +13,39 @@ import QtQuick.Layouts import QGroundControl import QGroundControl.Controls -import QGroundControl.ScreenTools -import QGroundControl.Palette RowLayout { spacing: _colSpacing function saveSettings() { + console.log(logField.text) subEditConfig.filename = logField.text } QGCLabel { text: qsTr("Log File") } QGCTextField { - id: logField - text: subEditConfig.fileName - width: _secondColumnWidth + id: logField + Layout.preferredWidth: _secondColumnWidth + text: subEditConfig.filename } QGCButton { - text: qsTr("Browse") - onClicked: filePicker.openForLoad() + text: qsTr("Browse") + onClicked: filePicker.openForLoad() } QGCFileDialog { - id: filePicker - title: qsTr("Select Telemetery Log") - nameFilters: [ qsTr("Telemetry Logs (*.%1)").arg(_logFileExtension), qsTr("All Files (*)") ] - folder: QGroundControl.settingsManager.appSettings.telemetrySavePath + id: filePicker + title: qsTr("Select Telemetery Log") + nameFilters: [ qsTr("Telemetry Logs (*.%1)").arg(_logFileExtension), qsTr("All Files (*)") ] + folder: QGroundControl.settingsManager.appSettings.telemetrySavePath + + property string _logFileExtension: QGroundControl.settingsManager.appSettings.telemetryFileExtension + onAcceptedForLoad: (file) => { logField.text = file close() } - - property string _logFileExtension: QGroundControl.settingsManager.appSettings.telemetryFileExtension } }