Merge pull request #5665 from mavlink/ftp_robustify

Make FTP more robust

src/comm/MockLinkFileServer.cc

@@ -36,9 +36,22 @@ MockLinkFileServer::MockLinkFileServer(uint8_t systemIdServer, uint8_t component
     _errMode(errModeNone),
     _systemIdServer(systemIdServer),
     _componentIdServer(componentIdServer),
-    _mockLink(mockLink)
+    _mockLink(mockLink),
+    _lastReplyValid(false),
+    _lastReplySequence(0),
+    _randomDropsEnabled(false)
 {
+    srand(0); // make sure unit tests are deterministic
+}
+
+void MockLinkFileServer::ensureNullTemination(FileManager::Request* request)
+{
+    if (request->hdr.size < sizeof(request->data)) {
+        request->data[request->hdr.size] = '\0';
 
+    } else {
+        request->data[sizeof(request->data)-1] = '\0';
+    }
 }
 
 /// @brief Handles List command requests. Only supports root folder paths.
@@ -51,6 +64,8 @@ void MockLinkFileServer::_listCommand(uint8_t senderSystemId, uint8_t senderComp
     QString                     path;
     uint16_t                    outgoingSeqNumber = _nextSeqNumber(seqNumber);
 
+    ensureNullTemination(request);
+
     // We only support root path
     path = (char *)&request->data[0];
     if (!path.isEmpty() && path != "/") {
@@ -103,6 +118,8 @@ void MockLinkFileServer::_openCommand(uint8_t senderSystemId, uint8_t senderComp
     QString                     path;
     uint16_t                    outgoingSeqNumber = _nextSeqNumber(seqNumber);
     
+    ensureNullTemination(request);
+
     size_t cchPath = strnlen((char *)request->data, sizeof(request->data));
     Q_ASSERT(cchPath != sizeof(request->data));
     Q_UNUSED(cchPath); // Fix initialized-but-not-referenced warning on release builds
@@ -273,6 +290,21 @@ void MockLinkFileServer::handleFTPMessage(const mavlink_message_t& message)
 
     FileManager::Request* request = (FileManager::Request*)&requestFTP.payload[0];
 
+	if (_randomDropsEnabled) {
+	    if (rand() % 3 == 0) {
+	        qDebug() << "FileServer: Random drop of incoming packet";
+	        return;
+	    }
+	}
+
+	if (_lastReplyValid && request->hdr.seqNumber + 1 == _lastReplySequence) {
+	    // this is the same request as the one we replied to last. It means the (n)ack got lost, and the GCS
+	    // resent the request
+	    qDebug() << "FileServer: resending response";
+	    _mockLink->respondWithMavlinkMessage(_lastReply);
+	    return;
+	}
+
     uint16_t incomingSeqNumber = request->hdr.seqNumber;
     uint16_t outgoingSeqNumber = _nextSeqNumber(incomingSeqNumber);
     
@@ -361,20 +393,27 @@ void MockLinkFileServer::_sendNak(uint8_t targetSystemId, uint8_t targetComponen
 /// @brief Emits a Request through the messageReceived signal.
 void MockLinkFileServer::_sendResponse(uint8_t targetSystemId, uint8_t targetComponentId, FileManager::Request* request, uint16_t seqNumber)
 {
-    mavlink_message_t   mavlinkMessage;
-    
     request->hdr.seqNumber = seqNumber;
+    _lastReplySequence = seqNumber;
+    _lastReplyValid = true;
     
     mavlink_msg_file_transfer_protocol_pack_chan(_systemIdServer,    // System ID
                                                  0,                  // Component ID
                                                  _mockLink->mavlinkChannel(),
-                                                 &mavlinkMessage,    // Mavlink Message to pack into
+                                                 &_lastReply,    // Mavlink Message to pack into
                                                  0,                  // Target network
                                                  targetSystemId,
                                                  targetComponentId,
                                                  (uint8_t*)request); // Payload
 
-    _mockLink->respondWithMavlinkMessage(mavlinkMessage);
+	if (_randomDropsEnabled) {
+	    if (rand() % 3 == 0) {
+	        qDebug() << "FileServer: Random drop of outgoing packet";
+	        return;
+	    }
+	}
+    
+    _mockLink->respondWithMavlinkMessage(_lastReply);
 }
 
 /// @brief Generates the next sequence number given an incoming sequence number. Handles generating

src/comm/MockLinkFileServer.h

@@ -70,6 +70,8 @@ public:
     /// @brief The set of files supported by the mock server for testing purposes. Each one represents a different edge case for testing.
     static const FileTestCase rgFileTestCases[cFileTestCases];
     
+    void enableRandromDrops(bool enable) { _randomDropsEnabled = enable; }
+
 signals:
     /// You can connect to this signal to be notified when the server receives a Terminate command.
     void terminateCommandReceived(void);
@@ -89,6 +91,9 @@ private:
     void _resetCommand(uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber);
     uint16_t _nextSeqNumber(uint16_t seqNumber);
     
+    /// if request is a string, this ensures it's null-terminated
+    static void ensureNullTemination(FileManager::Request* request);
+
     QStringList _fileList;  ///< List of files returned by List command
     
     static const uint8_t    _sessionId;
@@ -97,6 +102,12 @@ private:
     const uint8_t           _systemIdServer;    ///< System ID for server
     const uint8_t           _componentIdServer; ///< Component ID for server
     MockLink*               _mockLink;          ///< MockLink to communicate through
+
+    bool _lastReplyValid;
+    uint16_t _lastReplySequence;
+    mavlink_message_t _lastReply;
+
+    bool _randomDropsEnabled;
 };
 
 #endif

src/qgcunittest/FileManagerTest.cc

@@ -128,6 +128,9 @@ void FileManagerTest::_listTest(void)
     Q_ASSERT(_multiSpy);
     Q_ASSERT(_multiSpy->checkNoSignals() == true);
 
+    // test the automatic retry behavior by enabling random drops
+    _fileServer->enableRandromDrops(true);
+    
     // FileManager::listDirectory signalling as follows:
     //  Emits a listEntry signal for each list entry
     //  Emits an commandError signal if:
@@ -192,6 +195,8 @@ void FileManagerTest::_listTest(void)
         _multiSpy->clearAllSignals();
         _fileServer->setErrorMode(MockLinkFileServer::errModeNone);
     }
+
+    _fileServer->enableRandromDrops(false);
 }
 
 #if 0

src/qgcunittest/FileManagerTest.h

@@ -72,7 +72,7 @@ private:
     
     /// @brief This is the amount of time to wait to allow the FileManager enough time to timeout waiting for an Ack.
     /// As such it must be larger than the Ack Timeout used by the FileManager.
-    static const int _ackTimerTimeoutMsecs = FileManager::ackTimerTimeoutMsecs * 2;
+    static const int _ackTimerTimeoutMsecs = FileManager::ackTimerMaxRetries * FileManager::ackTimerTimeoutMsecs * 2;
     
     QStringList _fileListReceived;
 };

src/uas/FileManager.h

@@ -14,10 +14,18 @@
 #include <QObject>
 #include <QDir>
 #include <QTimer>
+#include <QQueue>
 
 #include "UASInterface.h"
 #include "QGCLoggingCategory.h"
 
+#ifdef __GNUC__
+  #define PACKED_STRUCT( __Declaration__ ) __Declaration__ __attribute__((packed))
+#else
+  #define PACKED_STRUCT( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )
+#endif
+
+
 Q_DECLARE_LOGGING_CATEGORY(FileManagerLog)
 
 class Vehicle;
@@ -35,7 +43,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
@@ -54,6 +64,9 @@ public:
     /// Upload the specified file to the specified location
     void uploadPath(const QString& toPath, const QFileInfo& uploadFile);
     
+    /// Create a remote directory
+    void createDirectory(const QString& directory);
+
 signals:
     // Signals associated with the listDirectory method
     
@@ -80,9 +93,11 @@ private slots:
 	void _ackTimeout(void);
 
 private:
-    /// @brief This is the fixed length portion of the protocol data. Trying to pack structures across differing compilers is
-    /// questionable, so we pad the structure ourselves to 32 bit alignment which should get us what we want.
-    struct RequestHeader
+    /// @brief This is the fixed length portion of the protocol data.
+    /// This needs to be packed, because it's typecasted from mavlink_file_transfer_protocol_t.payload, which starts
+    /// at a 3 byte offset, causing an unaligned access to seq_number and offset
+    PACKED_STRUCT(
+    typedef struct _RequestHeader
         {
             uint16_t    seqNumber;      ///< sequence number for message
             uint8_t     session;        ///< Session id for read and write commands
@@ -92,11 +107,12 @@ private:
             uint8_t     burstComplete;  ///< Only used if req_opcode=kCmdBurstReadFile - 1: set of burst packets complete, 0: More burst packets coming.
             uint8_t     padding;        ///< 32 bit aligment padding
             uint32_t    offset;         ///< Offsets for List and Read commands
-        };
+        }) RequestHeader;
 
