Commit 27776288 authored by Don Gagne's avatar Don Gagne

Convert Log Replay to Link

parent a48b1192
...@@ -161,6 +161,7 @@ FORMS += \ ...@@ -161,6 +161,7 @@ FORMS += \
src/QGCQmlWidgetHolder.ui \ src/QGCQmlWidgetHolder.ui \
src/ui/HDDisplay.ui \ src/ui/HDDisplay.ui \
src/ui/Linechart.ui \ src/ui/Linechart.ui \
src/ui/LogReplayLinkConfigurationWidget.ui \
src/ui/MainWindow.ui \ src/ui/MainWindow.ui \
src/ui/map/QGCMapTool.ui \ src/ui/map/QGCMapTool.ui \
src/ui/map/QGCMapToolBar.ui \ src/ui/map/QGCMapToolBar.ui \
...@@ -228,6 +229,7 @@ HEADERS += \ ...@@ -228,6 +229,7 @@ HEADERS += \
src/comm/LinkConfiguration.h \ src/comm/LinkConfiguration.h \
src/comm/LinkInterface.h \ src/comm/LinkInterface.h \
src/comm/LinkManager.h \ src/comm/LinkManager.h \
src/comm/LogReplayLink.h \
src/comm/MAVLinkProtocol.h \ src/comm/MAVLinkProtocol.h \
src/comm/MockLink.h \ src/comm/MockLink.h \
src/comm/MockLinkFileServer.h \ src/comm/MockLinkFileServer.h \
...@@ -276,6 +278,7 @@ HEADERS += \ ...@@ -276,6 +278,7 @@ HEADERS += \
src/ui/linechart/LinechartWidget.h \ src/ui/linechart/LinechartWidget.h \
src/ui/linechart/Scrollbar.h \ src/ui/linechart/Scrollbar.h \
src/ui/linechart/ScrollZoomer.h \ src/ui/linechart/ScrollZoomer.h \
src/ui/LogReplayLinkConfigurationWidget.h \
src/ui/MainWindow.h \ src/ui/MainWindow.h \
src/ui/map/MAV2DIcon.h \ src/ui/map/MAV2DIcon.h \
src/ui/map/QGCMapTool.h \ src/ui/map/QGCMapTool.h \
...@@ -365,6 +368,7 @@ SOURCES += \ ...@@ -365,6 +368,7 @@ SOURCES += \
src/CmdLineOptParser.cc \ src/CmdLineOptParser.cc \
src/comm/LinkConfiguration.cc \ src/comm/LinkConfiguration.cc \
src/comm/LinkManager.cc \ src/comm/LinkManager.cc \
src/comm/LogReplayLink.cc \
src/comm/MAVLinkProtocol.cc \ src/comm/MAVLinkProtocol.cc \
src/comm/MockLink.cc \ src/comm/MockLink.cc \
src/comm/MockLinkFileServer.cc \ src/comm/MockLinkFileServer.cc \
...@@ -405,6 +409,7 @@ SOURCES += \ ...@@ -405,6 +409,7 @@ SOURCES += \
src/ui/linechart/LinechartWidget.cc \ src/ui/linechart/LinechartWidget.cc \
src/ui/linechart/Scrollbar.cc \ src/ui/linechart/Scrollbar.cc \
src/ui/linechart/ScrollZoomer.cc \ src/ui/linechart/ScrollZoomer.cc \
src/ui/LogReplayLinkConfigurationWidget.cc \
src/ui/MainWindow.cc \ src/ui/MainWindow.cc \
src/ui/map/MAV2DIcon.cc \ src/ui/map/MAV2DIcon.cc \
src/ui/map/QGCMapTool.cc \ src/ui/map/QGCMapTool.cc \
......
...@@ -33,6 +33,7 @@ This file is part of the QGROUNDCONTROL project ...@@ -33,6 +33,7 @@ This file is part of the QGROUNDCONTROL project
#endif #endif
#include "UDPLink.h" #include "UDPLink.h"
#include "TCPLink.h" #include "TCPLink.h"
#include "LogReplayLink.h"
#ifdef QT_DEBUG #ifdef QT_DEBUG
#include "MockLink.h" #include "MockLink.h"
...@@ -95,6 +96,9 @@ LinkConfiguration* LinkConfiguration::createSettings(int type, const QString& na ...@@ -95,6 +96,9 @@ LinkConfiguration* LinkConfiguration::createSettings(int type, const QString& na
case LinkConfiguration::TypeTcp: case LinkConfiguration::TypeTcp:
config = new TCPConfiguration(name); config = new TCPConfiguration(name);
break; break;
case LinkConfiguration::TypeLogReplay:
config = new LogReplayLinkConfiguration(name);
break;
#ifdef QT_DEBUG #ifdef QT_DEBUG
case LinkConfiguration::TypeMock: case LinkConfiguration::TypeMock:
config = new MockConfiguration(name); config = new MockConfiguration(name);
...@@ -123,6 +127,9 @@ LinkConfiguration* LinkConfiguration::duplicateSettings(LinkConfiguration* sourc ...@@ -123,6 +127,9 @@ LinkConfiguration* LinkConfiguration::duplicateSettings(LinkConfiguration* sourc
case TypeTcp: case TypeTcp:
dupe = new TCPConfiguration(dynamic_cast<TCPConfiguration*>(source)); dupe = new TCPConfiguration(dynamic_cast<TCPConfiguration*>(source));
break; break;
case TypeLogReplay:
dupe = new LogReplayLinkConfiguration(dynamic_cast<LogReplayLinkConfiguration*>(source));
break;
#ifdef QT_DEBUG #ifdef QT_DEBUG
case TypeMock: case TypeMock:
dupe = new MockConfiguration(dynamic_cast<MockConfiguration*>(source)); dupe = new MockConfiguration(dynamic_cast<MockConfiguration*>(source));
......
...@@ -44,13 +44,14 @@ public: ...@@ -44,13 +44,14 @@ public:
#endif #endif
TypeUdp, ///< UDP Link TypeUdp, ///< UDP Link
TypeTcp, ///< TCP Link TypeTcp, ///< TCP Link
// TODO Below is not yet implemented
#if 0 #if 0
// TODO Below is not yet implemented
TypeForwarding, ///< Forwarding Link TypeForwarding, ///< Forwarding Link
TypeXbee, ///< XBee Proprietary Link TypeXbee, ///< XBee Proprietary Link
TypeOpal, ///< Opal-RT Link TypeOpal, ///< Opal-RT Link
#endif #endif
TypeMock, ///< Mock Link for Unitesting TypeMock, ///< Mock Link for Unitesting
TypeLogReplay,
TypeLast // Last type value (type >= TypeLast == invalid) TypeLast // Last type value (type >= TypeLast == invalid)
}; };
......
...@@ -91,6 +91,9 @@ public: ...@@ -91,6 +91,9 @@ public:
* @return The nominal data rate of the interface in bit per second, 0 if unknown * @return The nominal data rate of the interface in bit per second, 0 if unknown
**/ **/
virtual qint64 getConnectionSpeed() const = 0; virtual qint64 getConnectionSpeed() const = 0;
/// @return true: This link is replaying a log file, false: Normal two-way communication link
virtual bool isLogReplay(void) { return false; }
/** /**
* @Brief Get the current incoming data rate. * @Brief Get the current incoming data rate.
......
...@@ -96,6 +96,9 @@ LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config) ...@@ -96,6 +96,9 @@ LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config)
case LinkConfiguration::TypeTcp: case LinkConfiguration::TypeTcp:
pLink = new TCPLink(dynamic_cast<TCPConfiguration*>(config)); pLink = new TCPLink(dynamic_cast<TCPConfiguration*>(config));
break; break;
case LinkConfiguration::TypeLogReplay:
pLink = new LogReplayLink(dynamic_cast<LogReplayLinkConfiguration*>(config));
break;
#ifdef QT_DEBUG #ifdef QT_DEBUG
case LinkConfiguration::TypeMock: case LinkConfiguration::TypeMock:
pLink = new MockLink(dynamic_cast<MockConfiguration*>(config)); pLink = new MockLink(dynamic_cast<MockConfiguration*>(config));
...@@ -394,6 +397,10 @@ void LinkManager::loadLinkConfigurationList() ...@@ -394,6 +397,10 @@ void LinkManager::loadLinkConfigurationList()
pLink = (LinkConfiguration*)new TCPConfiguration(name); pLink = (LinkConfiguration*)new TCPConfiguration(name);
pLink->setPreferred(preferred); pLink->setPreferred(preferred);
break; break;
case LinkConfiguration::TypeLogReplay:
pLink = (LinkConfiguration*)new LogReplayLinkConfiguration(name);
pLink->setPreferred(preferred);
break;
#ifdef QT_DEBUG #ifdef QT_DEBUG
case LinkConfiguration::TypeMock: case LinkConfiguration::TypeMock:
pLink = (LinkConfiguration*)new MockConfiguration(name); pLink = (LinkConfiguration*)new MockConfiguration(name);
......
...@@ -41,6 +41,7 @@ This file is part of the PIXHAWK project ...@@ -41,6 +41,7 @@ This file is part of the PIXHAWK project
#endif #endif
#include "UDPLink.h" #include "UDPLink.h"
#include "TCPLink.h" #include "TCPLink.h"
#include "LogReplayLink.h"
#ifdef QT_DEBUG #ifdef QT_DEBUG
#include "MockLink.h" #include "MockLink.h"
...@@ -110,6 +111,7 @@ public: ...@@ -110,6 +111,7 @@ public:
void setConnectionsAllowed(void) { _connectionsSuspended = false; } void setConnectionsAllowed(void) { _connectionsSuspended = false; }
/// Creates, connects (and adds) a link based on the given configuration instance. /// Creates, connects (and adds) a link based on the given configuration instance.
/// Link takes ownership of config.
LinkInterface* createConnectedLink(LinkConfiguration* config); LinkInterface* createConnectedLink(LinkConfiguration* config);
/// Creates, connects (and adds) a link based on the given configuration name. /// Creates, connects (and adds) a link based on the given configuration name.
......
This diff is collapsed.
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
#ifndef LogReplayLink_H
#define LogReplayLink_H
#include "LinkInterface.h"
#include "LinkConfiguration.h"
#include "MAVLinkProtocol.h"
#include <QTimer>
#include <QFile>
class LogReplayLinkConfiguration : public LinkConfiguration
{
public:
LogReplayLinkConfiguration(const QString& name);
LogReplayLinkConfiguration(LogReplayLinkConfiguration* copy);
QString logFilename(void) { return _logFilename; }
void setLogFilename(const QString& logFilename) { _logFilename = logFilename; }
QString logFilenameShort(void);
// Virtuals from LinkConfiguration
virtual int type() { return LinkConfiguration::TypeLogReplay; }
virtual void copyFrom(LinkConfiguration* source);
virtual void loadSettings(QSettings& settings, const QString& root);
virtual void saveSettings(QSettings& settings, const QString& root);
virtual void updateSettings();
private:
static const char* _logFilenameKey;
QString _logFilename;
};
class LogReplayLink : public LinkInterface
{
Q_OBJECT
friend class LinkManager;
public:
/// @return true: log is currently playing, false: log playback is paused
bool isPlaying(void) { return _readTickTimer.isActive(); }
/// Start replay at current position
void play(void) { emit _playOnThread(); }
/// Pause replay
void pause(void) { emit _pauseOnThread(); }
/// Move the playhead to the specified percent complete
void movePlayhead(int percentComplete);
/// Sets the acceleration factor: -100: 0.01X, 0: 1.0X, 100: 100.0X
void setAccelerationFactor(int factor) { emit _setAccelerationFactorOnThread(factor); }
// Virtuals from LinkInterface
virtual QString getName(void) const { return _config->name(); }
virtual void requestReset(void){ }
virtual bool isConnected(void) const { return _connected; }
virtual qint64 getConnectionSpeed(void) const { return 100000000; }
virtual qint64 bytesAvailable(void) { return 0; }
virtual bool isLogReplay(void) { return true; }
// These are left unimplemented in order to cause linker errors which indicate incorrect usage of
// connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
bool connect(void);
bool disconnect(void);
public slots:
virtual void writeBytes(const char *bytes, qint64 cBytes);
signals:
void logFileStats(bool logTimestamped, int logDurationSecs, int binaryBaudRate);
void playbackStarted(void);
void playbackPaused(void);
void playbackAtEnd(void);
void playbackError(void);
void playbackPercentCompleteChanged(int percentComplete);
// Internal signals
void _playOnThread(void);
void _pauseOnThread(void);
void _setAccelerationFactorOnThread(int factor);
protected slots:
// FIXME: This should not be part of LinkInterface. It is an internal link implementation detail.
virtual void readBytes(void);
private slots:
void _readNextLogEntry(void);
void _play(void);
void _pause(void);
void _setAccelerationFactor(int factor);
private:
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
LogReplayLink(LogReplayLinkConfiguration* config);
~LogReplayLink();
void _replayError(const QString& errorMsg);
quint64 _parseTimestamp(const QByteArray& bytes);
quint64 _seekToNextMavlinkMessage(mavlink_message_t* nextMsg);
bool _loadLogFile(void);
void _finishPlayback(void);
void _playbackError(void);
void _resetPlaybackToBeginning(void);
// Virtuals from LinkInterface
virtual bool _connect(void);
virtual bool _disconnect(void);
// Virtuals from QThread
virtual void run(void);
LogReplayLinkConfiguration* _config;
bool _connected;
QTimer _readTickTimer; ///< Timer which signals a read of next log record
static const char* _errorTitle; ///< Title for communicatorError signals
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;
static const int _defaultBinaryBaudRate = 57600;
int _binaryBaudRate; ///< Playback rate for binary log format
float _replayAccelerationFactor; ///< Factor to apply to playback rate
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.
MAVLinkProtocol* _mavlink;
QFile _logFile;
quint64 _logFileSize;
bool _logTimestamped; ///< true: Timestamped log format, false: no timestamps
static const int cbTimestamp = sizeof(quint64);
};
#endif
...@@ -353,17 +353,12 @@ qint64 SerialLink::getConnectionSpeed() const ...@@ -353,17 +353,12 @@ qint64 SerialLink::getConnectionSpeed() const
void SerialLink::_resetConfiguration() void SerialLink::_resetConfiguration()
{ {
bool somethingChanged = false;
if (_port) { if (_port) {
somethingChanged = _port->setBaudRate (_config->baud()); _port->setBaudRate (_config->baud());
somethingChanged |= _port->setDataBits (static_cast<QSerialPort::DataBits> (_config->dataBits())); _port->setDataBits (static_cast<QSerialPort::DataBits> (_config->dataBits()));
somethingChanged |= _port->setFlowControl (static_cast<QSerialPort::FlowControl> (_config->flowControl())); _port->setFlowControl (static_cast<QSerialPort::FlowControl> (_config->flowControl()));
somethingChanged |= _port->setStopBits (static_cast<QSerialPort::StopBits> (_config->stopBits())); _port->setStopBits (static_cast<QSerialPort::StopBits> (_config->stopBits()));
somethingChanged |= _port->setParity (static_cast<QSerialPort::Parity> (_config->parity())); _port->setParity (static_cast<QSerialPort::Parity> (_config->parity()));
}
if(somethingChanged) {
qCDebug(SerialLinkLog) << "Reconfiguring port";
emit updateLink(this);
} }
} }
......
...@@ -119,14 +119,12 @@ public: ...@@ -119,14 +119,12 @@ public:
void requestReset(); void requestReset();
bool isConnected() const; bool isConnected() const;
qint64 getConnectionSpeed() const; qint64 getConnectionSpeed() const;
// These are left unimplemented in order to cause linker errors which indicate incorrect usage of // These are left unimplemented in order to cause linker errors which indicate incorrect usage of
// connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager. // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
bool connect(void); bool connect(void);
bool disconnect(void); bool disconnect(void);
signals: //[TODO] Refactor to Linkinterface
void updateLink(LinkInterface*);
public slots: public slots:
void readBytes(); void readBytes();
......
...@@ -3414,3 +3414,14 @@ bool UAS::_containsLink(LinkInterface* link) ...@@ -3414,3 +3414,14 @@ bool UAS::_containsLink(LinkInterface* link)
return false; return false;
} }
bool UAS::isLogReplay(void)
{
QList<LinkInterface*> links = getLinks();
if (links.count() == 1) {
return links[0]->isLogReplay();
} else {
return false;
}
}
...@@ -93,6 +93,7 @@ public: ...@@ -93,6 +93,7 @@ public:
float filterVoltage(float value) const; float filterVoltage(float value) const;
/** @brief Get the links associated with this robot */ /** @brief Get the links associated with this robot */
QList<LinkInterface*> getLinks(); QList<LinkInterface*> getLinks();
bool isLogReplay(void);
Q_PROPERTY(double localX READ getLocalX WRITE setLocalX NOTIFY localXChanged) Q_PROPERTY(double localX READ getLocalX WRITE setLocalX NOTIFY localXChanged)
Q_PROPERTY(double localY READ getLocalY WRITE setLocalY NOTIFY localYChanged) Q_PROPERTY(double localY READ getLocalY WRITE setLocalY NOTIFY localYChanged)
......
...@@ -166,6 +166,9 @@ public: ...@@ -166,6 +166,9 @@ public:
* interface. The LinkInterface can support multiple protocols. * interface. The LinkInterface can support multiple protocols.
**/ **/
virtual QList<LinkInterface*> getLinks() = 0; virtual QList<LinkInterface*> getLinks() = 0;
/// @returns true: UAS is connected to log replay link
virtual bool isLogReplay(void) = 0;
/** /**
* @brief Get the color for this UAS * @brief Get the color for this UAS
......
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
#include "LogReplayLinkConfigurationWidget.h"
#include "QGCFileDialog.h"
#include "QGCApplication.h"
LogReplayLinkConfigurationWidget::LogReplayLinkConfigurationWidget(LogReplayLinkConfiguration *config, QWidget *parent, Qt::WindowFlags flags) :
QWidget(parent, flags)
{
_ui.setupUi(this);
Q_ASSERT(config != NULL);
_config = config;
_ui.logFilename->setText(_config->logFilename());
connect(_ui.selectLogFileButton, &QPushButton::clicked, this, &LogReplayLinkConfigurationWidget::_selectLogFile);
}
void LogReplayLinkConfigurationWidget::_selectLogFile(bool checked)
{
Q_UNUSED(checked);
QString logFile = QGCFileDialog::getOpenFileName(this,
"Select log file to replay",
qgcApp()->mavlinkLogFilesLocation(),
"MAVLink Log Files (*.mavlink);;All Files (*)");
if (!logFile.isEmpty()) {
_ui.logFilename->setText(logFile);
_config->setLogFilename(logFile);
}
}
\ No newline at end of file
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
#ifndef _LogReplayLinkConfigurationWidget_H_
#define _LogReplayLinkConfigurationWidget_H_
#include <QWidget>
#include "LogReplayLink.h"
#include "ui_LogReplayLinkConfigurationWidget.h"
class LogReplayLinkConfigurationWidget : public QWidget
{
Q_OBJECT
public:
LogReplayLinkConfigurationWidget(LogReplayLinkConfiguration* config, QWidget *parent = 0, Qt::WindowFlags flags = Qt::Sheet);
private slots:
void _selectLogFile(bool checked);
private:
Ui::LogReplayLinkConfigurationWidget _ui;
LogReplayLinkConfiguration* _config;
};
#endif
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>LogReplayLinkConfigurationWidget</class>
<widget class="QWidget" name="LogReplayLinkConfigurationWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>325</width>
<height>347</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_7" stretch="0">
<item>
<widget class="QLabel" name="label">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Log File:</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QLabel" name="logFilename">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>60</height>
</size>
</property>
<property name="text">
<string/>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="selectLogFileButton">
<property name="text">
<string>Select Log File</string>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_8"/>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
<action name="actionDelete">
<property name="text">
<string>Delete</string>
</property>
<property name="toolTip">
<string>Delete this link</string>
</property>
<property name="statusTip">
<string>Delete this link</string>
</property>
<property name="whatsThis">
<string>Link delete button</string>
</property>
</action>
<action name="actionConnect">
<property name="text">
<string>Connect</string>
</property>
<property name="toolTip">
<string>Connect this link</string>
</property>
<property name="statusTip">
<string>Connect this link</string>
</property>
<property name="whatsThis">
<string>Connect this link</string>
</property>
</action>
<action name="actionClose">
<property name="text">
<string>Close</string>
</property>
<property name="toolTip">
<string>Close the configuration window</string>
</property>
<property name="statusTip">
<string>Close the configuration window</string>
</property>
<property name="whatsThis">
<string>Close the configuration window</string>
</property>
</action>
</widget>
<resources/>
<connections>
<connection>
<sender>actionClose</sender>
<signal>triggered()</signal>
<receiver>LogReplayLinkConfigurationWidget</receiver>
<slot>close()</slot>
<hints>
<hint type="sourcelabel">
<x>-1</x>
<y>-1</y>
</hint>
<hint type="destinationlabel">
<x>224</x>
<y>195</y>
</hint>
</hints>
</connection>
</connections>
</ui>
...@@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project ...@@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project
#endif #endif
#include "QGCUDPLinkConfiguration.h" #include "QGCUDPLinkConfiguration.h"
#include "QGCTCPLinkConfiguration.h" #include "QGCTCPLinkConfiguration.h"
#include "LogReplayLinkConfigurationWidget.h"
#include "QGCCommConfiguration.h" #include "QGCCommConfiguration.h"
#include "ui_QGCCommConfiguration.h" #include "ui_QGCCommConfiguration.h"
...@@ -52,6 +53,7 @@ QGCCommConfiguration::QGCCommConfiguration(QWidget *parent, LinkConfiguration *c ...@@ -52,6 +53,7 @@ QGCCommConfiguration::QGCCommConfiguration(QWidget *parent, LinkConfiguration *c
#endif #endif
_ui->typeCombo->addItem(tr("UDP"), LinkConfiguration::TypeUdp); _ui->typeCombo->addItem(tr("UDP"), LinkConfiguration::TypeUdp);
_ui->typeCombo->addItem(tr("TCP"), LinkConfiguration::TypeTcp); _ui->typeCombo->addItem(tr("TCP"), LinkConfiguration::TypeTcp);
_ui->typeCombo->addItem(tr("Log replay"), LinkConfiguration::TypeLogReplay);
#ifdef QT_DEBUG #ifdef QT_DEBUG
_ui->typeCombo->addItem(tr("Mock"), LinkConfiguration::TypeMock); _ui->typeCombo->addItem(tr("Mock"), LinkConfiguration::TypeMock);
#endif #endif
...@@ -140,6 +142,13 @@ void QGCCommConfiguration::_loadTypeConfigWidget(int type) ...@@ -140,6 +142,13 @@ void QGCCommConfiguration::_loadTypeConfigWidget(int type)
_ui->typeCombo->setCurrentIndex(_ui->typeCombo->findData(LinkConfiguration::TypeTcp)); _ui->typeCombo->setCurrentIndex(_ui->typeCombo->findData(LinkConfiguration::TypeTcp));
} }
break; break;
case LinkConfiguration::TypeLogReplay: {
QWidget* conf = new LogReplayLinkConfigurationWidget((LogReplayLinkConfiguration*)_config, this);
_ui->linkScrollArea->setWidget(conf);
_ui->linkGroupBox->setTitle("Log Replay");
_ui->typeCombo->setCurrentIndex(_ui->typeCombo->findData(LinkConfiguration::TypeLogReplay));
}
break;
#ifdef QT_DEBUG #ifdef QT_DEBUG
case LinkConfiguration::TypeMock: { case LinkConfiguration::TypeMock: {
_ui->linkScrollArea->setWidget(NULL); _ui->linkScrollArea->setWidget(NULL);
......
...@@ -160,6 +160,13 @@ void QGCLinkConfiguration::_fixUnnamed(LinkConfiguration* config) ...@@ -160,6 +160,13 @@ void QGCLinkConfiguration::_fixUnnamed(LinkConfiguration* config)
} }
} }
break; break;
case LinkConfiguration::TypeLogReplay: {
LogReplayLinkConfiguration* tconfig = dynamic_cast<LogReplayLinkConfiguration*>(config);
if(tconfig) {
config->setName(QString("Log Replay %1").arg(tconfig->logFilenameShort()));
}
}
break;
#ifdef QT_DEBUG #ifdef QT_DEBUG
case LinkConfiguration::TypeMock: case LinkConfiguration::TypeMock:
config->setName( config->setName(
......
This diff is collapsed.
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
#include "MAVLinkProtocol.h" #include "MAVLinkProtocol.h"
#include "LinkInterface.h" #include "LinkInterface.h"
#include "MockLink.h" #include "LogReplayLink.h"
namespace Ui namespace Ui
{ {
...@@ -27,97 +27,29 @@ class QGCMAVLinkLogPlayer : public QWidget ...@@ -27,97 +27,29 @@ class QGCMAVLinkLogPlayer : public QWidget
public: public:
explicit QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *parent = 0); explicit QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *parent = 0);
~QGCMAVLinkLogPlayer(); ~QGCMAVLinkLogPlayer();
bool isPlayingLogFile()
{
return isPlaying;
}
bool isLogFileSelected()
{
return logFile.isOpen();
}
public slots:
/** @brief Toggle between play and pause */
void playPauseToggle();
/** @brief Replay the logfile */
void play();
/** @brief Pause the log player. */
void pause();
/** @brief Reset the internal log player state, including the UI */
void reset();
/** @brief Load log file */
bool loadLogFile(const QString& file);
/** @brief Jump to a position in the logfile */
void jumpToSliderVal(int slidervalue);
/** @brief The logging mainloop */
void logLoop();
/** @brief Set acceleration factor in percent */
void setAccelerationFactorInt(int factor);
signals:
/** @brief Send ready bytes */
void bytesReady(LinkInterface* link, const QByteArray& bytes);
void logFileEndReached();
protected:
quint64 playbackStartTime; ///< 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. In units of milliseconds since epoch UTC.
quint64 logCurrentTime; ///< The timestamp of the next message in the log file. In units of microseconds since epoch UTC.
quint64 logStartTime; ///< The first timestamp in the current log file. In units of microseconds since epoch UTC.
quint64 logEndTime; ///< The last timestamp in the current log file. In units of microseconds since epoch UTC.
float accelerationFactor;
MAVLinkProtocol* mavlink;
MockLink* logLink;
QFile logFile;
QTimer loopTimer;
int loopCounter;
bool mavlinkLogFormat; ///< If the logfile is stored in the timestamped MAVLink log format
int binaryBaudRate;
static const int defaultBinaryBaudRate = 57600;
bool isPlaying;
unsigned int currPacketCount;
static const int packetLen = MAVLINK_MAX_PACKET_LEN;
static const int timeLen = sizeof(quint64);
void changeEvent(QEvent *e);
private slots: private slots:
void _selectLogFileForPlayback(void); void _selectLogFileForPlayback(void);
void _playPauseToggle(void);
void _pause(void);
void _setPlayheadFromSlider(int value);
void _setAccelerationFromSlider(int value);
void _logFileStats(bool logTimestamped, int logDurationSeconds, int binaryBaudRate);
void _playbackStarted(void);
void _playbackPaused(void);
void _playbackPercentCompleteChanged(int percentComplete);
void _playbackError(void);
void _replayLinkDisconnected(void);
private: private:
Ui::QGCMAVLinkLogPlayer *ui; void _finishPlayback(void);
virtual void paintEvent(QPaintEvent *); QString _secondsToHMS(int seconds);
void _enablePlaybackControls(bool enabled);
/** @brief Parse out a quint64 timestamp in microseconds in the proper endianness. */
quint64 parseTimestamp(const QByteArray &data);
/**
* This function parses out the next MAVLink message and its corresponding timestamp.
*
* It makes no assumptions about where in the file we currently are. It leaves the file right
* at the beginning of the successfully parsed message. Note that this function will not attempt to
* correct for any MAVLink parsing failures, so it always returns the next successfully-parsed
* message.
*
* @param msg[output] Where the final parsed message output will go.
* @return A Unix timestamp in microseconds UTC or 0 if parsing failed
*/
quint64 findNextMavlinkMessage(mavlink_message_t *msg);
/**
* Updates the QSlider UI to be at the given percentage.
* @param percent A percentage value between 0.0% and 100.0%.
*/
void updatePositionSliderUi(float percent);
/** LogReplayLink* _replayLink;
* Jumps to a new position in the current playback file as a percentage. int _logDurationSeconds;
* @param percentage The position of the file to jump to as a percentage.
* @return True if the new file position was successfully jumped to, false otherwise
*/
bool jumpToPlaybackLocation(float percentage);
void _finishPlayback(void); Ui::QGCMAVLinkLogPlayer* _ui;
void _playbackError(void);
}; };
#endif // QGCMAVLINKLOGPLAYER_H #endif
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>789</width> <width>948</width>
<height>38</height> <height>38</height>
</rect> </rect>
</property> </property>
...@@ -49,7 +49,7 @@ ...@@ -49,7 +49,7 @@
</property> </property>
<property name="icon"> <property name="icon">
<iconset resource="../../qgroundcontrol.qrc"> <iconset resource="../../qgroundcontrol.qrc">
<normaloff>:/res/Play</normaloff>:/res.Play</iconset> <normaloff>:/res/Play</normaloff>:/res/Play</iconset>
</property> </property>
<property name="checkable"> <property name="checkable">
<bool>true</bool> <bool>true</bool>
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment