diff --git a/src/uas/FileManager.cc b/src/uas/FileManager.cc index 4c345f5a723d6738e237c110be4fe5430c2aa44c..57c658894073c72332ccfdfc3f7683a027e30ef1 100644 --- a/src/uas/FileManager.cc +++ b/src/uas/FileManager.cc @@ -26,12 +26,13 @@ FileManager::FileManager(QObject* parent, Vehicle* vehicle) , _currentOperation(kCOIdle) , _vehicle(vehicle) , _dedicatedLink(NULL) - , _lastOutgoingSeqNumber(0) , _activeSession(0) , _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 @@ -303,7 +304,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,15 +315,22 @@ 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::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) { switch (_currentOperation) { case kCOBurst: @@ -353,7 +361,7 @@ void FileManager::receiveMessage(mavlink_message_t message) } // 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) { @@ -541,7 +549,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; } @@ -664,7 +672,8 @@ void FileManager::_setupAckTimeout(void) Q_ASSERT(!_ackTimer.isActive()); - _ackTimer.setSingleShot(true); + _ackNumTries = 0; + _ackTimer.setSingleShot(false); _ackTimer.start(ackTimerTimeoutMsecs); } @@ -680,6 +689,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. @@ -710,8 +739,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; } } @@ -740,19 +772,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; diff --git a/src/uas/FileManager.h b/src/uas/FileManager.h index 787ee25fb72f11fd582a298f3c7e26018dda01dc..41155fa78ba71a0b3bce457a8c79252ca78e3ecf 100644 --- a/src/uas/FileManager.h +++ b/src/uas/FileManager.h @@ -42,7 +42,9 @@ public: /// Timeout in msecs to wait for an Ack time come back. This is public so we can write unit tests which wait long enough /// for the FileManager to timeout. - static const int ackTimerTimeoutMsecs = 10000; + static const int ackTimerTimeoutMsecs = 50; + + static const int ackTimerMaxRetries = 6; /// Downloads the specified file. /// @param from File to download from UAS, fully qualified path @@ -186,6 +188,7 @@ private: void _emitErrorMessage(const QString& msg); void _emitListEntry(const QString& entry); void _sendRequest(Request* request); + void _sendRequestNoAck(Request* request); void _fillRequestWithString(Request* request, const QString& str); void _openAckResponse(Request* openAck); void _downloadAckResponse(Request* readAck, bool readFile); @@ -203,11 +206,12 @@ private: OperationState _currentOperation; ///< Current operation of state machine QTimer _ackTimer; ///< Used to signal a timeout waiting for an ack + int _ackNumTries; ///< current number of tries Vehicle* _vehicle; LinkInterface* _dedicatedLink; ///< Link to use for communication - uint16_t _lastOutgoingSeqNumber; ///< Sequence number sent in last outgoing packet + Request _lastOutgoingRequest; ///< contains the last outgoing packet unsigned _listOffset; ///< offset for the current List operation QString _listPath; ///< path for the current List operation