diff --git a/src/Vehicle/MavlinkLogManager.cc b/src/Vehicle/MavlinkLogManager.cc index 5d6a95863c4cfed5067b9cc13e07313f9d1e31db..ccb067f69bed08cdeef5425487595793573f6f08 100644 --- a/src/Vehicle/MavlinkLogManager.cc +++ b/src/Vehicle/MavlinkLogManager.cc @@ -106,14 +106,189 @@ MavlinkLogFiles::setUploaded(bool uploaded) //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -void CurrentRunningLog::close() +MavlinkLogProcessor::MavlinkLogProcessor() + : _fd(NULL) + , _written(0) + , _sequence(-1) + , _numDrops(0) + , _gotHeader(false) + , _error(false) + , _record(NULL) { - if(fd) { - fclose(fd); - fd = NULL; +} + +//----------------------------------------------------------------------------- +MavlinkLogProcessor::~MavlinkLogProcessor() +{ + close(); +} + +//----------------------------------------------------------------------------- +void +MavlinkLogProcessor::close() +{ + if(_fd) { + fclose(_fd); + _fd = NULL; } } +//----------------------------------------------------------------------------- +bool +MavlinkLogProcessor::valid() +{ + return (_fd != NULL) && (_record != NULL); +} + +//----------------------------------------------------------------------------- +bool +MavlinkLogProcessor::create(MavlinkLogManager* manager, const QString path, uint8_t id) +{ + _fileName.sprintf("%s/%03d-%s%s", + path.toLatin1().data(), + id, + QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz").toLatin1().data(), + kUlogExtension); + _fd = fopen(_fileName.toLatin1().data(), "wb"); + if(_fd) { + _record = new MavlinkLogFiles(manager, _fileName, true); + _record->setWriting(true); + _sequence = -1; + return true; + } + return false; +} + +//----------------------------------------------------------------------------- +bool +MavlinkLogProcessor::_checkSequence(uint16_t seq, int& num_drops) +{ + num_drops = 0; + //-- Check if a sequence is newer than the one previously received and if + // there were dropped messages between the last one and this. + if(_sequence == -1) { + _sequence = seq; + return true; + } + if((uint16_t)_sequence == seq) { + return false; + } + if(seq > (uint16_t)_sequence) { + // Account for wrap-arounds, sequence is 2 bytes + if((seq - _sequence) > (1 << 15)) { // Assume reordered + return false; + } + num_drops = seq - _sequence - 1; + _numDrops += num_drops; + _sequence = seq; + return true; + } else { + if((_sequence - seq) > (1 << 15)) { + num_drops = (1 << 16) - _sequence - 1 + seq; + _numDrops += num_drops; + _sequence = seq; + return true; + } + return false; + } +} + +//----------------------------------------------------------------------------- +void +MavlinkLogProcessor::_writeData(void* data, int len) +{ + if(!_error) { + _error = fwrite(data, 1, len, _fd) != (size_t)len; + if(!_error) { + _written += len; + if(_record) { + _record->setSize(_written); + } + } else { + qCDebug(MavlinkLogManagerLog) << "File IO error:" << len << "bytes into" << _fileName; + } + } +} + +//----------------------------------------------------------------------------- +QByteArray +MavlinkLogProcessor::_writeUlogMessage(QByteArray& data) +{ + //-- Write ulog data w/o integrity checking, assuming data starts with a + // valid ulog message. returns the remaining data at the end. + while(data.length() > 2) { + uint8_t* ptr = (uint8_t*)data.data(); + int message_length = ptr[0] + (ptr[1] * 256) + 3; // 3 = ULog msg header + if(message_length > data.length()) + break; + _writeData(data.data(), message_length); + data.remove(0, message_length); + return data; + } + return data; +} + +//----------------------------------------------------------------------------- +bool +MavlinkLogProcessor::processStreamData(uint16_t sequence, uint8_t first_message, QByteArray data) +{ + int num_drops = 0; + _error = false; + while(_checkSequence(sequence, num_drops)) { + //-- The first 16 bytes need special treatment (this sounds awfully brittle) + if(!_gotHeader) { + if(data.size() < 16) { + //-- Shouldn't happen but if it does, we might as well close shop. + qCCritical(MavlinkLogManagerLog) << "Corrupt log header. Canceling log download."; + return false; + } + //-- Write header + _writeData(data.data(), 16); + data.remove(0, 16); + _gotHeader = true; + // What about data start offset now that we removed 16 bytes off the start? + } + if(_gotHeader && num_drops > 0) { + if(num_drops > 25) num_drops = 25; + //-- Hocus Pocus + // Write a dropout message. We don't really know the actual duration, + // so just use the number of drops * 10 ms + uint8_t bogus[] = {2, 0, 79, 0, 0}; + bogus[3] = num_drops * 10; + _writeData(bogus, sizeof(bogus)); + } + if(num_drops > 0) { + _writeUlogMessage(_ulogMessage); + _ulogMessage.clear(); + //-- If no usefull information in this message. Drop it. + if(first_message == 255) { + break; + } + if(first_message > 0) { + data.remove(0, first_message); + first_message = 0; + } + } + if(first_message == 255 && _ulogMessage.length() > 0) { + _ulogMessage.append(data); + break; + } + if(_ulogMessage.length()) { + _writeData(_ulogMessage.data(), _ulogMessage.length()); + if(first_message) { + _writeData(data.left(first_message).data(), first_message); + } + _ulogMessage.clear(); + } + if(first_message) { + data.remove(0, first_message); + } + _ulogMessage = _writeUlogMessage(data); + break; + } + return !_error; +} + //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- MavlinkLogManager::MavlinkLogManager(QGCApplication* app) @@ -125,8 +300,7 @@ MavlinkLogManager::MavlinkLogManager(QGCApplication* app) , _vehicle(NULL) , _logRunning(false) , _loggingDisabled(false) - , _currentSavingFile(NULL) - , _sequence(0) + , _logProcessor(NULL) , _deleteAfterUpload(false) , _loggingCmdTryCount(0) { @@ -382,20 +556,20 @@ MavlinkLogManager::stopLogging() //-- Tell vehicle to stop sending logs _vehicle->stopMavlinkLog(); } - if(_currentSavingFile) { - _currentSavingFile->close(); - if(_currentSavingFile->record) { - _currentSavingFile->record->setWriting(false); + if(_logProcessor) { + _logProcessor->close(); + if(_logProcessor->record()) { + _logProcessor->record()->setWriting(false); if(_enableAutoUpload) { //-- Queue log for auto upload (set selected flag) - _currentSavingFile->record->setSelected(true); + _logProcessor->record()->setSelected(true); if(!uploading()) { uploadLog(); } } } - delete _currentSavingFile; - _currentSavingFile = NULL; + delete _logProcessor; + _logProcessor = NULL; _logRunning = false; if(_vehicle) { //-- Setup a timer to make sure vehicle received the command @@ -619,42 +793,24 @@ MavlinkLogManager::_processCmdAck() //----------------------------------------------------------------------------- void -MavlinkLogManager::_mavlinkLogData(Vehicle* /*vehicle*/, uint8_t /*target_system*/, uint8_t /*target_component*/, uint16_t sequence, uint8_t length, uint8_t first_message, const uint8_t* data, bool /*acked*/) +MavlinkLogManager::_mavlinkLogData(Vehicle* /*vehicle*/, uint8_t /*target_system*/, uint8_t /*target_component*/, uint16_t sequence, uint8_t first_message, QByteArray data, bool /*acked*/) { //-- Disable timer if we got a message before an ACK for the start command if(_logRunning) { _ackTimer.stop(); } - if(_currentSavingFile && _currentSavingFile->fd) { - if(sequence != _sequence) { - qCWarning(MavlinkLogManagerLog) << "Dropped Mavlink log data"; - if(first_message < 255) { - data += first_message; - length -= first_message; - } else { - return; - } - } - if(fwrite(data, 1, length, _currentSavingFile->fd) != (size_t)length) { - qCCritical(MavlinkLogManagerLog) << "Error writing Mavlink log file:" << _currentSavingFile->fileName; - delete _currentSavingFile; - _currentSavingFile = NULL; + if(_logProcessor && _logProcessor->valid()) { + if(!_logProcessor->processStreamData(sequence, first_message, data)) { + qCCritical(MavlinkLogManagerLog) << "Error writing Mavlink log file:" << _logProcessor->fileName(); + delete _logProcessor; + _logProcessor = NULL; _logRunning = false; _vehicle->stopMavlinkLog(); emit logRunningChanged(); } } else { - length = 0; qCWarning(MavlinkLogManagerLog) << "Mavlink log data received when not expected."; } - //-- Update file size - if(_currentSavingFile) { - if(_currentSavingFile->record) { - quint32 size = _currentSavingFile->record->size() + length; - _currentSavingFile->record->setSize(size); - } - } - _sequence = sequence + 1; } //----------------------------------------------------------------------------- @@ -682,13 +838,13 @@ void MavlinkLogManager::_discardLog() { //-- Delete (empty) log file (and record) - if(_currentSavingFile) { - _currentSavingFile->close(); - if(_currentSavingFile->record) { - _deleteLog(_currentSavingFile->record); + if(_logProcessor) { + _logProcessor->close(); + if(_logProcessor->record()) { + _deleteLog(_logProcessor->record()); } - delete _currentSavingFile; - _currentSavingFile = NULL; + delete _logProcessor; + _logProcessor = NULL; } _logRunning = false; emit logRunningChanged(); @@ -698,30 +854,20 @@ MavlinkLogManager::_discardLog() bool MavlinkLogManager::_createNewLog() { - if(_currentSavingFile) { - delete _currentSavingFile; - _currentSavingFile = NULL; + if(_logProcessor) { + delete _logProcessor; + _logProcessor = NULL; } - _currentSavingFile = new CurrentRunningLog; - _currentSavingFile->fileName.sprintf("%s/%03d-%s%s", - _logPath.toLatin1().data(), - _vehicle->id(), - QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz").toLatin1().data(), - kUlogExtension); - _currentSavingFile->fd = fopen(_currentSavingFile->fileName.toLatin1().data(), "wb"); - if(_currentSavingFile->fd) { - MavlinkLogFiles* newLog = new MavlinkLogFiles(this, _currentSavingFile->fileName, true); - newLog->setWriting(true); - _insertNewLog(newLog); - _currentSavingFile->record = newLog; + _logProcessor = new MavlinkLogProcessor; + if(_logProcessor->create(this, _logPath, _vehicle->id())) { + _insertNewLog(_logProcessor->record()); emit logFilesChanged(); } else { - qCCritical(MavlinkLogManagerLog) << "Could not create Mavlink log file:" << _currentSavingFile->fileName; - delete _currentSavingFile; - _currentSavingFile = NULL; + qCCritical(MavlinkLogManagerLog) << "Could not create Mavlink log file:" << _logProcessor->fileName(); + delete _logProcessor; + _logProcessor = NULL; } - _sequence = 0; - return _currentSavingFile != NULL; + return _logProcessor != NULL; } //----------------------------------------------------------------------------- diff --git a/src/Vehicle/MavlinkLogManager.h b/src/Vehicle/MavlinkLogManager.h index b0c69fbb5dcd3dc835cbe6b58b28bbcf3f8c4482..b39d2528c89245715c7f141393d27552ea70425a 100644 --- a/src/Vehicle/MavlinkLogManager.h +++ b/src/Vehicle/MavlinkLogManager.h @@ -73,24 +73,31 @@ private: }; //----------------------------------------------------------------------------- -class CurrentRunningLog +class MavlinkLogProcessor { public: - CurrentRunningLog() - : fd(NULL) - , record(NULL) - , written(0) - { - } - ~CurrentRunningLog() - { - close(); - } - void close(); - FILE* fd; - QString fileName; - MavlinkLogFiles* record; - quint32 written; + MavlinkLogProcessor(); + ~MavlinkLogProcessor(); + void close (); + bool valid (); + bool create (MavlinkLogManager *manager, const QString path, uint8_t id); + MavlinkLogFiles* record () { return _record; } + QString fileName () { return _fileName; } + bool processStreamData(uint16_t _sequence, uint8_t first_message, QByteArray data); +private: + bool _checkSequence(uint16_t seq, int &num_drops); + QByteArray _writeUlogMessage(QByteArray &data); + void _writeData(void* data, int len); +private: + FILE* _fd; + quint32 _written; + int _sequence; + int _numDrops; + bool _gotHeader; + bool _error; + QByteArray _ulogMessage; + QString _fileName; + MavlinkLogFiles* _record; }; //----------------------------------------------------------------------------- @@ -163,7 +170,7 @@ private slots: void _dataAvailable (); void _uploadProgress (qint64 bytesSent, qint64 bytesTotal); void _activeVehicleChanged (Vehicle* vehicle); - void _mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message, const uint8_t* data, bool acked); + void _mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked); void _armedChanged (bool armed); void _commandLongAck (uint8_t compID, uint16_t command, uint8_t result); void _processCmdAck (); @@ -191,8 +198,7 @@ private: Vehicle* _vehicle; bool _logRunning; bool _loggingDisabled; - CurrentRunningLog* _currentSavingFile; - uint16_t _sequence; + MavlinkLogProcessor* _logProcessor; bool _deleteAfterUpload; int _loggingCmdTryCount; QTimer _ackTimer; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f4ac52a02d4e8bbaf577377926050fbacbe4c6f4..c9ed3a839b49e0379792d9b3f3328b8f4d3e8ec1 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2015,7 +2015,8 @@ Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message) { mavlink_logging_data_t log; mavlink_msg_logging_data_decode(&message, &log); - emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, log.length, log.first_message_offset, log.data, false); + emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, + log.first_message_offset, QByteArray((const char*)log.data, log.length), false); } //----------------------------------------------------------------------------- @@ -2025,7 +2026,8 @@ Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message) mavlink_logging_data_t log; mavlink_msg_logging_data_decode(&message, &log); _ackMavlinkLogData(log.sequence); - emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, log.length, log.first_message_offset, log.data, true); + emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, + log.first_message_offset, QByteArray((const char*)log.data, log.length), true); } //----------------------------------------------------------------------------- diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 372a848e53b3699c53c41dfd94a405fa7f68b05e..0f6243519922c1ddfb48bea6c2eae67d8a04e99c 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -643,7 +643,7 @@ signals: void mavlinkScaledImu3(mavlink_message_t message); // Mavlink Log Download - void mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message, const uint8_t* data, bool acked); + void mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked); private slots: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);