Merge pull request #849 from DonLakeFlyer/FTP

Modified to use new FILE_TRANSFER_PROTOCOL message

src/qgcunittest/MockMavlinkFileServer.cc

@@ -45,8 +45,10 @@ const MockMavlinkFileServer::FileTestCase MockMavlinkFileServer::rgFileTestCases
 // We only support a single fixed session
 const uint8_t MockMavlinkFileServer::_sessionId = 1;
 
-MockMavlinkFileServer::MockMavlinkFileServer(void) :
-    _errMode(errModeNone)
+MockMavlinkFileServer::MockMavlinkFileServer(uint8_t systemIdQGC, uint8_t systemIdServer) :
+    _errMode(errModeNone),
+    _systemIdServer(systemIdServer),
+    _systemIdQGC(systemIdQGC)
 {
 
 }
@@ -74,7 +76,6 @@ void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request, ui
         return;
     }
     
-    ackResponse.hdr.magic = 'f';
     ackResponse.hdr.opcode = QGCUASFileManager::kRspAck;
     ackResponse.hdr.session = 0;
     ackResponse.hdr.offset = request->hdr.offset;
@@ -133,7 +134,6 @@ void MockMavlinkFileServer::_openCommand(QGCUASFileManager::Request* request, ui
         return;
     }
     
-    response.hdr.magic = 'f';
     response.hdr.opcode = QGCUASFileManager::kRspAck;
     response.hdr.session = _sessionId;
     
@@ -183,7 +183,6 @@ void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request, ui
     // We should always have written something, otherwise there is something wrong with the code above
     Q_ASSERT(cDataBytes);
     
-    response.hdr.magic = 'f';
     response.hdr.session = _sessionId;
     response.hdr.size = cDataBytes;
     response.hdr.offset = request->hdr.offset;
@@ -214,15 +213,15 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message)
 {
     QGCUASFileManager::Request  ackResponse;
 
-    Q_ASSERT(message.msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA);
+    Q_ASSERT(message.msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL);
     
-    mavlink_encapsulated_data_t requestEncapsulatedData;
-    mavlink_msg_encapsulated_data_decode(&message, &requestEncapsulatedData);
-    QGCUASFileManager::Request* request = (QGCUASFileManager::Request*)&requestEncapsulatedData.data[0];
+    mavlink_file_transfer_protocol_t requestFileTransferProtocol;
+    mavlink_msg_file_transfer_protocol_decode(&message, &requestFileTransferProtocol);
+    QGCUASFileManager::Request* request = (QGCUASFileManager::Request*)&requestFileTransferProtocol.payload[0];
 
-    Q_ASSERT(request->hdr.magic == QGCUASFileManager::kProtocolMagic);
-
-    uint16_t incomingSeqNumber = requestEncapsulatedData.seqnr;
+    Q_ASSERT(requestFileTransferProtocol.target_system == _systemIdServer);
+    
+    uint16_t incomingSeqNumber = request->hdr.seqNumber;
     uint16_t outgoingSeqNumber = _nextSeqNumber(incomingSeqNumber);
     
     if (_errMode == errModeNoResponse) {
@@ -236,6 +235,7 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message)
 
     // Validate CRC
     if (request->hdr.crc32 != QGCUASFileManager::crc32(request)) {
+        qDebug() << "Bad CRC received - opcode:" << request->hdr.opcode << "expected:" << request->hdr.crc32 << "actual:" << QGCUASFileManager::crc32(request);
         _sendNak(QGCUASFileManager::kErrCrc, outgoingSeqNumber);
     }
 
@@ -250,7 +250,6 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message)
 
         case QGCUASFileManager::kCmdNone:
             // ignored, always acked
-            ackResponse.hdr.magic = 'f';
             ackResponse.hdr.opcode = QGCUASFileManager::kRspAck;
             ackResponse.hdr.session = 0;
             ackResponse.hdr.crc32 = 0;