-    struct Request
+    PACKED_STRUCT(
+    typedef struct _Request
     {
-        struct RequestHeader hdr;
+        RequestHeader hdr;
 
         // We use a union here instead of just casting (uint32_t)&payload[0] to not break strict aliasing rules
         union {
@@ -110,7 +126,7 @@ private:
             // Length of file chunk written by write command
             uint32_t writeFileLength;
         };
-    };
+    }) Request;
 
     enum Opcode
 	{
@@ -164,6 +180,7 @@ private:
 			kCOBurst,		// waiting for Burst response
             kCOWrite,       // waiting for Write response
             kCOCreate,      // waiting for Create response
+            kCOCreateDir,   // waiting for Create Directory response
         };
     
     bool _sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpState);
@@ -172,6 +189,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);
@@ -184,16 +202,18 @@ private:
     void _closeDownloadSession(bool success);
     void _closeUploadSession(bool success);
     void _downloadWorker(const QString& from, const QDir& downloadDir, bool readFile);
+    void _requestMissingData();
     
     static QString errorString(uint8_t errorCode);
 
     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
@@ -207,7 +227,14 @@ private:
     uint32_t    _writeFileSize;             ///< Size of file being uploaded
     QByteArray  _writeFileAccumulator;      ///< Holds file being uploaded
     
+    struct MissingData {
+        uint32_t offset;
+        uint32_t size;
+    };
     uint32_t    _downloadOffset;            ///< current download offset
+    uint32_t    _missingDownloadedBytes;    ///< number of missing bytes for burst download
+    QQueue<MissingData> _missingData;       ///< missing chunks of downloaded file (for burst downloads)
+    bool        _downloadingMissingParts;   ///< true if we are currently downloading missing parts
     QByteArray  _readFileAccumulator;       ///< Holds file being downloaded
     QDir        _readFileDownloadDir;       ///< Directory to download file to
     QString     _readFileDownloadFilename;  ///< Filename (no path) for download file