|
|
|
@ -26,12 +26,15 @@ FileManager::FileManager(QObject* parent, Vehicle* vehicle)
@@ -26,12 +26,15 @@ FileManager::FileManager(QObject* parent, Vehicle* vehicle)
|
|
|
|
|
, _currentOperation(kCOIdle) |
|
|
|
|
, _vehicle(vehicle) |
|
|
|
|
, _dedicatedLink(NULL) |
|
|
|
|
, _lastOutgoingSeqNumber(0) |
|
|
|
|
, _activeSession(0) |
|
|
|
|
, _missingDownloadedBytes(0) |
|
|
|
|
, _downloadingMissingParts(false) |
|
|
|
|
, _systemIdQGC(0) |
|
|
|
|
{ |
|
|
|
|
connect(&_ackTimer, &QTimer::timeout, this, &FileManager::_ackTimeout); |
|
|
|
|
|
|
|
|
|
_lastOutgoingRequest.hdr.seqNumber = 0; |
|
|
|
|
|
|
|
|
|
_systemIdServer = _vehicle->id(); |
|
|
|
|
|
|
|
|
|
// Make sure we don't have bad structure packing
|
|
|
|
@ -55,6 +58,9 @@ void FileManager::_openAckResponse(Request* openAck)
@@ -55,6 +58,9 @@ void FileManager::_openAckResponse(Request* openAck)
|
|
|
|
|
|
|
|
|
|
_downloadOffset = 0; // Start reading at beginning of file
|
|
|
|
|
_readFileAccumulator.clear(); // Start with an empty file
|
|
|
|
|
_missingDownloadedBytes = 0; |
|
|
|
|
_downloadingMissingParts = false; |
|
|
|
|
_missingData.clear(); |
|
|
|
|
|
|
|
|
|
Request request; |
|
|
|
|
request.hdr.session = _activeSession; |
|
|
|
@ -66,15 +72,54 @@ void FileManager::_openAckResponse(Request* openAck)
@@ -66,15 +72,54 @@ void FileManager::_openAckResponse(Request* openAck)
|
|
|
|
|
_sendRequest(&request); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/// request the next missing part of a (partially) downloaded file
|
|
|
|
|
void FileManager::_requestMissingData() |
|
|
|
|
{ |
|
|
|
|
if (_missingData.empty()) { |
|
|
|
|
_downloadingMissingParts = false; |
|
|
|
|
_missingDownloadedBytes = 0; |
|
|
|
|
// there might be more data missing at the end
|
|
|
|
|
if ((uint32_t)_readFileAccumulator.length() != _downloadFileSize) { |
|
|
|
|
_downloadOffset = _readFileAccumulator.length(); |
|
|
|
|
qCDebug(FileManagerLog) << QString("_requestMissingData: missing parts done, downloadOffset(%1) downloadFileSize(%2)") |
|
|
|
|
.arg(_downloadOffset).arg(_downloadFileSize); |
|
|
|
|
} else { |
|
|
|
|
_closeDownloadSession(true); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
qCDebug(FileManagerLog) << QString("_requestMissingData: offset(%1) size(%2)").arg(_missingData.head().offset).arg(_missingData.head().size); |
|
|
|
|
|
|
|
|
|
_downloadOffset = _missingData.head().offset; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
Request request; |
|
|
|
|
_currentOperation = kCORead; |
|
|
|
|
request.hdr.session = _activeSession; |
|
|
|
|
request.hdr.opcode = kCmdReadFile; |
|
|
|
|
request.hdr.offset = _downloadOffset; |
|
|
|
|
request.hdr.size = sizeof(request.data); |
|
|
|
|
|
|
|
|
|
_sendRequest(&request); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/// Closes out a download session by writing the file and doing cleanup.
|
|
|
|
|
/// @param success true: successful download completion, false: error during download
|
|
|
|
|
void FileManager::_closeDownloadSession(bool success) |
|
|
|
|
{ |
|
|
|
|
qCDebug(FileManagerLog) << QString("_closeDownloadSession: success(%1)").arg(success); |
|
|
|
|
qCDebug(FileManagerLog) << QString("_closeDownloadSession: success(%1) missingBytes(%2)").arg(success).arg(_missingDownloadedBytes); |
|
|
|
|
|
|
|
|
|
_currentOperation = kCOIdle; |
|
|
|
|
|
|
|
|
|
if (success) { |
|
|
|
|
if (_missingDownloadedBytes > 0 || (uint32_t)_readFileAccumulator.length() < _downloadFileSize) { |
|
|
|
|
// we're not done yet: request the missing parts individually (either we had missing parts or
|
|
|
|
|
// the last (few) packets right before the EOF got dropped)
|
|
|
|
|
_downloadingMissingParts = true; |
|
|
|
|
_requestMissingData(); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename); |
|
|
|
|
|
|
|
|
|
QFile file(downloadFilePath); |
|
|
|
@ -129,21 +174,51 @@ void FileManager::_downloadAckResponse(Request* readAck, bool readFile)
@@ -129,21 +174,51 @@ void FileManager::_downloadAckResponse(Request* readAck, bool readFile)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (readAck->hdr.offset != _downloadOffset) { |
|
|
|
|
_closeDownloadSession(false /* failure */); |
|
|
|
|
_emitErrorMessage(tr("Download: Offset returned (%1) differs from offset requested/expected (%2)").arg(readAck->hdr.offset).arg(_downloadOffset)); |
|
|
|
|
return; |
|
|
|
|
if (readFile) { |
|
|
|
|
_closeDownloadSession(false /* failure */); |
|
|
|
|
_emitErrorMessage(tr("Download: Offset returned (%1) differs from offset requested/expected (%2)").arg(readAck->hdr.offset).arg(_downloadOffset)); |
|
|
|
|
return; |
|
|
|
|
} else { // burst
|
|
|
|
|
if (readAck->hdr.offset < _downloadOffset) { // old data: ignore it
|
|
|
|
|
_setupAckTimeout(); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
// keep track of missing data chunks
|
|
|
|
|
MissingData missingData; |
|
|
|
|
missingData.offset = _downloadOffset; |
|
|
|
|
missingData.size = readAck->hdr.offset - _downloadOffset; |
|
|
|
|
_missingData.push_back(missingData); |
|
|
|
|
_missingDownloadedBytes += readAck->hdr.offset - _downloadOffset; |
|
|
|
|
qCDebug(FileManagerLog) << QString("_downloadAckResponse: missing data: offset(%1) size(%2)").arg(missingData.offset).arg(missingData.size); |
|
|
|
|
_downloadOffset = readAck->hdr.offset; |
|
|
|
|
_readFileAccumulator.resize(_downloadOffset); // placeholder for the missing data
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
qCDebug(FileManagerLog) << QString("_downloadAckResponse: offset(%1) size(%2) burstComplete(%3)").arg(readAck->hdr.offset).arg(readAck->hdr.size).arg(readAck->hdr.burstComplete); |
|
|
|
|
|
|
|
|
|
_downloadOffset += readAck->hdr.size; |
|
|
|
|
_readFileAccumulator.append((const char*)readAck->data, readAck->hdr.size); |
|
|
|
|
if (_downloadingMissingParts) { |
|
|
|
|
Q_ASSERT(_missingData.head().offset == _downloadOffset); |
|
|
|
|
_missingDownloadedBytes -= readAck->hdr.size; |
|
|
|
|
_readFileAccumulator.replace(_downloadOffset, readAck->hdr.size, (const char*)readAck->data, readAck->hdr.size); |
|
|
|
|
if (_missingData.head().size <= readAck->hdr.size) { |
|
|
|
|
_missingData.pop_front(); |
|
|
|
|
} else { |
|
|
|
|
_missingData.head().size -= readAck->hdr.size; |
|
|
|
|
_missingData.head().offset += readAck->hdr.size; |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
_downloadOffset += readAck->hdr.size; |
|
|
|
|
_readFileAccumulator.append((const char*)readAck->data, readAck->hdr.size); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (_downloadFileSize != 0) { |
|
|
|
|
emit commandProgress(100 * ((float)_readFileAccumulator.length() / (float)_downloadFileSize)); |
|
|
|
|
emit commandProgress(100 * ((float)(_readFileAccumulator.length() - _missingDownloadedBytes) / (float)_downloadFileSize)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (readFile || readAck->hdr.burstComplete) { |
|
|
|
|
if (_downloadingMissingParts) { |
|
|
|
|
_requestMissingData(); |
|
|
|
|
} else if (readFile || readAck->hdr.burstComplete) { |
|
|
|
|
// Possibly still more data to read, send next read request
|
|
|
|
|
|
|
|
|
|
Request request; |
|
|
|
@ -163,6 +238,7 @@ void FileManager::_downloadAckResponse(Request* readAck, bool readFile)
@@ -163,6 +238,7 @@ void FileManager::_downloadAckResponse(Request* readAck, bool readFile)
|
|
|
|
|
void FileManager::_listAckResponse(Request* listAck) |
|
|
|
|
{ |
|
|
|
|
if (listAck->hdr.offset != _listOffset) { |
|
|
|
|
// this is a real error (directory listing is synchronous), no need to retransmit
|
|
|
|
|
_currentOperation = kCOIdle; |
|
|
|
|
_emitErrorMessage(tr("List: Offset returned (%1) differs from offset requested (%2)").arg(listAck->hdr.offset).arg(_listOffset)); |
|
|
|
|
return; |
|
|
|
@ -303,7 +379,7 @@ void FileManager::receiveMessage(mavlink_message_t message)
@@ -303,7 +379,7 @@ void FileManager::receiveMessage(mavlink_message_t message)
|
|
|
|
|
if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
mavlink_file_transfer_protocol_t data; |
|
|
|
|
mavlink_msg_file_transfer_protocol_decode(&message, &data); |
|
|
|
|
|
|
|
|
@ -314,18 +390,28 @@ void FileManager::receiveMessage(mavlink_message_t message)
@@ -314,18 +390,28 @@ void FileManager::receiveMessage(mavlink_message_t message)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
Request* request = (Request*)&data.payload[0]; |
|
|
|
|
|
|
|
|
|
uint16_t incomingSeqNumber = request->hdr.seqNumber; |
|
|
|
|
|
|
|
|
|
// Make sure we have a good sequence number
|
|
|
|
|
uint16_t expectedSeqNumber = _lastOutgoingRequest.hdr.seqNumber + 1; |
|
|
|
|
|
|
|
|
|
// ignore old/reordered packets (handle wrap-around properly)
|
|
|
|
|
if ((uint16_t)((expectedSeqNumber - 1) - incomingSeqNumber) < (std::numeric_limits<uint16_t>::max()/2)) { |
|
|
|
|
qDebug() << "Received old packet: expected seq:" << expectedSeqNumber << "got:" << incomingSeqNumber; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
_clearAckTimeout(); |
|
|
|
|
|
|
|
|
|
qCDebug(FileManagerLog) << "receiveMessage" << request->hdr.opcode; |
|
|
|
|
|
|
|
|
|
uint16_t incomingSeqNumber = request->hdr.seqNumber; |
|
|
|
|
|
|
|
|
|
// Make sure we have a good sequence number
|
|
|
|
|
uint16_t expectedSeqNumber = _lastOutgoingSeqNumber + 1; |
|
|
|
|
if (incomingSeqNumber != expectedSeqNumber) { |
|
|
|
|
bool doAbort = true; |
|
|
|
|
switch (_currentOperation) { |
|
|
|
|
case kCOBurst: |
|
|
|
|
case kCOBurst: // burst download drops are handled in _downloadAckResponse()
|
|
|
|
|
doAbort = false; |
|
|
|
|
break; |
|
|
|
|
case kCORead: |
|
|
|
|
_closeDownloadSession(false /* failure */); |
|
|
|
|
break; |
|
|
|
@ -348,12 +434,14 @@ void FileManager::receiveMessage(mavlink_message_t message)
@@ -348,12 +434,14 @@ void FileManager::receiveMessage(mavlink_message_t message)
|
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
_emitErrorMessage(tr("Bad sequence number on received message: expected(%1) received(%2)").arg(expectedSeqNumber).arg(incomingSeqNumber)); |
|
|
|
|
return; |
|
|
|
|
if (doAbort) { |
|
|
|
|
_emitErrorMessage(tr("Bad sequence number on received message: expected(%1) received(%2)").arg(expectedSeqNumber).arg(incomingSeqNumber)); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Move past the incoming sequence number for next request
|
|
|
|
|
_lastOutgoingSeqNumber = incomingSeqNumber; |
|
|
|
|
_lastOutgoingRequest.hdr.seqNumber = incomingSeqNumber; |
|
|
|
|
|
|
|
|
|
if (request->hdr.opcode == kRspAck) { |
|
|
|
|
switch (request->hdr.req_opcode) { |
|
|
|
@ -406,6 +494,9 @@ void FileManager::receiveMessage(mavlink_message_t message)
@@ -406,6 +494,9 @@ void FileManager::receiveMessage(mavlink_message_t message)
|
|
|
|
|
} else if (request->hdr.req_opcode == kCmdCreateFile) { |
|
|
|
|
_emitErrorMessage(tr("Nak received creating file, error: %1").arg(errorString(request->data[0]))); |
|
|
|
|
return; |
|
|
|
|
} else if (request->hdr.req_opcode == kCmdCreateDirectory) { |
|
|
|
|
_emitErrorMessage(tr("Nak received creating directory, error: %1").arg(errorString(request->data[0]))); |
|
|
|
|
return; |
|
|
|
|
} else { |
|
|
|
|
// Generic Nak handling
|
|
|
|
|
if (request->hdr.req_opcode == kCmdReadFile || request->hdr.req_opcode == kCmdBurstReadFile) { |
|
|
|
@ -538,7 +629,7 @@ void FileManager::_downloadWorker(const QString& from, const QDir& downloadDir,
@@ -538,7 +629,7 @@ void FileManager::_downloadWorker(const QString& from, const QDir& downloadDir,
|
|
|
|
|
void FileManager::uploadPath(const QString& toPath, const QFileInfo& uploadFile) |
|
|
|
|
{ |
|
|
|
|
if(_currentOperation != kCOIdle){ |
|
|
|
|
_emitErrorMessage(tr("UAS File manager busy. Try again later")); |
|
|
|
|
_emitErrorMessage(tr("UAS File manager busy. Try again later")); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -584,6 +675,24 @@ void FileManager::uploadPath(const QString& toPath, const QFileInfo& uploadFile)
@@ -584,6 +675,24 @@ void FileManager::uploadPath(const QString& toPath, const QFileInfo& uploadFile)
|
|
|
|
|
_sendRequest(&request); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void FileManager::createDirectory(const QString& directory) |
|
|
|
|
{ |
|
|
|
|
if(_currentOperation != kCOIdle){ |
|
|
|
|
_emitErrorMessage(tr("UAS File manager busy. Try again later")); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
_currentOperation = kCOCreateDir; |
|
|
|
|
|
|
|
|
|
Request request; |
|
|
|
|
request.hdr.session = 0; |
|
|
|
|
request.hdr.opcode = kCmdCreateDirectory; |
|
|
|
|
request.hdr.offset = 0; |
|
|
|
|
request.hdr.size = 0; |
|
|
|
|
_fillRequestWithString(&request, directory); |
|
|
|
|
_sendRequest(&request); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
QString FileManager::errorString(uint8_t errorCode) |
|
|
|
|
{ |
|
|
|
|
switch(errorCode) { |
|
|
|
@ -643,7 +752,8 @@ void FileManager::_setupAckTimeout(void)
@@ -643,7 +752,8 @@ void FileManager::_setupAckTimeout(void)
|
|
|
|
|
|
|
|
|
|
Q_ASSERT(!_ackTimer.isActive()); |
|
|
|
|
|
|
|
|
|
_ackTimer.setSingleShot(true); |
|
|
|
|
_ackNumTries = 0; |
|
|
|
|
_ackTimer.setSingleShot(false); |
|
|
|
|
_ackTimer.start(ackTimerTimeoutMsecs); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -659,6 +769,26 @@ void FileManager::_ackTimeout(void)
@@ -659,6 +769,26 @@ void FileManager::_ackTimeout(void)
|
|
|
|
|
{ |
|
|
|
|
qCDebug(FileManagerLog) << "_ackTimeout"; |
|
|
|
|
|
|
|
|
|
if (++_ackNumTries <= ackTimerMaxRetries) { |
|
|
|
|
qCDebug(FileManagerLog) << "ack timeout - retrying"; |
|
|
|
|
if (_currentOperation == kCOBurst) { |
|
|
|
|
// for burst downloads try to initiate a new burst
|
|
|
|
|
Request request; |
|
|
|
|
request.hdr.session = _activeSession; |
|
|
|
|
request.hdr.opcode = kCmdBurstReadFile; |
|
|
|
|
request.hdr.offset = _downloadOffset; |
|
|
|
|
request.hdr.size = 0; |
|
|
|
|
request.hdr.seqNumber = ++_lastOutgoingRequest.hdr.seqNumber; |
|
|
|
|
|
|
|
|
|
_sendRequestNoAck(&request); |
|
|
|
|
} else { |
|
|
|
|
_sendRequestNoAck(&_lastOutgoingRequest); |
|
|
|
|
} |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
_clearAckTimeout(); |
|
|
|
|
|
|
|
|
|
// Make sure to set _currentOperation state before emitting error message. Code may respond
|
|
|
|
|
// to error message signal by sending another command, which will fail if state is not back
|
|
|
|
|
// to idle. FileView UI works this way with the List command.
|
|
|
|
@ -689,8 +819,11 @@ void FileManager::_ackTimeout(void)
@@ -689,8 +819,11 @@ void FileManager::_ackTimeout(void)
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
default: |
|
|
|
|
{ |
|
|
|
|
OperationState currentOperation = _currentOperation; |
|
|
|
|
_currentOperation = kCOIdle; |
|
|
|
|
_emitErrorMessage(QString("Timeout waiting for ack: Command failed (%1)").arg(_currentOperation)); |
|
|
|
|
_emitErrorMessage(QString("Timeout waiting for ack: Command failed (%1)").arg(currentOperation)); |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -719,19 +852,28 @@ void FileManager::_emitListEntry(const QString& entry)
@@ -719,19 +852,28 @@ void FileManager::_emitListEntry(const QString& entry)
|
|
|
|
|
void FileManager::_sendRequest(Request* request) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
mavlink_message_t message; |
|
|
|
|
|
|
|
|
|
_setupAckTimeout(); |
|
|
|
|
|
|
|
|
|
_lastOutgoingSeqNumber++; |
|
|
|
|
|
|
|
|
|
request->hdr.seqNumber = _lastOutgoingSeqNumber; |
|
|
|
|
request->hdr.seqNumber = ++_lastOutgoingRequest.hdr.seqNumber; |
|
|
|
|
// store the current request
|
|
|
|
|
if (request->hdr.size <= sizeof(request->data)) { |
|
|
|
|
memcpy(&_lastOutgoingRequest, request, sizeof(RequestHeader) + request->hdr.size); |
|
|
|
|
} else { |
|
|
|
|
qCCritical(FileManagerLog) << "request length too long:" << request->hdr.size; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
qCDebug(FileManagerLog) << "_sendRequest opcode:" << request->hdr.opcode << "seqNumber:" << request->hdr.seqNumber; |
|
|
|
|
|
|
|
|
|
if (_systemIdQGC == 0) { |
|
|
|
|
_systemIdQGC = qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(); |
|
|
|
|
} |
|
|
|
|
_sendRequestNoAck(request); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/// @brief Sends the specified Request out to the UAS, without ack timeout handling
|
|
|
|
|
void FileManager::_sendRequestNoAck(Request* request) |
|
|
|
|
{ |
|
|
|
|
mavlink_message_t message; |
|
|
|
|
|
|
|
|
|
// Unit testing code can end up here without _dedicateLink set since it tests inidividual commands.
|
|
|
|
|
LinkInterface* link; |
|
|
|
|