@ -13,238 +13,50 @@
@@ -13,238 +13,50 @@
# include "QGCFileDialog.h"
# include "QGCMessageBox.h"
QGCMAVLinkLogPlayer : : QGCMAVLinkLogPlayer ( MAVLinkProtocol * mavlink , QWidget * parent ) :
QGCMAVLinkLogPlayer : : QGCMAVLinkLogPlayer ( QWidget * parent ) :
QWidget ( parent ) ,
playbackStartTime ( 0 ) ,
logStartTime ( 0 ) ,
logEndTime ( 0 ) ,
accelerationFactor ( 1.0f ) ,
mavlink ( mavlink ) ,
logLink ( NULL ) ,
loopCounter ( 0 ) ,
mavlinkLogFormat ( true ) ,
binaryBaudRate ( defaultBinaryBaudRate ) ,
isPlaying ( false ) ,
currPacketCount ( 0 ) ,
ui ( new Ui : : QGCMAVLinkLogPlayer )
_replayLink ( NULL ) ,
_ui ( new Ui : : QGCMAVLinkLogPlayer )
{
Q_ASSERT ( mavlink ) ;
ui - > setupUi ( this ) ;
ui - > horizontalLayout - > setAlignment ( Qt : : AlignTop ) ;
// Connect protocol
connect ( this , SIGNAL ( bytesReady ( LinkInterface * , QByteArray ) ) , mavlink , SLOT ( receiveBytes ( LinkInterface * , QByteArray ) ) ) ;
// Setup timer
connect ( & loopTimer , & QTimer : : timeout , this , & QGCMAVLinkLogPlayer : : logLoop ) ;
_ui - > setupUi ( this ) ;
_ui - > horizontalLayout - > setAlignment ( Qt : : AlignTop ) ;
// Setup buttons
connect ( ui - > selectFileButton , & QPushButton : : clicked , this , & QGCMAVLinkLogPlayer : : _selectLogFileForPlayback ) ;
connect ( ui - > playButton , & QPushButton : : clicked , this , & QGCMAVLinkLogPlayer : : playPauseToggle ) ;
connect ( ui - > speedSlider , & QSlider : : valueChanged , this , & QGCMAVLinkLogPlayer : : setAccelerationFactorInt ) ;
connect ( ui - > positionSlider , & QSlider : : valueChanged , this , & QGCMAVLinkLogPlayer : : jumpToSliderVal ) ;
connect ( ui - > positionSlider , & QSlider : : sliderPressed , this , & QGCMAVLinkLogPlayer : : pause ) ;
setAccelerationFactorInt ( 49 ) ;
ui - > speedSlider - > setValue ( 49 ) ;
updatePositionSliderUi ( 0.0 ) ;
ui - > playButton - > setEnabled ( false ) ;
ui - > speedSlider - > setEnabled ( false ) ;
ui - > positionSlider - > setEnabled ( false ) ;
ui - > speedLabel - > setEnabled ( false ) ;
ui - > logFileNameLabel - > setEnabled ( false ) ;
ui - > logStatsLabel - > setEnabled ( false ) ;
// Monitor for when the end of the log file is reached. This is done using signals because the main work is in a timer.
connect ( this , & QGCMAVLinkLogPlayer : : logFileEndReached , & loopTimer , & QTimer : : stop ) ;
}
QGCMAVLinkLogPlayer : : ~ QGCMAVLinkLogPlayer ( )
{
delete ui ;
}
void QGCMAVLinkLogPlayer : : playPauseToggle ( )
{
if ( isPlaying )
{
pause ( ) ;
}
else
{
play ( ) ;
}
}
void QGCMAVLinkLogPlayer : : play ( )
{
Q_ASSERT ( logFile . isOpen ( ) ) ;
LinkManager : : instance ( ) - > setConnectionsSuspended ( tr ( " Connect not allowed during Flight Data replay. " ) ) ;
mavlink - > suspendLogForReplay ( true ) ;
// Disable the log file selector button
ui - > selectFileButton - > setEnabled ( false ) ;
// 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 ( ) )
{
reset ( ) ;
}
// Always correct the current start time such that the next message will play immediately at playback.
// We do this by subtracting the current file playback offset from now()
playbackStartTime = ( quint64 ) QDateTime : : currentMSecsSinceEpoch ( ) - ( logCurrentTime - logStartTime ) / 1000 ;
// Start timer
if ( mavlinkLogFormat )
{
loopTimer . start ( 1 ) ;
}
else
{
// Read len bytes at a time
int len = 100 ;
// Calculate the number of times to read 100 bytes per second
// to guarantee the baud rate, then divide 1000 by the number of read
// operations to obtain the interval in milliseconds
int interval = 1000 / ( ( binaryBaudRate / 10 ) / len ) ;
loopTimer . start ( interval / accelerationFactor ) ;
}
isPlaying = true ;
ui - > playButton - > setChecked ( true ) ;
ui - > playButton - > setIcon ( QIcon ( " :/res/Pause " ) ) ;
}
void QGCMAVLinkLogPlayer : : pause ( )
{
LinkManager : : instance ( ) - > setConnectionsAllowed ( ) ;
mavlink - > suspendLogForReplay ( false ) ;
loopTimer . stop ( ) ;
isPlaying = false ;
ui - > playButton - > setIcon ( QIcon ( " :/res/Play " ) ) ;
ui - > playButton - > setChecked ( false ) ;
ui - > selectFileButton - > setEnabled ( true ) ;
}
void QGCMAVLinkLogPlayer : : reset ( )
{
pause ( ) ;
loopCounter = 0 ;
if ( logFile . isOpen ( ) ) {
logFile . reset ( ) ;
}
connect ( _ui - > selectFileButton , & QPushButton : : clicked , this , & QGCMAVLinkLogPlayer : : _selectLogFileForPlayback ) ;
connect ( _ui - > playButton , & QPushButton : : clicked , this , & QGCMAVLinkLogPlayer : : _playPauseToggle ) ;
connect ( _ui - > speedSlider , & QSlider : : valueChanged , this , & QGCMAVLinkLogPlayer : : _setAccelerationFromSlider ) ;
connect ( _ui - > positionSlider , & QSlider : : valueChanged , this , & QGCMAVLinkLogPlayer : : _setPlayheadFromSlider ) ;
connect ( _ui - > positionSlider , & QSlider : : sliderPressed , this , & QGCMAVLinkLogPlayer : : _pause ) ;
// Now update the position slider to its default location
updatePositionSliderUi ( 0.0 ) ;
_enablePlaybackControls ( false ) ;
// And since we haven't starting playback, clear the time of initial playback and the current timestamp.
playbackStartTime = 0 ;
logCurrentTime = logStartTime ;
}
_ui - > positionSlider - > setMinimum ( 0 ) ;
_ui - > positionSlider - > setMaximum ( 100 ) ;
bool QGCMAVLinkLogPlayer : : jumpToPlaybackLocation ( float percentage )
{
// Reset only for valid values
if ( percentage < = 100.0f & & percentage > = 0.0f )
{
bool result = true ;
if ( mavlinkLogFormat )
{
// 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 ) ( percentage * ( float ) logFile . size ( ) ) ;
// Now seek to the appropriate position, failing gracefully if we can't.
if ( ! logFile . seek ( newFilePos ) )
{
// Fallback: Start from scratch
logFile . reset ( ) ;
ui - > logStatsLabel - > setText ( tr ( " Changing packet index failed, back to start. " ) ) ;
result = false ;
_ui - > speedSlider - > setMinimum ( - 100 ) ;
_ui - > speedSlider - > setMaximum ( 100 ) ;
_ui - > speedSlider - > setValue ( 0 ) ;
}
// But we do align to the next MAVLink message for consistency.
mavlink_message_t dummy ;
logCurrentTime = findNextMavlinkMessage ( & dummy ) ;
// Now calculate the current file location based on time.
float newRelativeTime = ( float ) ( logCurrentTime - logStartTime ) ;
// Calculate the effective baud rate of the file in bytes/s.
float logDuration = ( logEndTime - logStartTime ) ;
float baudRate = logFile . size ( ) / logDuration / 1e6 ;
// And the desired time is:
float desiredTime = percentage * logDuration ;
// And now jump the necessary number of bytes in the proper direction
qint64 offset = ( newRelativeTime - desiredTime ) * baudRate ;
logFile . seek ( logFile . pos ( ) + offset ) ;
// And scan until we reach the start of a MAVLink message. We make sure to record this timestamp for
// smooth jumping around the file.
logCurrentTime = findNextMavlinkMessage ( & dummy ) ;
// Now update the UI with our actual final position.
newRelativeTime = ( float ) ( logCurrentTime - logStartTime ) ;
percentage = newRelativeTime / logDuration ;
updatePositionSliderUi ( percentage ) ;
}
else
{
// If we're working with a non-timestamped file, we just jump to that percentage of the file,
// align to the next MAVLink message and roll with it. No reason to do anything more complicated.
qint64 newFilePos = ( qint64 ) ( percentage * ( float ) logFile . size ( ) ) ;
// Now seek to the appropriate position, failing gracefully if we can't.
if ( ! logFile . seek ( newFilePos ) )
QGCMAVLinkLogPlayer : : ~ QGCMAVLinkLogPlayer ( )
{
// Fallback: Start from scratch
logFile . reset ( ) ;
ui - > logStatsLabel - > setText ( tr ( " Changing packet index failed, back to start. " ) ) ;
result = false ;
}
// But we do align to the next MAVLink message for consistency.
mavlink_message_t dummy ;
findNextMavlinkMessage ( & dummy ) ;
delete _ui ;
}
// Now update the UI. This is necessary because stop() is called when loading a new logfile
return result ;
}
else
void QGCMAVLinkLogPlayer : : _playPauseToggle ( void )
{
return false ;
if ( _replayLink - > isPlaying ( ) ) {
_pause ( ) ;
} else {
_replayLink - > play ( ) ;
}
}
void QGCMAVLinkLogPlayer : : updatePositionSliderUi ( float percent )
void QGCMAVLinkLogPlayer : : _pause ( void )
{
ui - > positionSlider - > blockSignals ( true ) ;
int sliderVal = ui - > positionSlider - > minimum ( ) + ( int ) ( percent * ( float ) ( ui - > positionSlider - > maximum ( ) - ui - > positionSlider - > minimum ( ) ) ) ;
ui - > positionSlider - > setValue ( sliderVal ) ;
// Calculate the runtime in hours:minutes:seconds
// WARNING: Order matters in this computation
quint32 seconds = percent * ( logEndTime - logStartTime ) / 1e6 ;
quint32 minutes = seconds / 60 ;
quint32 hours = minutes / 60 ;
seconds - = 60 * minutes ;
minutes - = 60 * hours ;
// And show the user the details we found about this file.
QString timeLabel = tr ( " %1h:%2m:%3s " ) . arg ( hours , 2 ) . arg ( minutes , 2 ) . arg ( seconds , 2 ) ;
ui - > positionSlider - > setToolTip ( timeLabel ) ;
ui - > positionSlider - > blockSignals ( false ) ;
_replayLink - > pause ( ) ;
}
/// @brief Displays a file dialog to allow the user to select a log file to play back.
void QGCMAVLinkLogPlayer : : _selectLogFileForPlayback ( void )
{
// Disallow replay when any links are connected
@ -253,403 +65,127 @@ void QGCMAVLinkLogPlayer::_selectLogFileForPlayback(void)
@@ -253,403 +65,127 @@ void QGCMAVLinkLogPlayer::_selectLogFileForPlayback(void)
return ;
}
QString logFile = QGCFileDialog : : getOpenFileName (
QString logFilename = QGCFileDialog : : getOpenFileName (
this ,
tr ( " Load MAVLink Log File " ) ,
qgcApp ( ) - > mavlinkLogFilesLocation ( ) ,
tr ( " MAVLink Log Files (*.mavlink);;All Files (*) " ) ) ;
if ( ! logFile . isEmpty ( ) ) {
loadLogFile ( logFile ) ;
}
}
/**
* @ param factor 0 : 0.01 X , 50 : 1.0 X , 100 : 100.0 X
*/
void QGCMAVLinkLogPlayer : : setAccelerationFactorInt ( int factor )
{
float f = factor + 1.0f ;
f - = 50.0f ;
if ( f < 0.0f )
{
accelerationFactor = 1.0f / ( - f / 2.0f ) ;
}
else
{
accelerationFactor = 1 + ( f / 2.0f ) ;
}
// Update timer interval
if ( ! mavlinkLogFormat )
{
// Read len bytes at a time
int len = 100 ;
// Calculate the number of times to read 100 bytes per second
// to guarantee the baud rate, then divide 1000 by the number of read
// operations to obtain the interval in milliseconds
int interval = 1000 / ( ( binaryBaudRate / 10 ) / len ) ;
loopTimer . stop ( ) ;
loopTimer . start ( interval / accelerationFactor ) ;
if ( logFilename . isEmpty ( ) ) {
return ;
}
ui - > speedLabel - > setText ( tr ( " Speed: %1X " ) . arg ( accelerationFactor , 5 , ' f ' , 2 , ' 0 ' ) ) ;
}
LinkInterface * createConnectedLink ( LinkConfiguration * config ) ;
bool QGCMAVLinkLogPlayer : : loadLogFile ( const QString & file )
{
// Make sure to stop the logging process and reset everything.
reset ( ) ;
logFile . close ( ) ;
// Now load the new file.
logFile . setFileName ( file ) ;
if ( ! logFile . open ( QFile : : ReadOnly ) ) {
QGCMessageBox : : critical ( tr ( " Log Replay " ) , tr ( " The selected file is unreadable. Please make sure that the file %1 is readable or select a different file " ) . arg ( file ) ) ;
_playbackError ( ) ;
return false ;
}
LogReplayLinkConfiguration * linkConfig = new LogReplayLinkConfiguration ( QString ( " Log Replay " ) ) ;
linkConfig - > setLogFilename ( logFilename ) ;
linkConfig - > setName ( linkConfig - > logFilenameShort ( ) ) ;
_ui - > logFileNameLabel - > setText ( linkConfig - > logFilenameShort ( ) ) ;
_replayLink = ( LogReplayLink * ) LinkManager : : instance ( ) - > createConnectedLink ( linkConfig ) ;
QFileInfo logFileInfo ( file ) ;
ui - > logFileNameLabel - > setText ( tr ( " File: %1 " ) . arg ( logFileInfo . fileName ( ) ) ) ;
connect ( _replayLink , & LogReplayLink : : logFileStats , this , & QGCMAVLinkLogPlayer : : _logFileStats ) ;
connect ( _replayLink , & LogReplayLink : : playbackStarted , this , & QGCMAVLinkLogPlayer : : _playbackStarted ) ;
connect ( _replayLink , & LogReplayLink : : playbackPaused , this , & QGCMAVLinkLogPlayer : : _playbackPaused ) ;
connect ( _replayLink , & LogReplayLink : : playbackPercentCompleteChanged , this , & QGCMAVLinkLogPlayer : : _playbackPercentCompleteChanged ) ;
connect ( _replayLink , & LogReplayLink : : disconnected , this , & QGCMAVLinkLogPlayer : : _replayLinkDisconnected ) ;
// If there's an existing MAVLinkSimulationLink() being used for an old file,
// we replace it.
if ( logLink ) {
LinkManager : : instance ( ) - > _deleteLink ( logLink ) ;
_ui - > positionSlider - > setValue ( 0 ) ;
_ui - > speedSlider - > setValue ( 0 ) ;
}
logLink = new MockLink ( ) ;
LinkManager : : instance ( ) - > _addLink ( logLink ) ;
// Select if binary or MAVLink log format is used
mavlinkLogFormat = file . endsWith ( " .mavlink " ) ;
if ( mavlinkLogFormat )
void QGCMAVLinkLogPlayer : : _playbackError ( void )
{
// Get the first timestamp from the logfile
// This should be a big-endian uint64.
QByteArray timestamp = logFile . read ( timeLen ) ;
quint64 starttime = parseTimestamp ( timestamp ) ;
// Now find the last timestamp by scanning for the last MAVLink packet and
// find the timestamp before it. To do this we start searchin a little before
// the end of the file, specifically the maximum MAVLink packet size + the
// timestamp size. This guarantees that we will hit a MAVLink packet before
// the end of the file. Unfortunately, it basically guarantees that we will
// hit more than one. This is why we have to search for a bit.
qint64 fileLoc = logFile . size ( ) - MAVLINK_MAX_PACKET_LEN - timeLen ;
logFile . seek ( fileLoc ) ;
quint64 endtime = starttime ; // Set a sane default for the endtime
mavlink_message_t msg ;
quint64 newTimestamp ;
while ( ( newTimestamp = findNextMavlinkMessage ( & msg ) ) > endtime ) {
endtime = newTimestamp ;
}
if ( endtime = = starttime ) {
QGCMessageBox : : critical ( tr ( " Log Replay " ) , tr ( " The selected file is corrupt. No valid timestamps were found at the end of the file. " ) . arg ( file ) ) ;
_playbackError ( ) ;
return false ;
_ui - > logFileNameLabel - > setText ( " Error " ) ;
_enablePlaybackControls ( false ) ;
}
// Remember the start and end time so we can move around this logfile with the slider.
logEndTime = endtime ;
logStartTime = starttime ;
logCurrentTime = logStartTime ;
// Reset our log file so when we go to read it for the first time, we start at the beginning.
logFile . reset ( ) ;
// Calculate the runtime in hours:minutes:seconds
// WARNING: Order matters in this computation
quint32 seconds = ( endtime - starttime ) / 1000000 ;
quint32 minutes = seconds / 60 ;
quint32 hours = minutes / 60 ;
seconds - = 60 * minutes ;
minutes - = 60 * hours ;
// And show the user the details we found about this file.
QString timelabel = tr ( " %1h:%2m:%3s " ) . arg ( hours , 2 ) . arg ( minutes , 2 ) . arg ( seconds , 2 ) ;
currPacketCount = logFileInfo . size ( ) / ( 32 + MAVLINK_NUM_NON_PAYLOAD_BYTES + sizeof ( quint64 ) ) ; // Count packets by assuming an average payload size of 32 bytes
ui - > logStatsLabel - > setText ( tr ( " %2 MB, ~%3 packets, %4 " ) . arg ( logFileInfo . size ( ) / 1000000.0f , 0 , ' f ' , 2 ) . arg ( currPacketCount ) . arg ( timelabel ) ) ;
}
else
QString QGCMAVLinkLogPlayer : : _secondsToHMS ( int seconds )
{
// Load in binary mode. In this mode, files should be have a filename postfix
// of the baud rate they were recorded at, like `test_run_115200.bin`. Then on
// playback, the datarate is equal to set to this value.
// Set baud rate if any present. Otherwise we default to 57600.
QStringList parts = logFileInfo . baseName ( ) . split ( " _ " ) ;
binaryBaudRate = defaultBinaryBaudRate ;
if ( parts . count ( ) > 1 )
{
bool ok ;
int rate = parts . last ( ) . toInt ( & ok ) ;
// 9600 baud to 100 MBit
if ( ok & & ( rate > 9600 & & rate < 100000000 ) )
{
// Accept this as valid baudrate
binaryBaudRate = rate ;
}
}
int secondsPart = seconds ;
int minutesPart = secondsPart / 60 ;
int hoursPart = minutesPart / 60 ;
secondsPart - = 60 * minutesPart ;
minutesPart - = 60 * hoursPart ;
int seconds = logFileInfo . size ( ) / ( binaryBaudRate / 10 ) ;
int minutes = seconds / 60 ;
int hours = minutes / 60 ;
seconds - = 60 * minutes ;
minutes - = 60 * hours ;
QString timelabel = tr ( " %1h:%2m:%3s " ) . arg ( hours , 2 ) . arg ( minutes , 2 ) . arg ( seconds , 2 ) ;
ui - > logStatsLabel - > setText ( tr ( " %2 MB, %4 at %5 KB/s " ) . arg ( logFileInfo . size ( ) / 1000000.0f , 0 , ' f ' , 2 ) . arg ( timelabel ) . arg ( binaryBaudRate / 10.0f / 1024.0f , 0 , ' f ' , 2 ) ) ;
return QString ( " %1h:%2m:%3s " ) . arg ( hoursPart , 2 ) . arg ( minutesPart , 2 ) . arg ( secondsPart , 2 ) ;
}
// Enable controls
ui - > playButton - > setEnabled ( true ) ;
ui - > speedSlider - > setEnabled ( true ) ;
ui - > positionSlider - > setEnabled ( true ) ;
ui - > speedLabel - > setEnabled ( true ) ;
ui - > logFileNameLabel - > setEnabled ( true ) ;
ui - > logStatsLabel - > setEnabled ( true ) ;
play ( ) ;
return true ;
}
quint64 QGCMAVLinkLogPlayer : : parseTimestamp ( const QByteArray & data )
/// Signalled from LogReplayLink once log file information is known
void QGCMAVLinkLogPlayer : : _logFileStats ( bool logTimestamped , ///< true: timestamped log
int logDurationSeconds , ///< Log duration
int binaryBaudRate ) ///< Baud rate for non-timestamped log
{
// Retrieve the timestamp from the ByteArray assuming a proper BigEndian quint64 timestamp in microseconds.
quint64 timestamp = qFromBigEndian ( * ( ( quint64 * ) ( data . constData ( ) ) ) ) ;
// And get the current time in microseconds
quint64 currentTimestamp = ( ( quint64 ) QDateTime : : currentMSecsSinceEpoch ( ) ) * 1000 ;
Q_UNUSED ( logTimestamped ) ;
Q_UNUSED ( binaryBaudRate ) ;
// 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 ) ;
}
_logDurationSeconds = logDurationSeconds ;
return timestamp ;
_ui - > logStatsLabel - > setText ( _secondsToHMS ( logDurationSeconds ) ) ;
}
/**
* Jumps to the current percentage of the position slider . When this is called , the LogPlayer should already
* have been paused , so it just jumps to the proper location in the file and resumes playing .
*/
void QGCMAVLinkLogPlayer : : jumpToSliderVal ( int slidervalue )
{
// Determine what percentage through the file we should be (time or packet number depending).
float newLocation = slidervalue / ( float ) ( ui - > positionSlider - > maximum ( ) - ui - > positionSlider - > minimum ( ) ) ;
// And clamp our calculated values to the valid range of [0,100]
if ( newLocation > 100.0f )
{
newLocation = 100.0f ;
}
if ( newLocation < 0.0f )
/// Signalled from LogReplayLink when replay starts
void QGCMAVLinkLogPlayer : : _playbackStarted ( void )
{
newLocation = 0.0f ;
_enablePlaybackControls ( true ) ;
_ui - > playButton - > setChecked ( true ) ;
_ui - > playButton - > setIcon ( QIcon ( " :/res/Pause " ) ) ;
}
// Do only valid jumps
if ( jumpToPlaybackLocation ( newLocation ) )
{
if ( mavlinkLogFormat )
/// Signalled from LogReplayLink when replay is paused
void QGCMAVLinkLogPlayer : : _playbackPaused ( void )
{
// Grab the total seconds of this file (1e6 is due to microsecond -> second conversion)
int seconds = newLocation * ( logEndTime - logStartTime ) / 1e6 ;
int minutes = seconds / 60 ;
int hours = minutes / 60 ;
seconds - = 60 * minutes ;
minutes - = 60 * hours ;
ui - > logStatsLabel - > setText ( tr ( " Jumped to time %1h:%2m:%3s " ) . arg ( hours , 2 ) . arg ( minutes , 2 ) . arg ( seconds , 2 ) ) ;
}
else
{
ui - > logStatsLabel - > setText ( tr ( " Jumped to %1 " ) . arg ( newLocation ) ) ;
_ui - > playButton - > setIcon ( QIcon ( " :/res/Play " ) ) ;
_ui - > playButton - > setChecked ( false ) ;
}
play ( ) ;
}
else
void QGCMAVLinkLogPlayer : : _playbackPercentCompleteChanged ( int percentComplete )
{
reset ( ) ;
}
_ui - > positionSlider - > blockSignals ( true ) ;
_ui - > positionSlider - > setValue ( percentComplete ) ;
_ui - > positionSlider - > blockSignals ( false ) ;
}
/**
* This function is the " mainloop " of the log player , reading one line
* and adjusting the mainloop timer to read the next line in time .
* It might not perfectly match the timing of the log file ,
* but it will never induce a static drift into the log file replay .
* For scientific logging , the use of onboard timestamps and the log
* functionality of the line chart plot is recommended .
*/
void QGCMAVLinkLogPlayer : : logLoop ( )
void QGCMAVLinkLogPlayer : : _setPlayheadFromSlider ( int value )
{
// If we have a file with timestamps, try and pace this out following the time differences
// between the timestamps and the current playback speed.
if ( mavlinkLogFormat )
{
// 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 nextExecutionTime = 0 ;
// We use the `findNextMavlinkMessage()` function to scan ahead for MAVLink messages. This
// is necessary because we don't know how big each MAVLink message is until we finish parsing
// one, and since we only output arrays of bytes, we need to know the size of that array.
mavlink_message_t msg ;
findNextMavlinkMessage ( & msg ) ;
while ( nextExecutionTime < 3 ) {
// Now we're sitting at the start of a MAVLink message, so read it all into a byte array for feeding to our parser.
QByteArray message = logFile . read ( msg . len + MAVLINK_NUM_NON_PAYLOAD_BYTES ) ;
// Emit this message to our MAVLink parser.
emit bytesReady ( logLink , message ) ;
// If we've reached the end of the of the file, make sure we handle that well
if ( logFile . atEnd ( ) ) {
_finishPlayback ( ) ;
return ;
}
// Run our parser to find the next timestamp and leave us at the start of the next MAVLink message.
logCurrentTime = findNextMavlinkMessage ( & msg ) ;
// 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())
qint64 timediff = ( logCurrentTime - logStartTime ) / accelerationFactor ;
quint64 desiredPacedTime = playbackStartTime + ( ( quint64 ) timediff ) / 1000 ;
quint64 currentTime = ( quint64 ) QDateTime : : currentMSecsSinceEpoch ( ) ;
nextExecutionTime = desiredPacedTime - currentTime ;
}
// And schedule the next execution of this function.
loopTimer . start ( nextExecutionTime ) ;
}
else
{
// Binary format - read at fixed rate
const int len = 100 ;
QByteArray chunk = logFile . read ( len ) ;
// Emit this packet
emit bytesReady ( logLink , chunk ) ;
// Check if reached end of file before reading next timestamp
if ( chunk . length ( ) < len | | logFile . atEnd ( ) )
{
_finishPlayback ( ) ;
return ;
if ( _replayLink ) {
_replayLink - > movePlayhead ( value ) ;
}
}
// Update the UI every 2^5=32 times, or when there isn't much data to be played back.
// Reduces flickering and minimizes CPU load.
if ( ( loopCounter & 0x1F ) = = 0 | | currPacketCount < 2000 )
void QGCMAVLinkLogPlayer : : _enablePlaybackControls ( bool enabled )
{
QFileInfo logFileInfo ( logFile ) ;
updatePositionSliderUi ( logFile . pos ( ) / static_cast < float > ( logFileInfo . size ( ) ) ) ;
}
loopCounter + + ;
_ui - > playButton - > setEnabled ( enabled ) ;
_ui - > speedSlider - > setEnabled ( enabled ) ;
_ui - > positionSlider - > setEnabled ( enabled ) ;
}
/**
* 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 QGCMAVLinkLogPlayer : : findNextMavlinkMessage ( mavlink_message_t * msg )
void QGCMAVLinkLogPlayer : : _setAccelerationFromSlider ( int value )
{
char nextByte ;
mavlink_status_t comm ;
while ( logFile . getChar ( & nextByte ) ) { // Loop over every byte
bool messageFound = mavlink_parse_char ( logLink - > getMavlinkChannel ( ) , nextByte , msg , & comm ) ;
// 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 ) {
logFile . seek ( logFile . pos ( ) - ( msg - > len + MAVLINK_NUM_NON_PAYLOAD_BYTES + timeLen ) ) ;
QByteArray rawTime = logFile . read ( timeLen ) ;
return parseTimestamp ( rawTime ) ;
}
qDebug ( ) < < value ;
if ( _replayLink ) {
_replayLink - > setAccelerationFactor ( value ) ;
}
// Otherwise, if we never find a message, return a failure code of 0.
return 0 ;
}
// Factor: -100: 0.01x, 0: 1.0x, 100: 100x
void QGCMAVLinkLogPlayer : : changeEvent ( QEvent * e )
{
QWidget : : changeEvent ( e ) ;
switch ( e - > type ( ) )
{
case QEvent : : LanguageChange :
ui - > retranslateUi ( this ) ;
break ;
default :
break ;
float accelerationFactor ;
if ( value < 0 ) {
accelerationFactor = 0.01f ;
value - = - 100 ;
if ( value > 0 ) {
accelerationFactor * = ( float ) value ;
}
} else if ( value > 0 ) {
accelerationFactor = 1.0f * ( float ) value ;
} else {
accelerationFactor = 1.0f ;
}
/**
* Implement paintEvent ( ) so that stylesheets work for our custom widget .
*/
void QGCMAVLinkLogPlayer : : paintEvent ( QPaintEvent * )
{
QStyleOption opt ;
opt . init ( this ) ;
QPainter p ( this ) ;
style ( ) - > drawPrimitive ( QStyle : : PE_Widget , & opt , & p , this ) ;
}
/// @brief Called when playback is complete
void QGCMAVLinkLogPlayer : : _finishPlayback ( void )
{
pause ( ) ;
QString status = tr ( " Flight Data replay complete " ) ;
ui - > logStatsLabel - > setText ( status ) ;
// Note that we explicitly set the slider to 100%, as it may not hit that by itself depending on log file size.
updatePositionSliderUi ( 100.0f ) ;
emit logFileEndReached ( ) ;
mavlink - > suspendLogForReplay ( false ) ;
LinkManager : : instance ( ) - > setConnectionsAllowed ( ) ;
_ui - > speedLabel - > setText ( QString ( " Speed: %1X " ) . arg ( accelerationFactor , 5 , ' f ' , 2 , ' 0 ' ) ) ;
}
/// @brief Called when an error occurs during playback to reset playback system state.
void QGCMAVLinkLogPlayer : : _playbackError ( void )
void QGCMAVLinkLogPlayer : : _replayLinkDisconnected ( void )
{
pause ( ) ;
logFile . close ( ) ;
logFile . setFileName ( " " ) ;
ui - > logFileNameLabel - > setText ( tr ( " No Flight Data selected " ) ) ;
// Disable playback controls
ui - > playButton - > setEnabled ( false ) ;
ui - > speedSlider - > setEnabled ( false ) ;
ui - > positionSlider - > setEnabled ( false ) ;
ui - > speedLabel - > setEnabled ( false ) ;
ui - > logFileNameLabel - > setEnabled ( false ) ;
ui - > logStatsLabel - > setEnabled ( false ) ;
_enablePlaybackControls ( false ) ;
_replayLink = NULL ;
}
Don Gagne
10 years ago