diff --git a/src/qgcunittest/MockMavlinkFileServer.cc b/src/qgcunittest/MockMavlinkFileServer.cc index bbcbfed..f675927 100644 --- a/src/qgcunittest/MockMavlinkFileServer.cc +++ b/src/qgcunittest/MockMavlinkFileServer.cc @@ -29,6 +29,7 @@ const MockMavlinkFileServer::ErrorMode_t MockMavlinkFileServer::rgFailureModes[] MockMavlinkFileServer::errModeNoSecondResponse, MockMavlinkFileServer::errModeNakSecondResponse, MockMavlinkFileServer::errModeBadCRC, + MockMavlinkFileServer::errModeBadSequence, }; const size_t MockMavlinkFileServer::cFailureModes = sizeof(MockMavlinkFileServer::rgFailureModes) / sizeof(MockMavlinkFileServer::rgFailureModes[0]); @@ -52,23 +53,24 @@ MockMavlinkFileServer::MockMavlinkFileServer(void) : /// @brief Handles List command requests. Only supports root folder paths. /// File list returned is set using the setFileList method. -void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request) +void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request, uint16_t seqNumber) { // FIXME: Does not support directories that span multiple packets QGCUASFileManager::Request ackResponse; QString path; + uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); // We only support root path path = (char *)&request->data[0]; if (!path.isEmpty() && path != "/") { - _sendNak(QGCUASFileManager::kErrNotDir); + _sendNak(QGCUASFileManager::kErrNotDir, outgoingSeqNumber); return; } // Offset requested is past the end of the list if (request->hdr.offset > (uint32_t)_fileList.size()) { - _sendNak(QGCUASFileManager::kErrEOF); + _sendNak(QGCUASFileManager::kErrEOF, outgoingSeqNumber); return; } @@ -90,25 +92,26 @@ void MockMavlinkFileServer::_listCommand(QGCUASFileManager::Request* request) bufPtr += cchFilename + 1; } - _emitResponse(&ackResponse); + _emitResponse(&ackResponse, outgoingSeqNumber); } else if (_errMode == errModeNakSecondResponse) { // Nak error all subsequent requests - _sendNak(QGCUASFileManager::kErrPerm); + _sendNak(QGCUASFileManager::kErrPerm, outgoingSeqNumber); return; } else if (_errMode == errModeNoSecondResponse) { // No response for all subsequent requests return; } else { // FIXME: Does not support directories that span multiple packets - _sendNak(QGCUASFileManager::kErrEOF); + _sendNak(QGCUASFileManager::kErrEOF, outgoingSeqNumber); } } /// @brief Handles Open command requests. -void MockMavlinkFileServer::_openCommand(QGCUASFileManager::Request* request) +void MockMavlinkFileServer::_openCommand(QGCUASFileManager::Request* request, uint16_t seqNumber) { QGCUASFileManager::Request response; QString path; + uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); size_t cchPath = strnlen((char *)request->data, sizeof(request->data)); Q_ASSERT(cchPath != sizeof(request->data)); @@ -126,25 +129,29 @@ void MockMavlinkFileServer::_openCommand(QGCUASFileManager::Request* request) } } if (!found) { - _sendNak(QGCUASFileManager::kErrNotFile); + _sendNak(QGCUASFileManager::kErrNotFile, outgoingSeqNumber); return; } response.hdr.magic = 'f'; response.hdr.opcode = QGCUASFileManager::kRspAck; response.hdr.session = _sessionId; - response.hdr.size = 0; - _emitResponse(&response); + // Data contains file length + response.hdr.size = sizeof(uint32_t); + *((uint32_t*)response.data) = _readFileLength; + + _emitResponse(&response, outgoingSeqNumber); } /// @brief Handles Read command requests. -void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request) +void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request, uint16_t seqNumber) { - QGCUASFileManager::Request response; + QGCUASFileManager::Request response; + uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); if (request->hdr.session != _sessionId) { - _sendNak(QGCUASFileManager::kErrNoSession); + _sendNak(QGCUASFileManager::kErrNoSession, outgoingSeqNumber); return; } @@ -155,7 +162,7 @@ void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request) // If we get here it means the client is requesting additional data past the first request if (_errMode == errModeNakSecondResponse) { // Nak error all subsequent requests - _sendNak(QGCUASFileManager::kErrPerm); + _sendNak(QGCUASFileManager::kErrPerm, outgoingSeqNumber); return; } else if (_errMode == errModeNoSecondResponse) { // No rsponse for all subsequent requests @@ -164,7 +171,7 @@ void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request) } if (readOffset >= _readFileLength) { - _sendNak(QGCUASFileManager::kErrEOF); + _sendNak(QGCUASFileManager::kErrEOF, outgoingSeqNumber); return; } @@ -182,18 +189,20 @@ void MockMavlinkFileServer::_readCommand(QGCUASFileManager::Request* request) response.hdr.offset = request->hdr.offset; response.hdr.opcode = QGCUASFileManager::kRspAck; - _emitResponse(&response); + _emitResponse(&response, outgoingSeqNumber); } /// @brief Handles Terminate commands -void MockMavlinkFileServer::_terminateCommand(QGCUASFileManager::Request* request) +void MockMavlinkFileServer::_terminateCommand(QGCUASFileManager::Request* request, uint16_t seqNumber) { + uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); + if (request->hdr.session != _sessionId) { - _sendNak(QGCUASFileManager::kErrNoSession); + _sendNak(QGCUASFileManager::kErrNoSession, outgoingSeqNumber); return; } - _sendAck(); + _sendAck(outgoingSeqNumber); // Let our test harness know that we got a terminate command. This is used to validate the a Terminate is correctly // sent after an Open. @@ -203,26 +212,31 @@ void MockMavlinkFileServer::_terminateCommand(QGCUASFileManager::Request* reques /// @brief Handles messages sent to the FTP server. void MockMavlinkFileServer::sendMessage(mavlink_message_t message) { - QGCUASFileManager::Request ackResponse; + QGCUASFileManager::Request ackResponse; Q_ASSERT(message.msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA); + mavlink_encapsulated_data_t requestEncapsulatedData; + mavlink_msg_encapsulated_data_decode(&message, &requestEncapsulatedData); + QGCUASFileManager::Request* request = (QGCUASFileManager::Request*)&requestEncapsulatedData.data[0]; + + Q_ASSERT(request->hdr.magic == QGCUASFileManager::kProtocolMagic); + + uint16_t incomingSeqNumber = requestEncapsulatedData.seqnr; + uint16_t outgoingSeqNumber = _nextSeqNumber(incomingSeqNumber); + if (_errMode == errModeNoResponse) { // Don't respond to any requests, this shold cause the client to eventually timeout waiting for the ack return; } else if (_errMode == errModeNakResponse) { // Nak all requests, the actual error send back doesn't really matter as long as it's an error - _sendNak(QGCUASFileManager::kErrPerm); + _sendNak(QGCUASFileManager::kErrPerm, outgoingSeqNumber); return; } - mavlink_encapsulated_data_t requestEncapsulatedData; - mavlink_msg_encapsulated_data_decode(&message, &requestEncapsulatedData); - QGCUASFileManager::Request* request = (QGCUASFileManager::Request*)&requestEncapsulatedData.data[0]; - // Validate CRC if (request->hdr.crc32 != QGCUASFileManager::crc32(request)) { - _sendNak(QGCUASFileManager::kErrCrc); + _sendNak(QGCUASFileManager::kErrCrc, outgoingSeqNumber); } switch (request->hdr.opcode) { @@ -241,23 +255,23 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) ackResponse.hdr.session = 0; ackResponse.hdr.crc32 = 0; ackResponse.hdr.size = 0; - _emitResponse(&ackResponse); + _emitResponse(&ackResponse, outgoingSeqNumber); break; case QGCUASFileManager::kCmdList: - _listCommand(request); + _listCommand(request, incomingSeqNumber); break; case QGCUASFileManager::kCmdOpen: - _openCommand(request); + _openCommand(request, incomingSeqNumber); break; case QGCUASFileManager::kCmdRead: - _readCommand(request); + _readCommand(request, incomingSeqNumber); break; case QGCUASFileManager::kCmdTerminate: - _terminateCommand(request); + _terminateCommand(request, incomingSeqNumber); break; // Remainder of commands are NYI @@ -270,13 +284,13 @@ void MockMavlinkFileServer::sendMessage(mavlink_message_t message) // remove file (only if created by server?) default: // nack for all NYI opcodes - _sendNak(QGCUASFileManager::kErrUnknownCommand); + _sendNak(QGCUASFileManager::kErrUnknownCommand, outgoingSeqNumber); break; } } /// @brief Sends an Ack -void MockMavlinkFileServer::_sendAck(void) +void MockMavlinkFileServer::_sendAck(uint16_t seqNumber) { QGCUASFileManager::Request ackResponse; @@ -285,11 +299,11 @@ void MockMavlinkFileServer::_sendAck(void) ackResponse.hdr.session = 0; ackResponse.hdr.size = 0; - _emitResponse(&ackResponse); + _emitResponse(&ackResponse, seqNumber); } /// @brief Sends a Nak with the specified error code. -void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) +void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error, uint16_t seqNumber) { QGCUASFileManager::Request nakResponse; @@ -299,21 +313,34 @@ void MockMavlinkFileServer::_sendNak(QGCUASFileManager::ErrorCode error) nakResponse.hdr.size = 1; nakResponse.data[0] = error; - _emitResponse(&nakResponse); + _emitResponse(&nakResponse, seqNumber); } /// @brief Emits a Request through the messageReceived signal. -void MockMavlinkFileServer::_emitResponse(QGCUASFileManager::Request* request) +void MockMavlinkFileServer::_emitResponse(QGCUASFileManager::Request* request, uint16_t seqNumber) { mavlink_message_t mavlinkMessage; + request->hdr.magic = QGCUASFileManager::kProtocolMagic; request->hdr.crc32 = QGCUASFileManager::crc32(request); if (_errMode == errModeBadCRC) { // Return a bad CRC request->hdr.crc32++; } - mavlink_msg_encapsulated_data_pack(250, MAV_COMP_ID_IMU, &mavlinkMessage, 0 /*_encdata_seq*/, (uint8_t*)request); + mavlink_msg_encapsulated_data_pack(250, 50, &mavlinkMessage, seqNumber, (uint8_t*)request); emit messageReceived(NULL, mavlinkMessage); } + +/// @brief Generates the next sequence number given an incoming sequence number. Handles generating +/// bad sequence numbers when errModeBadSequence is set. +uint16_t MockMavlinkFileServer::_nextSeqNumber(uint16_t seqNumber) +{ + uint16_t outgoingSeqNumber = seqNumber + 1; + + if (_errMode == errModeBadSequence) { + outgoingSeqNumber++; + } + return outgoingSeqNumber; +} diff --git a/src/qgcunittest/MockMavlinkFileServer.h b/src/qgcunittest/MockMavlinkFileServer.h index fd00339..c3cd782 100644 --- a/src/qgcunittest/MockMavlinkFileServer.h +++ b/src/qgcunittest/MockMavlinkFileServer.h @@ -54,7 +54,7 @@ public: errModeNoSecondResponse, ///< No response to subsequent request to initial command errModeNakSecondResponse, ///< Nak subsequent request to initial command errModeBadCRC, ///< Return response with bad CRC - errModeBadSequence ///< Return response with bad sequence number, NYI: Waiting on Firmware sequence # support + errModeBadSequence ///< Return response with bad sequence number } ErrorMode_t; /// @brief Sets the error mode for command responses. This allows you to simulate various server errors. @@ -89,13 +89,14 @@ signals: void terminateCommandReceived(void); private: - void _sendAck(void); - void _sendNak(QGCUASFileManager::ErrorCode error); - void _emitResponse(QGCUASFileManager::Request* request); - void _listCommand(QGCUASFileManager::Request* request); - void _openCommand(QGCUASFileManager::Request* request); - void _readCommand(QGCUASFileManager::Request* request); - void _terminateCommand(QGCUASFileManager::Request* request); + void _sendAck(uint16_t seqNumber); + void _sendNak(QGCUASFileManager::ErrorCode error, uint16_t seqNumber); + void _emitResponse(QGCUASFileManager::Request* request, uint16_t seqNumber); + void _listCommand(QGCUASFileManager::Request* request, uint16_t seqNumber); + void _openCommand(QGCUASFileManager::Request* request, uint16_t seqNumber); + void _readCommand(QGCUASFileManager::Request* request, uint16_t seqNumber); + void _terminateCommand(QGCUASFileManager::Request* request, uint16_t seqNumber); + uint16_t _nextSeqNumber(uint16_t seqNumber); QStringList _fileList; ///< List of files returned by List command diff --git a/src/qgcunittest/QGCUASFileManagerTest.cc b/src/qgcunittest/QGCUASFileManagerTest.cc index 96f1c14..de1a899 100644 --- a/src/qgcunittest/QGCUASFileManagerTest.cc +++ b/src/qgcunittest/QGCUASFileManagerTest.cc @@ -65,6 +65,8 @@ void QGCUASFileManagerUnitTest::init(void) _rgSignals[statusMessageSignalIndex] = SIGNAL(statusMessage(const QString&)); _rgSignals[errorMessageSignalIndex] = SIGNAL(errorMessage(const QString&)); _rgSignals[resetStatusMessagesSignalIndex] = SIGNAL(resetStatusMessages(void)); + _rgSignals[listCompleteSignalIndex] = SIGNAL(listComplete(void)); + _rgSignals[openFileLengthSignalIndex] = SIGNAL(openFileLength(unsigned int)); _multiSpy = new MultiSignalSpy(); Q_CHECK_PTR(_multiSpy); @@ -101,13 +103,21 @@ void QGCUASFileManagerUnitTest::_ackTest(void) // If the file manager doesn't receive an ack it will timeout and emit an error. So make sure // we don't get any error signals. QVERIFY(_fileManager->_sendCmdTestAck()); + QTest::qWait(_ackTimerTimeoutMsecs); // Let the file manager timeout QVERIFY(_multiSpy->checkNoSignals()); - // Setup for no response from ack. This should cause a timeout error; + // Setup for no response from ack. This should cause a timeout error _mockFileServer.setErrorMode(MockMavlinkFileServer::errModeNoResponse); QVERIFY(_fileManager->_sendCmdTestAck()); QTest::qWait(_ackTimerTimeoutMsecs); // Let the file manager timeout QCOMPARE(_multiSpy->checkOnlySignalByMask(errorMessageSignalMask), true); + _multiSpy->clearAllSignals(); + + // Setup for a bad sequence number in the ack. This should cause an error; + _mockFileServer.setErrorMode(MockMavlinkFileServer::errModeBadSequence); + QVERIFY(_fileManager->_sendCmdTestAck()); + QCOMPARE(_multiSpy->checkOnlySignalByMask(errorMessageSignalMask), true); + _multiSpy->clearAllSignals(); } void QGCUASFileManagerUnitTest::_noAckTest(void) @@ -140,6 +150,18 @@ void QGCUASFileManagerUnitTest::_listTest(void) Q_ASSERT(_multiSpy); Q_ASSERT(_multiSpy->checkNoSignals() == true); + // QGCUASFileManager::listDirectory signalling as follows: + // Emits the resetStatusMessages signal + // Emits a statusMessage signal for each list entry + // Emits an errorMessage signal if: + // It gets a Nak back + // Sequence number is incorrrect on any response + // CRC is incorrect on any responses + // List entry is formatted incorrectly + // It is possible to get a number of good statusMessage signals, followed by an errorMessage signal + // Emits listComplete after it receives the final list entry + // If an errorMessage signal is signalled no listComplete is signalled + // Send a bogus path // We should get a single resetStatusMessages signal // We should get a single errorMessage signal @@ -182,14 +204,9 @@ void QGCUASFileManagerUnitTest::_listTest(void) _mockFileServer.setErrorMode(MockMavlinkFileServer::errModeNone); } - // Send a list command at the root of the directory tree which should succeed - // We should get back a single resetStatusMessages signal - // We should not get back an errorMessage signal - // We should get back a statusMessage signal for each entry - // The returned list should match our inputs - + // Send a list command at the root of the directory tree which should succeed _fileManager->listDirectory("/"); - QCOMPARE(_multiSpy->checkSignalByMask(resetStatusMessagesSignalMask), true); + QCOMPARE(_multiSpy->checkSignalByMask(resetStatusMessagesSignalMask | listCompleteSignalMask), true); QCOMPARE(_multiSpy->checkNoSignalByMask(errorMessageSignalMask), true); QCOMPARE(_multiSpy->getSpyByIndex(statusMessageSignalIndex)->count(), fileList.count()); QVERIFY(_fileListReceived == fileList); @@ -214,17 +231,39 @@ void QGCUASFileManagerUnitTest::_validateFileContents(const QString& filePath, u } } -void QGCUASFileManagerUnitTest::_openTest(void) +void QGCUASFileManagerUnitTest::_downloadTest(void) { Q_ASSERT(_fileManager); Q_ASSERT(_multiSpy); Q_ASSERT(_multiSpy->checkNoSignals() == true); + // QGCUASFileManager::downloadPath works as follows: + // Emits the resetStatusMessages signal + // Sends an Open Command to the server + // Expects an Ack Response back from the server with the correct sequence numner + // Emits an errorMessage signal if it gets a Nak back + // Emits an openFileLength signal with the file length if it gets back a good Ack + // Sends subsequent Read commands to the server until it gets the full file contents back + // Sends Terminate command to server when download is complete to close Open command + // Mock file server will signal terminateCommandReceived when it gets a Terminate command + // Sends statusMessage signal to indicate the download is complete + // Emits an errorMessage signal if sequence number is incorrrect on any response + // Emits an errorMessage signal if CRC is incorrect on any responses + + // Expected signals if the Open command fails for any reason + quint16 signalMaskOpenFailure = resetStatusMessagesSignalMask | errorMessageSignalMask; + + // Expected signals if the Read command fails for any reason + quint16 signalMaskReadFailure = resetStatusMessagesSignalMask | openFileLengthSignalMask | errorMessageSignalMask; + + // Expected signals if the downloadPath command succeeds + quint16 signalMaskDownloadSuccess = resetStatusMessagesSignalMask | openFileLengthSignalMask | statusMessageSignalMask; + // Send a bogus path // We should get a single resetStatusMessages signal // We should get a single errorMessage signal _fileManager->downloadPath("bogus", QDir::temp()); - QCOMPARE(_multiSpy->checkOnlySignalByMask(errorMessageSignalMask | resetStatusMessagesSignalMask), true); + QCOMPARE(_multiSpy->checkOnlySignalByMask(signalMaskOpenFailure), true); _multiSpy->clearAllSignals(); // Clean previous downloads @@ -235,50 +274,49 @@ void QGCUASFileManagerUnitTest::_openTest(void) } } - // We setup a spy on the Terminate command signal so that we can determine that a Terminate command was - // correctly sent after the Open/Read commands complete. + // We setup a spy on the Terminate command signal of the mock file server so that we can determine that a + // Terminate command was correctly sent after the Open/Read commands complete. QSignalSpy terminateSpy(&_mockFileServer, SIGNAL(terminateCommandReceived())); // Run through the set of file test cases for (size_t i=0; idownloadPath(MockMavlinkFileServer::rgFileTestCases[i].filename, QDir::temp()); + _fileManager->downloadPath(testCase->filename, QDir::temp()); QTest::qWait(_ackTimerTimeoutMsecs); // Let the file manager timeout if (errMode == MockMavlinkFileServer::errModeNoSecondResponse || errMode == MockMavlinkFileServer::errModeNakSecondResponse) { // For simulated server errors on subsequent Acks, the first Ack will go through. We must handle things differently depending // on whether the downloaded file requires multiple packets to complete the download. - if (MockMavlinkFileServer::rgFileTestCases[i].fMultiPacketResponse) { + if (testCase->fMultiPacketResponse) { // The downloaded file requires multiple Acks to complete. Hence second Read should have failed. - QCOMPARE(_multiSpy->checkOnlySignalByMask(errorMessageSignalMask | resetStatusMessagesSignalMask), true); + QCOMPARE(_multiSpy->checkOnlySignalByMask(signalMaskReadFailure), true); // Open command succeeded, so we should get a Terminate for the open QCOMPARE(terminateSpy.count(), 1); } else { // The downloaded file fits within a single Ack response, hence there is no second Read issued. // This should result in a successful download. - - // We should get a single resetStatusMessages signal - // We should get a single statusMessage signal, which indicates download completion - QCOMPARE(_multiSpy->checkOnlySignalByMask(statusMessageSignalMask | resetStatusMessagesSignalMask), true); + QCOMPARE(_multiSpy->checkOnlySignalByMask(signalMaskDownloadSuccess), true); // We should get a single Terminate command to close the Open session QCOMPARE(terminateSpy.count(), 1); // Validate file contents - QString filePath = QDir::temp().absoluteFilePath(MockMavlinkFileServer::rgFileTestCases[i].filename); - _validateFileContents(filePath, MockMavlinkFileServer::rgFileTestCases[i].length); + QString filePath = QDir::temp().absoluteFilePath(testCase->filename); + _validateFileContents(filePath, testCase->length); } } else { // For all the other simulated server errors the Open command should have failed. Since the Open failed // there is no session to terminate, hence no Terminate in this case. - QCOMPARE(_multiSpy->checkOnlySignalByMask(errorMessageSignalMask | resetStatusMessagesSignalMask), true); + QCOMPARE(_multiSpy->checkOnlySignalByMask(signalMaskOpenFailure), true); QCOMPARE(terminateSpy.count(), 0); } @@ -289,11 +327,14 @@ void QGCUASFileManagerUnitTest::_openTest(void) } // Run what should be a successful file download test case. No servers errors are being simulated. - _fileManager->downloadPath(MockMavlinkFileServer::rgFileTestCases[i].filename, QDir::temp()); + _fileManager->downloadPath(testCase->filename, QDir::temp()); + + // This should be a succesful download + QCOMPARE(_multiSpy->checkOnlySignalByMask(signalMaskDownloadSuccess), true); + + // Make sure the file length coming back through the openFileLength signal is correct + QVERIFY(_multiSpy->getSpyByIndex(openFileLengthSignalIndex)->takeFirst().at(0).toInt() == testCase->length); - // We should get a single resetStatusMessages signal - // We should get a single statusMessage signal, which indicated download completion - QCOMPARE(_multiSpy->checkOnlySignalByMask(statusMessageSignalMask | resetStatusMessagesSignalMask), true); _multiSpy->clearAllSignals(); // We should get a single Terminate command to close the session diff --git a/src/qgcunittest/QGCUASFileManagerTest.h b/src/qgcunittest/QGCUASFileManagerTest.h index 25ed905..bc5cb9c 100644 --- a/src/qgcunittest/QGCUASFileManagerTest.h +++ b/src/qgcunittest/QGCUASFileManagerTest.h @@ -56,7 +56,7 @@ private slots: void _noAckTest(void); void _resetTest(void); void _listTest(void); - void _openTest(void); + void _downloadTest(void); // Connected to QGCUASFileManager statusMessage signal void statusMessage(const QString&); @@ -68,7 +68,8 @@ private: statusMessageSignalIndex = 0, errorMessageSignalIndex, resetStatusMessagesSignalIndex, - + listCompleteSignalIndex, + openFileLengthSignalIndex, maxSignalIndex }; @@ -76,6 +77,8 @@ private: statusMessageSignalMask = 1 << statusMessageSignalIndex, errorMessageSignalMask = 1 << errorMessageSignalIndex, resetStatusMessagesSignalMask = 1 << resetStatusMessagesSignalIndex, + listCompleteSignalMask = 1 << listCompleteSignalIndex, + openFileLengthSignalMask = 1 << openFileLengthSignalIndex, }; MockUAS _mockUAS; diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc index 1b37015..6ef7d19 100644 --- a/src/uas/QGCUASFileManager.cc +++ b/src/uas/QGCUASFileManager.cc @@ -70,7 +70,7 @@ QGCUASFileManager::QGCUASFileManager(QObject* parent, UASInterface* uas) : QObject(parent), _currentOperation(kCOIdle), _mav(uas), - _encdata_seq(0), + _lastOutgoingSeqNumber(0), _activeSession(0) { bool connected = connect(&_ackTimer, SIGNAL(timeout()), this, SLOT(_ackTimeout())); @@ -104,12 +104,16 @@ void QGCUASFileManager::_openAckResponse(Request* openAck) { _currentOperation = kCORead; _activeSession = openAck->hdr.session; + + // File length comes back in data + Q_ASSERT(openAck->hdr.size == sizeof(uint32_t)); + uint32_t fileLength = *((uint32_t*)openAck->data); + emit openFileLength(fileLength); _readOffset = 0; // Start reading at beginning of file _readFileAccumulator.clear(); // Start with an empty file Request request; - request.hdr.magic = 'f'; request.hdr.session = _activeSession; request.hdr.opcode = kCmdRead; request.hdr.offset = _readOffset; @@ -173,7 +177,6 @@ void QGCUASFileManager::_readAckResponse(Request* readAck) _readOffset += readAck->hdr.size; Request request; - request.hdr.magic = 'f'; request.hdr.session = _activeSession; request.hdr.opcode = kCmdRead; request.hdr.offset = _readOffset; @@ -251,23 +254,42 @@ void QGCUASFileManager::receiveMessage(LinkInterface* link, mavlink_message_t me } // XXX: hack to prevent files from videostream to interfere - if (message.compid != MAV_COMP_ID_IMU) { + // FIXME: magic number + if (message.compid != 50) { return; } - _clearAckTimeout(); - mavlink_encapsulated_data_t data; mavlink_msg_encapsulated_data_decode(&message, &data); Request* request = (Request*)&data.data[0]; + + if (request->hdr.magic != kProtocolMagic) { + return; + } + _clearAckTimeout(); + + uint16_t incomingSeqNumber = data.seqnr; + // Make sure we have a good CRC - if (request->hdr.crc32 != crc32(request)) { + quint32 expectedCRC = crc32(request); + quint32 receivedCRC = request->hdr.crc32; + if (receivedCRC != expectedCRC) { _currentOperation = kCOIdle; - _emitErrorMessage(tr("Bad CRC on received message")); + _emitErrorMessage(tr("Bad CRC on received message: expected(%1) received(%2)").arg(expectedCRC).arg(receivedCRC)); return; } - + + // Make sure we have a good sequence number + uint16_t expectedSeqNumber = _lastOutgoingSeqNumber + 1; + if (incomingSeqNumber != expectedSeqNumber) { + _currentOperation = kCOIdle; + _emitErrorMessage(tr("Bad sequence number on received message: expected(%1) received(%2)").arg(expectedSeqNumber).arg(incomingSeqNumber)); + return; + } + + // Move past the incoming sequence number for next request + _lastOutgoingSeqNumber = incomingSeqNumber; if (request->hdr.opcode == kRspAck) { @@ -474,8 +496,6 @@ void QGCUASFileManager::_setupAckTimeout(void) /// @brief Clears the ack timeout timer void QGCUASFileManager::_clearAckTimeout(void) { - Q_ASSERT(_ackTimer.isActive()); - _ackTimer.stop(); } @@ -502,7 +522,6 @@ void QGCUASFileManager::_ackTimeout(void) void QGCUASFileManager::_sendTerminateCommand(void) { Request request; - request.hdr.magic = 'f'; request.hdr.session = _activeSession; request.hdr.opcode = kCmdTerminate; _sendRequest(&request); @@ -526,9 +545,13 @@ void QGCUASFileManager::_sendRequest(Request* request) mavlink_message_t message; _setupAckTimeout(); + + _lastOutgoingSeqNumber++; + request->hdr.magic = kProtocolMagic; request->hdr.crc32 = crc32(request); // FIXME: Send correct system id instead of harcoded 250 - mavlink_msg_encapsulated_data_pack(250, 0, &message, _encdata_seq, (uint8_t*)request); + // 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); _mav->sendMessage(message); } diff --git a/src/uas/QGCUASFileManager.h b/src/uas/QGCUASFileManager.h index 817cefb..e63dcc6 100644 --- a/src/uas/QGCUASFileManager.h +++ b/src/uas/QGCUASFileManager.h @@ -49,6 +49,7 @@ signals: void resetStatusMessages(); void errorMessage(const QString& msg); void listComplete(void); + void openFileLength(unsigned int length); public slots: void receiveMessage(LinkInterface* link, mavlink_message_t message); @@ -56,6 +57,7 @@ public slots: void downloadPath(const QString& from, const QDir& downloadDir); protected: + static const uint8_t kProtocolMagic = 'f'; struct RequestHeader { uint8_t magic; ///> Magic byte 'f' to idenitfy FTP protocol @@ -148,7 +150,8 @@ protected: QTimer _ackTimer; ///> Used to signal a timeout waiting for an ack UASInterface* _mav; - quint16 _encdata_seq; + + 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