@@ -294,7 +293,6 @@ void MockMavlinkFileServer::_sendAck(uint16_t seqNumber)
 {
     QGCUASFileManager::Request ackResponse;
     
-    ackResponse.hdr.magic = 'f';
     ackResponse.hdr.opcode = QGCUASFileManager::kRspAck;
     ackResponse.hdr.session = 0;
     ackResponse.hdr.size = 0;
@@ -307,7 +305,6 @@ void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error, uint16_
 {
     QGCUASFileManager::Request nakResponse;
 
-    nakResponse.hdr.magic = 'f';
     nakResponse.hdr.opcode = QGCUASFileManager::kRspNak;
     nakResponse.hdr.session = 0;
     nakResponse.hdr.size = 1;
@@ -321,14 +318,21 @@ void MockMavlinkFileServer::_emitResponse(QGCUASFileManager::Request* request, u
 {
     mavlink_message_t   mavlinkMessage;
     
-    request->hdr.magic = QGCUASFileManager::kProtocolMagic;
+    request->hdr.seqNumber = seqNumber;
+    
     request->hdr.crc32 = QGCUASFileManager::crc32(request);
     if (_errMode == errModeBadCRC) {
         // Return a bad CRC
         request->hdr.crc32++;
     }
     
-    mavlink_msg_encapsulated_data_pack(250, 50, &mavlinkMessage, seqNumber, (uint8_t*)request);
+    mavlink_msg_file_transfer_protocol_pack(_systemIdServer,    // System ID
+                                            0,                  // Component ID
+                                            &mavlinkMessage,    // Mavlink Message to pack into
+                                            0,                  // Target network
+                                            _systemIdQGC,       // QGC Target System ID
+                                            0,                  // Target component
+                                            (uint8_t*)request); // Payload
     
     emit messageReceived(NULL, mavlinkMessage);
 }

src/qgcunittest/MockMavlinkFileServer.h

@@ -40,7 +40,10 @@ class MockMavlinkFileServer : public MockMavlinkInterface
     Q_OBJECT
     
 public:
-    MockMavlinkFileServer(void);
+    /// @brief Constructor for MockMavlinkFileServer
+    ///     @param System ID for QGroundControl App
+    ///     @pqram System ID for this Server
+    MockMavlinkFileServer(uint8_t systemIdQGC, uint8_t systemIdServer);
     
     /// @brief Sets the list of files returned by the List command. Prepend names with F or D
     /// to indicate (F)ile or (D)irectory.
@@ -103,6 +106,8 @@ private:
     static const uint8_t    _sessionId;
     uint8_t                 _readFileLength;    ///< Length of active file being read
     ErrorMode_t             _errMode;           ///< Currently set error mode, as specified by setErrorMode
+    const uint8_t           _systemIdServer;    ///< System ID for server
+    const uint8_t           _systemIdQGC;       ///< QGC System ID
 };
 
 #endif

src/qgcunittest/QGCUASFileManagerTest.cc

@@ -31,6 +31,7 @@
 ///     @author Don Gagne <don@thegagnes.com>
 
 QGCUASFileManagerUnitTest::QGCUASFileManagerUnitTest(void) :
+    _mockFileServer(_systemIdQGC, _systemIdServer),
     _fileManager(NULL),
     _multiSpy(NULL)
 {
@@ -40,6 +41,7 @@ QGCUASFileManagerUnitTest::QGCUASFileManagerUnitTest(void) :
 // Called once before all test cases are run
 void QGCUASFileManagerUnitTest::initTestCase(void)
 {
+    _mockUAS.setMockSystemId(_systemIdServer);
     _mockUAS.setMockMavlinkPlugin(&_mockFileServer);
 }
 
@@ -48,19 +50,16 @@ void QGCUASFileManagerUnitTest::init(void)
 {
     Q_ASSERT(_multiSpy == NULL);
     
-    _fileManager = new QGCUASFileManager(NULL, &_mockUAS);
+    _fileManager = new QGCUASFileManager(NULL, &_mockUAS, _systemIdQGC);
     Q_CHECK_PTR(_fileManager);
     
     // Reset any internal state back to normal
     _mockFileServer.setErrorMode(MockMavlinkFileServer::errModeNone);
     _fileListReceived.clear();
     
-    bool connected = connect(&_mockFileServer, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), _fileManager, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
-    Q_ASSERT(connected);
-    Q_UNUSED(connected);    // Silent release build compiler warning
+    connect(&_mockFileServer, &MockMavlinkFileServer::messageReceived, _fileManager, &QGCUASFileManager::receiveMessage);
 
-    connected = connect(_fileManager, SIGNAL(listEntry(const QString&)), this, SLOT(listEntry(const QString&)));
-    Q_ASSERT(connected);
+    connect(_fileManager, &QGCUASFileManager::listEntry, this, &QGCUASFileManagerUnitTest::listEntry);
 
     _rgSignals[listEntrySignalIndex] = SIGNAL(listEntry(const QString&));
     _rgSignals[listCompleteSignalIndex] = SIGNAL(listComplete(void));

src/qgcunittest/QGCUASFileManagerTest.h

@@ -80,6 +80,10 @@ private:
         downloadFileCompleteSignalMask =    1 << downloadFileCompleteSignalIndex,
         errorMessageSignalMask =            1 << errorMessageSignalIndex,
     };
+
+    static const uint8_t    _systemIdQGC = 255;
+    static const uint8_t    _systemIdServer = 128;
+
     MockUAS                 _mockUAS;
     MockMavlinkFileServer   _mockFileServer;
     

src/uas/QGCUASFileManager.cc

@@ -24,6 +24,7 @@
 #include "QGCUASFileManager.h"
 #include "QGC.h"
 #include "MAVLinkProtocol.h"
+#include "MainWindow.h"
 
 #include <QFile>
 #include <QDir>
@@ -66,16 +67,20 @@ static const quint32 crctab[] =
 };
 
 
-QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) :
+QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas, uint8_t unitTestSystemIdQGC) :
     QObject(parent),
     _currentOperation(kCOIdle),
     _mav(uas),
     _lastOutgoingSeqNumber(0),
