FileManager: add retry handling

This stores the last request and resends it after a timeout, with a max
of 6 retries.
It also checks for old incoming packets and drops them.

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
@@ -315,14 +316,21 @@ 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) {
         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) {
@@ -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;

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