diff --git a/src/qgcunittest/MockMavlinkFileServer.cc b/src/qgcunittest/MockMavlinkFileServer.cc index b4945b3b015cf72aab58221f9b87dc9042934e00..9d5e52bce80c48d889d7e590cc25e47330b7d0d3 100644 --- a/src/qgcunittest/MockMavlinkFileServer.cc +++ b/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) { @@ -250,7 +249,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 +292,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 +304,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 +317,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); } diff --git a/src/qgcunittest/MockMavlinkFileServer.h b/src/qgcunittest/MockMavlinkFileServer.h index c3cd78208874277962fe16196f112cc6d08d2b3e..0ac2552f6d56d513a2281771eae6d324101e0307 100644 --- a/src/qgcunittest/MockMavlinkFileServer.h +++ b/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 diff --git a/src/qgcunittest/QGCUASFileManagerTest.cc b/src/qgcunittest/QGCUASFileManagerTest.cc index 0e83d47f95a0312966af001cfe0f98b1ddab73bb..40d0ae20f7505f66e6f7047596878c0804fe93c3 100644 --- a/src/qgcunittest/QGCUASFileManagerTest.cc +++ b/src/qgcunittest/QGCUASFileManagerTest.cc @@ -31,6 +31,7 @@ /// @author Don Gagne 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)); diff --git a/src/qgcunittest/QGCUASFileManagerTest.h b/src/qgcunittest/QGCUASFileManagerTest.h index fc7d0063eeff36c8f17015a7cad8516e700d69ca..9b241747d3129c6154552ba8c54937e94ebc97ac 100644 --- a/src/qgcunittest/QGCUASFileManagerTest.h +++ b/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; diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 354a437f7401decd3af8392c7d3f7076765d531c..b28858d760f5011aa8d6aa51baebb7230b9287a8 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -24,6 +24,7 @@ #include "QGCUASFileManager.h" #include "QGC.h" #include "MAVLinkProtocol.h" +#include "MainWindow.h" #include #include @@ -66,16 +67,17 @@ 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(); } /// @brief Calculates a 32 bit CRC for the specified request. @@ -252,28 +254,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,7 +380,6 @@ void QGCUASFileManager::_sendListCommand(void) { Request request; - request.hdr.magic = 'f'; request.hdr.session = 0; request.hdr.opcode = kCmdList; request.hdr.offset = _listOffset; @@ -416,7 +414,6 @@ 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; @@ -470,7 +467,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; @@ -547,10 +543,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); } diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 7fe7f9d9d0dd134401cdce902267c19fff3e8f8d..4c41eb34e120b7568a0d553a275dae12adb0dfe1 100644 --- a/src/uas/QGCUASFileManager.h +++ b/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,28 @@ 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. This structure is layed out such that it should not require + /// any special compiler packing to not consume extra space. 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 + unsigned int session:4; ///< Session id for read and write commands + unsigned int opcode:4; ///< 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 }; 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 +105,22 @@ protected: enum Opcode { // Commands - kCmdNone, ///> ignored, always acked - kCmdTerminate, ///> releases sessionID, closes file - kCmdReset, ///> terminates all sessions - kCmdList, ///> list files in from - kCmdOpen, ///> opens for reading, returns - kCmdRead, ///> reads bytes from in - kCmdCreate, ///> creates for writing, returns - kCmdWrite, ///> appends bytes at in - 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 from + kCmdOpen, ///< opens for reading, returns + kCmdRead, ///< reads bytes from in + kCmdCreate, ///< creates for writing, returns + kCmdWrite, ///< appends bytes at in + 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 +172,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.