-    _activeSession(0)
+    _activeSession(0),
+    _systemIdQGC(unitTestSystemIdQGC)
 {
-    bool connected = connect(&_ackTimer, SIGNAL(timeout()), this, SLOT(_ackTimeout()));
-    Q_ASSERT(connected);
-    Q_UNUSED(connected);    // Silence retail unused variable error
+    connect(&_ackTimer, &QTimer::timeout, this, &QGCUASFileManager::_ackTimeout);
+    
+    _systemIdServer = _mav->getUASID();
+    
+    // Make sure we don't have bad structure packing
+    Q_ASSERT(sizeof(RequestHeader) == 16);
 }
 
 /// @brief Calculates a 32 bit CRC for the specified request.
@@ -90,6 +95,9 @@ quint32 QGCUASFileManager::crc32(Request* request, unsigned state)
     // Always calculate CRC with 0 initial CRC value
     quint32 crcSave = request->hdr.crc32;
     request->hdr.crc32 = 0;
+    request->hdr.padding[0] = 0;
+    request->hdr.padding[1] = 0;
+    request->hdr.padding[2] = 0;
 
     for (size_t i=0; i < cbData; i++)
         state = crctab[(state ^ data[i]) & 0xff] ^ (state >> 8);
@@ -182,6 +190,7 @@ void QGCUASFileManager::_readAckResponse(Request* readAck)
         request.hdr.session = _activeSession;
         request.hdr.opcode = kCmdRead;
         request.hdr.offset = _readOffset;
+        request.hdr.size = 0;
 
         _sendRequest(&request);
     } else {
@@ -252,28 +261,25 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me
 {
     Q_UNUSED(link);
 
-    if (message.msgid != MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
-        // wtf, not for us
-        return;
-    }
-
-    // XXX: hack to prevent files from videostream to interfere
-    // FIXME: magic number
-    if (message.compid != 50) {
+    // receiveMessage is signalled will all mavlink messages so we need to filter everything else out but ours.
+    if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
         return;
     }
-
-    mavlink_encapsulated_data_t data;
-    mavlink_msg_encapsulated_data_decode(&message, &data);
-    Request* request = (Request*)&data.data[0];
     
-    if (request->hdr.magic != kProtocolMagic) {
+    mavlink_file_transfer_protocol_t data;
+    mavlink_msg_file_transfer_protocol_decode(&message, &data);
+    
+    // Make sure we are the target system
+    if (data.target_system != _systemIdQGC) {
+        qDebug() << "Received MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL with possibly incorrect system id" << _systemIdQGC;
         return;
     }
-
+    
+    Request* request = (Request*)&data.payload[0];
+    
     _clearAckTimeout();
     
-    uint16_t incomingSeqNumber = data.seqnr;
+    uint16_t incomingSeqNumber = request->hdr.seqNumber;
     
     // Make sure we have a good CRC
     quint32 expectedCRC = crc32(request);
@@ -381,10 +387,10 @@ void QGCUASFileManager::_sendListCommand(void)
 {
     Request request;
 
-    request.hdr.magic = 'f';
     request.hdr.session = 0;
     request.hdr.opcode = kCmdList;
     request.hdr.offset = _listOffset;
+    request.hdr.size = 0;
 
     _fillRequestWithString(&request, _listPath);
 
@@ -416,10 +422,10 @@ void QGCUASFileManager::downloadPath(const QString& from, const QDir& downloadDi
     _currentOperation = kCOOpen;
 
     Request request;
-    request.hdr.magic = 'f';
     request.hdr.session = 0;
     request.hdr.opcode = kCmdOpen;
     request.hdr.offset = 0;
+    request.hdr.size = 0;
     _fillRequestWithString(&request, from);
     _sendRequest(&request);
 }
@@ -470,7 +476,6 @@ bool QGCUASFileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpS
     }
 
     Request request;
-    request.hdr.magic = 'f';
     request.hdr.session = 0;
     request.hdr.opcode = opcode;
     request.hdr.offset = 0;
@@ -523,6 +528,7 @@ void QGCUASFileManager::_sendTerminateCommand(void)
     Request request;
     request.hdr.session = _activeSession;
     request.hdr.opcode = kCmdTerminate;
+    request.hdr.size = 0;
     _sendRequest(&request);
 }
 
@@ -547,10 +553,22 @@ void QGCUASFileManager::_sendRequest(Request* request)
     
     _lastOutgoingSeqNumber++;
 
-    request->hdr.magic = kProtocolMagic;
+    request->hdr.seqNumber = _lastOutgoingSeqNumber;
+    
     request->hdr.crc32 = crc32(request);
-    // FIXME: Send correct system id instead of harcoded 250
-    // FIXME: What about the component id? Should it be set to something specific.
-    mavlink_msg_encapsulated_data_pack(250, 0, &message, _lastOutgoingSeqNumber, (uint8_t*)request);
+    
+    if (_systemIdQGC == 0) {
+        _systemIdQGC = MainWindow::instance()->getMAVLink()->getSystemId();
+    }
+    
+    Q_ASSERT(_mav);
+    mavlink_msg_file_transfer_protocol_pack(_systemIdQGC,       // QGC System ID
+                                            0,                  // QGC Component ID
+                                            &message,           // Mavlink Message to pack into
+                                            0,                  // Target network
+                                            _systemIdServer,    // Target system
+                                            0,                  // Target component
+                                            (uint8_t*)request); // Payload
+    
     _mav->sendMessage(message);
 }

src/uas/QGCUASFileManager.h

@@ -33,7 +33,7 @@ class QGCUASFileManager : public QObject
 {
     Q_OBJECT
 public:
-    QGCUASFileManager(QObject* parent, UASInterface* uas);
+    QGCUASFileManager(QObject* parent, UASInterface* uas, uint8_t unitTestSystemIdQGC = 0);
     
     /// These methods are only used for testing purposes.
     bool _sendCmdTestAck(void) { return _sendOpcodeOnlyCmd(kCmdNone, kCOAck); };
@@ -74,26 +74,29 @@ public slots:
     void downloadPath(const QString& from, const QDir& downloadDir);
 
 protected:
-    static const uint8_t kProtocolMagic = 'f';
+    
+    /// @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
         {
-            uint8_t		magic;      ///> Magic byte 'f' to idenitfy FTP protocol
-            uint8_t		session;    ///> Session id for read and write commands
-            uint8_t		opcode;     ///> Command opcode
-            uint8_t		size;       ///> Size of data
-            uint32_t	crc32;      ///> CRC for entire Request structure, with crc32 set to 0
-            uint32_t	offset;     ///> Offsets for List and Read commands
+            uint16_t    seqNumber;  ///< sequence number for message
+            uint8_t     session;    ///< Session id for read and write commands
+            uint8_t     opcode;     ///< Command opcode
+            uint8_t     size;       ///< Size of data
+            uint8_t     padding[3];
+            uint32_t    crc32;      ///< CRC for entire Request structure, with crc32 and padding set to 0
+            uint32_t    offset;     ///< Offsets for List and Read commands
         };
 
     struct Request
     {
         struct RequestHeader hdr;
 
-        // We use a union here instead of just casting (uint32_t)&data[0] to not break strict aliasing rules
+        // We use a union here instead of just casting (uint32_t)&payload[0] to not break strict aliasing rules
         union {
-            // The entire Request must fit into the data member of the mavlink_encapsulated_data_t structure. We use as many leftover bytes
+            // The entire Request must fit into the payload member of the mavlink_file_transfer_protocol_t structure. We use as many leftover bytes
             // after we use up space for the RequestHeader for the data portion of the Request.
-            uint8_t data[sizeof(((mavlink_encapsulated_data_t*)0)->data) - sizeof(RequestHeader)];
+            uint8_t data[sizeof(((mavlink_file_transfer_protocol_t*)0)->payload) - sizeof(RequestHeader)];
 
             // File length returned by Open command
             uint32_t openFileLength;
@@ -103,22 +106,22 @@ protected:
     enum Opcode
         {
             // Commands
-            kCmdNone,       ///> ignored, always acked
-            kCmdTerminate,	///> releases sessionID, closes file
-            kCmdReset,      ///> terminates all sessions
-            kCmdList,       ///> list files in <path> from <offset>
-            kCmdOpen,       ///> opens <path> for reading, returns <session>
-            kCmdRead,       ///> reads <size> bytes from <offset> in <session>
-            kCmdCreate,     ///> creates <path> for writing, returns <session>
-            kCmdWrite,      ///> appends <size> bytes at <offset> in <session>
-            kCmdRemove,     ///> remove file (only if created by server?)
+            kCmdNone,       ///< ignored, always acked
+            kCmdTerminate,	///< releases sessionID, closes file
+            kCmdReset,      ///< terminates all sessions
+            kCmdList,       ///< list files in <path> from <offset>
+            kCmdOpen,       ///< opens <path> for reading, returns <session>
+            kCmdRead,       ///< reads <size> bytes from <offset> in <session>
+            kCmdCreate,     ///< creates <path> for writing, returns <session>
+            kCmdWrite,      ///< appends <size> bytes at <offset> in <session>
+            kCmdRemove,     ///< remove file (only if created by server?)
 
             // Responses
-            kRspAck,        ///> positive acknowledgement of previous command
-            kRspNak,        ///> negative acknowledgement of previous command
+            kRspAck,        ///< positive acknowledgement of previous command
+            kRspNak,        ///< negative acknowledgement of previous command
             
             // Used for testing only, not part of protocol
-            kCmdTestNoAck,  // ignored, ack not sent back, should timeout waiting for ack
+            kCmdTestNoAck,  ///< ignored, ack not sent back, should timeout waiting for ack
         };
 
     enum ErrorCode
@@ -170,21 +173,24 @@ protected:
     static quint32 crc32(Request* request, unsigned state = 0);
     static QString errorString(uint8_t errorCode);
 
-    OperationState  _currentOperation;              ///> Current operation of state machine
-    QTimer          _ackTimer;                      ///> Used to signal a timeout waiting for an ack
+    OperationState  _currentOperation;              ///< Current operation of state machine
+    QTimer          _ackTimer;                      ///< Used to signal a timeout waiting for an ack
     
     UASInterface* _mav;
     
     uint16_t _lastOutgoingSeqNumber; ///< Sequence number sent in last outgoing packet
 
-    unsigned    _listOffset;    ///> offset for the current List operation
-    QString     _listPath;      ///> path for the current List operation
+    unsigned    _listOffset;    ///< offset for the current List operation
+    QString     _listPath;      ///< path for the current List operation
+    
+    uint8_t     _activeSession;             ///< currently active session, 0 for none
+    uint32_t    _readOffset;                ///< current read offset
+    QByteArray  _readFileAccumulator;       ///< Holds file being downloaded
+    QDir        _readFileDownloadDir;       ///< Directory to download file to
+    QString     _readFileDownloadFilename;  ///< Filename (no path) for download file
     
-    uint8_t     _activeSession;             ///> currently active session, 0 for none
-    uint32_t    _readOffset;                ///> current read offset
-    QByteArray  _readFileAccumulator;       ///> Holds file being downloaded
-    QDir        _readFileDownloadDir;       ///> Directory to download file to
-    QString     _readFileDownloadFilename;  ///> Filename (no path) for download file
+    uint8_t     _systemIdQGC;               ///< System ID for QGC
+    uint8_t     _systemIdServer;            ///< System ID for server
     
     // We give MockMavlinkFileServer friend access so that it can use the data structures and opcodes
     // to build a mock mavlink file server for testing.