From 09c26a3087f4135946f1c53e8870510b359701eb Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Mon, 29 Jun 2020 11:07:51 -0700 Subject: [PATCH] Component Information FTP download support --- qgroundcontrol.pro | 14 +- src/QGCTemporaryFile.cc | 35 +- src/QGCTemporaryFile.h | 16 +- src/Vehicle/ComponentInformationManager.cc | 111 ++- src/Vehicle/ComponentInformationManager.h | 42 +- src/Vehicle/FTPManager.cc | 787 +++++++++++++++ src/Vehicle/FTPManager.h | 133 +++ src/Vehicle/FTPManagerTest.cc | 49 + src/Vehicle/FTPManagerTest.h | 29 + src/Vehicle/InitialConnectTest.cc | 72 ++ src/Vehicle/InitialConnectTest.h | 42 + src/Vehicle/MAVLinkFTPManager.cc | 887 ----------------- src/Vehicle/MAVLinkFTPManager.h | 249 ----- src/Vehicle/StateMachine.h | 6 +- src/Vehicle/Vehicle.cc | 9 +- src/Vehicle/Vehicle.h | 6 +- src/comm/MockLink.cc | 6 +- src/comm/MockLink.h | 6 +- .../{MockLinkFileServer.cc => MockLinkFTP.cc} | 271 +++--- .../{MockLinkFileServer.h => MockLinkFTP.h} | 57 +- src/comm/QGCMAVLink.cc | 74 ++ src/comm/QGCMAVLink.h | 80 ++ src/qgcunittest/UnitTestList.cc | 4 + src/uas/FileManager.cc | 895 ------------------ src/uas/FileManager.h | 259 ----- src/uas/UAS.cc | 8 - src/uas/UAS.h | 16 - src/uas/UASInterface.h | 8 - 28 files changed, 1609 insertions(+), 2562 deletions(-) create mode 100644 src/Vehicle/FTPManager.cc create mode 100644 src/Vehicle/FTPManager.h create mode 100644 src/Vehicle/FTPManagerTest.cc create mode 100644 src/Vehicle/FTPManagerTest.h create mode 100644 src/Vehicle/InitialConnectTest.cc create mode 100644 src/Vehicle/InitialConnectTest.h delete mode 100644 src/Vehicle/MAVLinkFTPManager.cc delete mode 100644 src/Vehicle/MAVLinkFTPManager.h rename src/comm/{MockLinkFileServer.cc => MockLinkFTP.cc} (51%) rename src/comm/{MockLinkFileServer.h => MockLinkFTP.h} (56%) delete mode 100644 src/uas/FileManager.cc delete mode 100644 src/uas/FileManager.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 2b22c5d24..3552bb777 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -498,6 +498,8 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/qgcunittest/TCPLinkTest.h \ src/qgcunittest/TCPLoopBackServer.h \ src/qgcunittest/UnitTest.h \ + src/Vehicle/FTPManagerTest.h \ + src/Vehicle/InitialConnectTest.h \ src/Vehicle/RequestMessageTest.h \ src/Vehicle/SendMavCommandWithHandlerTest.h \ src/Vehicle/SendMavCommandWithSignallingTest.h \ @@ -543,6 +545,8 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/qgcunittest/TCPLoopBackServer.cc \ src/qgcunittest/UnitTest.cc \ src/qgcunittest/UnitTestList.cc \ + src/Vehicle/FTPManagerTest.cc \ + src/Vehicle/InitialConnectTest.cc \ src/Vehicle/RequestMessageTest.cc \ src/Vehicle/SendMavCommandWithHandlerTest.cc \ src/Vehicle/SendMavCommandWithSignallingTest.cc \ @@ -679,9 +683,9 @@ HEADERS += \ src/Terrain/TerrainQuery.h \ src/TerrainTile.h \ src/Vehicle/ComponentInformationManager.h \ + src/Vehicle/FTPManager.h \ src/Vehicle/GPSRTKFactGroup.h \ src/Vehicle/InitialConnectStateMachine.h \ - src/Vehicle/MAVLinkFTPManager.h \ src/Vehicle/MAVLinkLogManager.h \ src/Vehicle/MultiVehicleManager.h \ src/Vehicle/StateMachine.h \ @@ -705,7 +709,6 @@ HEADERS += \ src/uas/UASMessageHandler.h \ src/AnalyzeView/GeoTagController.h \ src/AnalyzeView/ExifParser.h \ - src/uas/FileManager.h \ contains (DEFINES, QGC_ENABLE_PAIRING) { HEADERS += \ @@ -720,7 +723,7 @@ HEADERS += \ DebugBuild { HEADERS += \ src/comm/MockLink.h \ - src/comm/MockLinkFileServer.h \ + src/comm/MockLinkFTP.h \ src/comm/MockLinkMissionItemHandler.h \ } @@ -895,9 +898,9 @@ SOURCES += \ src/Terrain/TerrainQuery.cc \ src/TerrainTile.cc\ src/Vehicle/ComponentInformationManager.cc \ + src/Vehicle/FTPManager.cc \ src/Vehicle/GPSRTKFactGroup.cc \ src/Vehicle/InitialConnectStateMachine.cc \ - src/Vehicle/MAVLinkFTPManager.cc \ src/Vehicle/MAVLinkLogManager.cc \ src/Vehicle/MultiVehicleManager.cc \ src/Vehicle/StateMachine.cc \ @@ -921,7 +924,6 @@ SOURCES += \ src/uas/UASMessageHandler.cc \ src/AnalyzeView/GeoTagController.cc \ src/AnalyzeView/ExifParser.cc \ - src/uas/FileManager.cc \ contains (DEFINES, QGC_ENABLE_PAIRING) { SOURCES += \ @@ -931,7 +933,7 @@ contains (DEFINES, QGC_ENABLE_PAIRING) { DebugBuild { SOURCES += \ src/comm/MockLink.cc \ - src/comm/MockLinkFileServer.cc \ + src/comm/MockLinkFTP.cc \ src/comm/MockLinkMissionItemHandler.cc \ } diff --git a/src/QGCTemporaryFile.cc b/src/QGCTemporaryFile.cc index 400801fc8..9dd2968e7 100644 --- a/src/QGCTemporaryFile.cc +++ b/src/QGCTemporaryFile.cc @@ -28,30 +28,43 @@ QGCTemporaryFile::QGCTemporaryFile(const QString& fileTemplate, QObject* parent) } +QGCTemporaryFile::~QGCTemporaryFile() +{ + if (_autoRemove) { + remove(); + } +} + bool QGCTemporaryFile::open(QFile::OpenMode openMode) { - QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation)); + setFileName(_newTempFileFullyQualifiedName(_template)); + return QFile::open(openMode); +} + +QString QGCTemporaryFile::_newTempFileFullyQualifiedName(const QString& fileTemplate) +{ + QString nameTemplate = fileTemplate; + QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation)); + // Generate unique, non-existing filename - + static const char rgDigits[] = "0123456789"; - + QString tempFilename; - + do { QString uniqueStr; for (int i=0; i<6; i++) { uniqueStr += rgDigits[QRandomGenerator::global()->generate() % 10]; } - - if (_template.contains("XXXXXX")) { - tempFilename = _template.replace("XXXXXX", uniqueStr, Qt::CaseSensitive); + + if (fileTemplate.contains("XXXXXX")) { + tempFilename = nameTemplate.replace("XXXXXX", uniqueStr, Qt::CaseSensitive); } else { - tempFilename = _template + uniqueStr; + tempFilename = nameTemplate + uniqueStr; } } while (tempDir.exists(tempFilename)); - setFileName(tempDir.filePath(tempFilename)); - - return QFile::open(openMode); + return tempDir.filePath(tempFilename); } diff --git a/src/QGCTemporaryFile.h b/src/QGCTemporaryFile.h index df1b8739c..f4b1ccaf3 100644 --- a/src/QGCTemporaryFile.h +++ b/src/QGCTemporaryFile.h @@ -7,9 +7,7 @@ * ****************************************************************************/ - -#ifndef QGCTemporaryFile_H -#define QGCTemporaryFile_H +#pragma once #include @@ -30,13 +28,15 @@ public: // directory path, only file name. QGCTemporaryFile(const QString& fileTemplate, QObject* parent = nullptr); - /// @brief Opens the file in ReadWrite mode. - /// @returns false - open failed + ~QGCTemporaryFile(); + bool open(OpenMode openMode = ReadWrite); + + void setAutoRemove(bool autoRemove) { _autoRemove = autoRemove; } private: + static QString _newTempFileFullyQualifiedName(const QString& fileTemplate); + QString _template; + bool _autoRemove = false; }; - - -#endif diff --git a/src/Vehicle/ComponentInformationManager.cc b/src/Vehicle/ComponentInformationManager.cc index 7993c4304..f940cbe00 100644 --- a/src/Vehicle/ComponentInformationManager.cc +++ b/src/Vehicle/ComponentInformationManager.cc @@ -9,6 +9,9 @@ #include "ComponentInformationManager.h" #include "Vehicle.h" +#include "FTPManager.h" + +#include QGC_LOGGING_CATEGORY(ComponentInformationManagerLog, "ComponentInformationManagerLog") @@ -22,6 +25,8 @@ int ComponentInformationManager::_cStates = sizeof(ComponentInformationManager:: RequestMetaDataTypeStateMachine::StateFn RequestMetaDataTypeStateMachine::_rgStates[]= { RequestMetaDataTypeStateMachine::_stateRequestCompInfo, + RequestMetaDataTypeStateMachine::_stateRequestMetaDataJson, + RequestMetaDataTypeStateMachine::_stateRequestTranslationJson, }; int RequestMetaDataTypeStateMachine::_cStates = sizeof(RequestMetaDataTypeStateMachine::_rgStates) / sizeof(RequestMetaDataTypeStateMachine::_rgStates[0]); @@ -43,33 +48,6 @@ const ComponentInformationManager::StateFn* ComponentInformationManager::rgState return &_rgStates[0]; } -void ComponentInformationManager::_componentInformationReceived(const mavlink_message_t& message) -{ - mavlink_component_information_t componentInformation; - mavlink_msg_component_information_decode(&message, &componentInformation); - - ComponentInformation_t* pCompInfo = nullptr; - switch (componentInformation.metadata_type) { - case COMP_METADATA_TYPE_VERSION: - qCDebug(ComponentInformationManagerLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION received"; - _versionCompInfoAvailable = true; - pCompInfo = &_versionCompInfo; - break; - case COMP_METADATA_TYPE_PARAMETER: - qCDebug(ComponentInformationManagerLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER received"; - _paramCompInfoAvailable = true; - pCompInfo = &_parameterCompInfo; - break; - } - - if (pCompInfo) { - pCompInfo->metadataUID = componentInformation.metadata_uid; - pCompInfo->metadataURI = componentInformation.metadata_uri; - pCompInfo->translationUID = componentInformation.translation_uid; - pCompInfo->translationURI = componentInformation.translation_uri; - } -} - void ComponentInformationManager::requestAllComponentInformation(RequestAllCompleteFn requestAllCompletFn, void * requestAllCompleteFnData) { _requestAllCompleteFn = requestAllCompletFn; @@ -110,8 +88,9 @@ RequestMetaDataTypeStateMachine::RequestMetaDataTypeStateMachine(ComponentInform void RequestMetaDataTypeStateMachine::request(COMP_METADATA_TYPE type) { - _type = type; - _stateIndex = -1; + _compInfoAvailable = false; + _type = type; + _stateIndex = -1; start(); } @@ -136,12 +115,24 @@ QString RequestMetaDataTypeStateMachine::typeToString(void) return _type == COMP_METADATA_TYPE_VERSION ? "COMP_METADATA_TYPE_VERSION" : "COMP_METADATA_TYPE_PARAM"; } +void RequestMetaDataTypeStateMachine::handleComponentInformation(const mavlink_message_t& message) +{ + mavlink_component_information_t componentInformation; + mavlink_msg_component_information_decode(&message, &componentInformation); + + _compInfo.metadataUID = componentInformation.metadata_uid; + _compInfo.metadataURI = componentInformation.metadata_uri; + _compInfo.translationUID = componentInformation.translation_uid; + _compInfo.translationURI = componentInformation.translation_uri; + _compInfoAvailable = true; +} + static void _requestMessageResultHandler(void* resultHandlerData, MAV_RESULT result, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t &message) { RequestMetaDataTypeStateMachine* requestMachine = static_cast(resultHandlerData); if (result == MAV_RESULT_ACCEPTED) { - requestMachine->compMgr()->_componentInformationReceived(message); + requestMachine->handleComponentInformation(message); } else { switch (failureCode) { case Vehicle::RequestMessageFailureCommandError: @@ -180,3 +171,63 @@ void RequestMetaDataTypeStateMachine::_stateRequestCompInfo(StateMachine* stateM } } +void RequestMetaDataTypeStateMachine::_downloadComplete(const QString& file, const QString& errorMsg) +{ + qDebug() << "RequestMetaDataTypeStateMachine::_downloadComplete" << file << errorMsg; + advance(); +} + +void RequestMetaDataTypeStateMachine::_stateRequestMetaDataJson(StateMachine* stateMachine) +{ + RequestMetaDataTypeStateMachine* requestMachine = static_cast(stateMachine); + Vehicle* vehicle = requestMachine->_compMgr->vehicle(); + FTPManager* ftpManager = vehicle->ftpManager(); + + if (requestMachine->_compInfoAvailable) { + ComponentInformation_t& compInfo = requestMachine->_compInfo; + + qCDebug(ComponentInformationManagerLog) << "Downloading metadata json" << compInfo.translationURI; + if (_uriIsFTP(compInfo.metadataURI)) { + connect(ftpManager, &FTPManager::downloadComplete, requestMachine, &RequestMetaDataTypeStateMachine::_downloadComplete); + ftpManager->download(compInfo.metadataURI, QStandardPaths::writableLocation(QStandardPaths::TempLocation)); + } else { + // FIXME: NYI + qCDebug(ComponentInformationManagerLog) << "Skipping metadata json download. http download NYI"; + } + } else { + qCDebug(ComponentInformationManagerLog) << "Skipping metadata json download. Component informatiom not available"; + requestMachine->advance(); + } +} + +void RequestMetaDataTypeStateMachine::_stateRequestTranslationJson(StateMachine* stateMachine) +{ + RequestMetaDataTypeStateMachine* requestMachine = static_cast(stateMachine); + Vehicle* vehicle = requestMachine->_compMgr->vehicle(); + FTPManager* ftpManager = vehicle->ftpManager(); + + if (requestMachine->_compInfoAvailable) { + ComponentInformation_t& compInfo = requestMachine->_compInfo; + if (compInfo.translationURI.isEmpty()) { + qCDebug(ComponentInformationManagerLog) << "Skipping translation json download. No translation json specified"; + requestMachine->advance(); + } else { + qCDebug(ComponentInformationManagerLog) << "Downloading translation json" << compInfo.translationURI; + if (_uriIsFTP(compInfo.translationURI)) { + connect(ftpManager, &FTPManager::downloadComplete, requestMachine, &RequestMetaDataTypeStateMachine::_downloadComplete); + ftpManager->download(compInfo.metadataURI, QStandardPaths::writableLocation(QStandardPaths::TempLocation)); + } else { + // FIXME: NYI + qCDebug(ComponentInformationManagerLog) << "Skipping translation json download. http download NYI"; + } + } + } else { + qCDebug(ComponentInformationManagerLog) << "Skipping translation json download. Component informatiom not available"; + requestMachine->advance(); + } +} + +bool RequestMetaDataTypeStateMachine::_uriIsFTP(const QString& uri) +{ + return uri.startsWith("mavlinkftp", Qt::CaseInsensitive); +} diff --git a/src/Vehicle/ComponentInformationManager.h b/src/Vehicle/ComponentInformationManager.h index 6dae1d475..356c1bbc4 100644 --- a/src/Vehicle/ComponentInformationManager.h +++ b/src/Vehicle/ComponentInformationManager.h @@ -18,27 +18,44 @@ Q_DECLARE_LOGGING_CATEGORY(ComponentInformationManagerLog) class Vehicle; class ComponentInformationManager; +typedef struct { + uint32_t metadataUID; + QString metadataURI; + uint32_t translationUID; + QString translationURI; +} ComponentInformation_t; + class RequestMetaDataTypeStateMachine : public StateMachine { + Q_OBJECT + public: RequestMetaDataTypeStateMachine(ComponentInformationManager* compMgr); - void request(COMP_METADATA_TYPE type); - ComponentInformationManager* compMgr(void) { return _compMgr; } - QString typeToString(void); + void request (COMP_METADATA_TYPE type); + QString typeToString (void); + ComponentInformationManager* compMgr (void) { return _compMgr; } + void handleComponentInformation (const mavlink_message_t& message); // Overrides from StateMachine int stateCount (void) const final; const StateFn* rgStates (void) const final; void statesCompleted (void) const final; +private slots: + void _downloadComplete(const QString& file, const QString& errorMsg); + private: static void _stateRequestCompInfo (StateMachine* stateMachine); static void _stateRequestMetaDataJson (StateMachine* stateMachine); static void _stateRequestTranslationJson (StateMachine* stateMachine); + static bool _uriIsFTP (const QString& uri); + ComponentInformationManager* _compMgr; - COMP_METADATA_TYPE _type = COMP_METADATA_TYPE_VERSION; + COMP_METADATA_TYPE _type = COMP_METADATA_TYPE_VERSION; + bool _compInfoAvailable = false; + ComponentInformation_t _compInfo; static StateFn _rgStates[]; static int _cStates; @@ -46,30 +63,23 @@ private: class ComponentInformationManager : public StateMachine { + Q_OBJECT + public: ComponentInformationManager(Vehicle* vehicle); - typedef struct { - uint32_t metadataUID; - QString metadataURI; - uint32_t translationUID; - QString translationURI; - } ComponentInformation_t; - typedef void (*RequestAllCompleteFn)(void* requestAllCompleteFnData); void requestAllComponentInformation (RequestAllCompleteFn requestAllCompletFn, void * requestAllCompleteFnData); Vehicle* vehicle (void) { return _vehicle; } - // These methods should only be called by RequestMetaDataTypeStateMachine - void _componentInformationReceived(const mavlink_message_t& message); - void _stateRequestCompInfoComplete(void); - // Overrides from StateMachine int stateCount (void) const final; const StateFn* rgStates (void) const final; private: + void _stateRequestCompInfoComplete(void); + static void _stateRequestCompInfoVersion (StateMachine* stateMachine); static void _stateRequestCompInfoParam (StateMachine* stateMachine); static void _stateRequestAllCompInfoComplete (StateMachine* stateMachine); @@ -86,4 +96,6 @@ private: static StateFn _rgStates[]; static int _cStates; + + friend class RequestMetaDataTypeStateMachine; }; diff --git a/src/Vehicle/FTPManager.cc b/src/Vehicle/FTPManager.cc new file mode 100644 index 000000000..d0db05915 --- /dev/null +++ b/src/Vehicle/FTPManager.cc @@ -0,0 +1,787 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +#include "FTPManager.h" +#include "QGC.h" +#include "MAVLinkProtocol.h" +#include "Vehicle.h" +#include "QGCApplication.h" + +#include +#include +#include + +QGC_LOGGING_CATEGORY(FTPManagerLog, "FTPManagerLog") + +FTPManager::FTPManager(Vehicle* vehicle) + : QObject (vehicle) + , _vehicle (vehicle) +{ + connect(&_ackTimer, &QTimer::timeout, this, &FTPManager::_ackTimeout); + + _lastOutgoingRequest.hdr.seqNumber = 0; + + // Make sure we don't have bad structure packing + Q_ASSERT(sizeof(MavlinkFTP::RequestHeader) == 12); +} + +void FTPManager::_handlOpenFileROAck(MavlinkFTP::Request* ack) +{ + qCDebug(FTPManagerLog) << QString("_openAckResponse: _waitState(%1) _openFileType(%2) _readFileLength(%3)").arg(MavlinkFTP::opCodeToString(_waitState)).arg(MavlinkFTP::opCodeToString(_openFileType)).arg(ack->openFileLength); + + if (_waitState != MavlinkFTP::kCmdOpenFileRO && _waitState != MavlinkFTP::kCmdBurstReadFile) { + qCDebug(FTPManagerLog) << "Received OpenFileRO Ack while not waiting for it. _waitState" << MavlinkFTP::opCodeToString(_waitState); + return; + } + + if (ack->hdr.size != sizeof(uint32_t)) { + _downloadComplete(tr("Download failed: Invalid response to OpenFileRO command.")); + return; + } + + _waitState = _openFileType; + _activeSession = ack->hdr.session; + _downloadFileSize = ack->openFileLength; + _requestedDownloadOffset = 0; + _readFileAccumulator.clear(); + + MavlinkFTP::Request request; + request.hdr.session = _activeSession; + request.hdr.opcode = _waitState; + request.hdr.offset = _requestedDownloadOffset; + request.hdr.size = sizeof(request.data); + _sendRequestExpectAck(&request); +} + +/// request the next missing part of a (partially) downloaded file +void FTPManager::_requestMissingData() +{ +#if 0 + if (_missingData.empty()) { + _downloadingMissingParts = false; + _missingDownloadedBytes = 0; + // there might be more data missing at the end + if ((uint32_t)_readFileAccumulator.length() != _downloadFileSize) { + _downloadOffset = _readFileAccumulator.length(); + qCDebug(FTPManagerLog) << QString("_requestMissingData: missing parts done, downloadOffset(%1) downloadFileSize(%2)") + .arg(_downloadOffset).arg(_downloadFileSize); + } else { + _closeDownloadSession(true); + return; + } + } else { + qCDebug(FTPManagerLog) << QString("_requestMissingData: offset(%1) size(%2)").arg(_missingData.head().offset).arg(_missingData.head().size); + + _downloadOffset = _missingData.head().offset; + } + + MavlinkFTP::Request request; + _currentOperation = kCORead; + request.hdr.session = _activeSession; + request.hdr.opcode = MavlinkFTP::kCmdReadFile; + request.hdr.offset = _downloadOffset; + request.hdr.size = sizeof(request.data); + + _sendRequestExpectAck(&request); +#endif +} + +/// Closes out a download session by writing the file and doing cleanup. +/// @param success true: successful download completion, false: error during download +void FTPManager::_downloadComplete(const QString& errorMsg) +{ + qCDebug(FTPManagerLog) << QString("_downloadComplete: errorMsg(%1)").arg(errorMsg); + + QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename); + QString error = errorMsg; + + _waitState = MavlinkFTP::kCmdNone; + + if (error.isEmpty()) { + QFile file(downloadFilePath); + + if (file.open(QIODevice::WriteOnly | QIODevice::Truncate)) { + qint64 bytesWritten = file.write((const char *)_readFileAccumulator, _readFileAccumulator.length()); + if (bytesWritten != _readFileAccumulator.length()) { + error = tr("Download failed: Unable to write data to local file '%1'.").arg(downloadFilePath); + } + } else { + error = tr("Download failed: Unable to open local file '%1' for writing. Error: '%2'.").arg(downloadFilePath).arg(file.errorString()); + } + file.close(); + } + + _readFileAccumulator.clear(); + _sendResetCommand(); // Close the open session + emit downloadComplete(downloadFilePath, error); +} + +/// Closes out an upload session doing cleanup. +/// @param success true: successful upload completion, false: error during download +void FTPManager::_uploadComplete(const QString& errorMsg) +{ + qCDebug(FTPManagerLog) << QString("_uploadComplete: errorMsg(%1)").arg(errorMsg); + + _waitState = MavlinkFTP::kCmdNone; + _writeFileSize = 0; + _writeFileAccumulator.clear(); + _sendResetCommand(); + emit uploadComplete(errorMsg); +} + +void FTPManager::_handleReadFileAck(MavlinkFTP::Request* ack, bool burstReadFile) +{ + if (ack->hdr.session != _activeSession) { + return; + } + + qCDebug(FTPManagerLog) << QString("_handleReadFileAck: burstReadFile(%1) offset(%2) size(%3) burstComplete(%4)").arg(burstReadFile).arg(ack->hdr.offset).arg(ack->hdr.size).arg(ack->hdr.burstComplete); + + if (ack->hdr.offset != _requestedDownloadOffset) { + // FIXME: NYI deal with missing packets + _downloadComplete(tr("Download failed: Received incorrect offset: received:expected %1/%2").arg(ack->hdr.offset).arg(_requestedDownloadOffset)); + return; + } + + _readFileAccumulator.append((const char*)ack->data, ack->hdr.size); + + _requestedDownloadOffset += ack->hdr.size; + + if (_downloadFileSize != 0) { + emit commandProgress(100 * ((float)(_readFileAccumulator.length()) / (float)_downloadFileSize)); + } + + if (!burstReadFile || ack->hdr.burstComplete) { + MavlinkFTP::Request request; + request.hdr.session = _activeSession; + request.hdr.opcode = _waitState; + request.hdr.offset = _requestedDownloadOffset; + request.hdr.size = 0; + _sendRequestExpectAck(&request); + } else if (burstReadFile) { + // Burst read, next ack should come without having to request it + _setupAckTimeout(); + } +} + +/// @brief Respond to the Ack associated with the List command. +void FTPManager::_listAckResponse(MavlinkFTP::Request* listAck) +{ +#if 0 + if (listAck->hdr.offset != _listOffset) { + // this is a real error (directory listing is synchronous), no need to retransmit + _currentOperation = kCOIdle; + _emitErrorMessage(tr("List: Offset returned (%1) differs from offset requested (%2)").arg(listAck->hdr.offset).arg(_listOffset)); + return; + } + + uint8_t offset = 0; + uint8_t cListEntries = 0; + uint8_t cBytes = listAck->hdr.size; + + // parse filenames out of the buffer + while (offset < cBytes) { + const char * ptr = ((const char *)listAck->data) + offset; + + // get the length of the name + uint8_t cBytesLeft = cBytes - offset; + uint8_t nlen = static_cast(strnlen(ptr, cBytesLeft)); + if ((*ptr == 'S' && nlen > 1) || (*ptr != 'S' && nlen < 2)) { + _currentOperation = kCOIdle; + _emitErrorMessage(tr("Incorrectly formed list entry: '%1'").arg(ptr)); + return; + } else if (nlen == cBytesLeft) { + _currentOperation = kCOIdle; + _emitErrorMessage(tr("Missing NULL termination in list entry")); + return; + } + + // Returned names are prepended with D for directory, F for file, S for skip + if (*ptr == 'F' || *ptr == 'D') { + // put it in the view + _emitListEntry(ptr); + } else if (*ptr == 'S') { + // do nothing + } else { + qDebug() << "unknown entry" << ptr; + } + + // account for the name + NUL + offset += nlen + 1; + + cListEntries++; + } + + if (listAck->hdr.size == 0 || cListEntries == 0) { + // Directory is empty, we're done + Q_ASSERT(listAck->hdr.opcode == MavlinkFTP::kRspAck); + _currentOperation = kCOIdle; + emit commandComplete(); + } else { + // Possibly more entries to come, need to keep trying till we get EOF + _currentOperation = kCOList; + _listOffset += cListEntries; + _sendListCommand(); + } +#endif +} + +/// @brief Respond to the Ack associated with the create command. +void FTPManager::_createAckResponse(MavlinkFTP::Request* createAck) +{ +#if 0 + qCDebug(FTPManagerLog) << "_createAckResponse"; + + _currentOperation = kCOWrite; + _activeSession = createAck->hdr.session; + + // Start the sequence of write commands from the beginning of the file + + _writeOffset = 0; + _writeSize = 0; + + _writeFileDatablock(); +#endif +} + +/// @brief Respond to the Ack associated with the write command. +void FTPManager::_writeAckResponse(MavlinkFTP::Request* writeAck) +{ +#if 0 + if(_writeOffset + _writeSize >= _writeFileSize){ + _closeUploadSession(true /* success */); + return; + } + + if (writeAck->hdr.session != _activeSession) { + _closeUploadSession(false /* failure */); + _emitErrorMessage(tr("Write: Incorrect session returned")); + return; + } + + if (writeAck->hdr.offset != _writeOffset) { + _closeUploadSession(false /* failure */); + _emitErrorMessage(tr("Write: Offset returned (%1) differs from offset requested (%2)").arg(writeAck->hdr.offset).arg(_writeOffset)); + return; + } + + if (writeAck->hdr.size != sizeof(uint32_t)) { + _closeUploadSession(false /* failure */); + _emitErrorMessage(tr("Write: Returned invalid size of write size data")); + return; + } + + + if( writeAck->writeFileLength !=_writeSize) { + _closeUploadSession(false /* failure */); + _emitErrorMessage(tr("Write: Size returned (%1) differs from size requested (%2)").arg(writeAck->writeFileLength).arg(_writeSize)); + return; + } + + _writeFileDatablock(); +#endif +} + +/// @brief Send next write file data block. +void FTPManager::_writeFileDatablock(void) +{ +#if 0 + if (_writeOffset + _writeSize >= _writeFileSize){ + _closeUploadSession(true /* success */); + return; + } + + _writeOffset += _writeSize; + + MavlinkFTP::Request request; + request.hdr.session = _activeSession; + request.hdr.opcode = MavlinkFTP::kCmdWriteFile; + request.hdr.offset = _writeOffset; + + if(_writeFileSize -_writeOffset > sizeof(request.data) ) + _writeSize = sizeof(request.data); + else + _writeSize = _writeFileSize - _writeOffset; + + request.hdr.size = _writeSize; + + memcpy(request.data, &_writeFileAccumulator.data()[_writeOffset], _writeSize); + + _sendRequestExpectAck(&request); +#endif +} + +void FTPManager::mavlinkMessageReceived(mavlink_message_t message) +{ + if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + return; + } + + mavlink_file_transfer_protocol_t data; + mavlink_msg_file_transfer_protocol_decode(&message, &data); + + // Make sure we are the target system + int qgcId = qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(); + if (data.target_system != qgcId) { + return; + } + + MavlinkFTP::Request* request = (MavlinkFTP::Request*)&data.payload[0]; + + uint16_t incomingSeqNumber = request->hdr.seqNumber; + uint16_t expectedSeqNumber = _lastOutgoingRequest.hdr.seqNumber + 1; + + // ignore old/reordered packets (handle wrap-around properly) + if ((uint16_t)((expectedSeqNumber - 1) - incomingSeqNumber) < (std::numeric_limits::max()/2)) { + qDebug() << "Received old packet: expected seq:" << expectedSeqNumber << "got:" << incomingSeqNumber; + return; + } + + _clearAckTimeout(); + + qCDebug(FTPManagerLog) << "mavlinkMessageReceived" << MavlinkFTP::opCodeToString(static_cast(request->hdr.opcode)) << MavlinkFTP::opCodeToString(static_cast(request->hdr.req_opcode)); + + if (incomingSeqNumber != expectedSeqNumber) { + switch (_waitState) { + case MavlinkFTP::kCmdOpenFileRO: + case MavlinkFTP::kCmdReadFile: + case MavlinkFTP::kCmdBurstReadFile: + _downloadComplete(tr("Download failed: Unable to handle packet loss")); + return; +#if 0 + case kCOWrite: + _closeUploadSession(false /* failure */); + break; + + case kCOOpenRead: + case kCOOpenBurst: + case kCOCreate: + // We could have an open session hanging around + _currentOperation = kCOIdle; + _sendResetCommand(); + break; +#endif + default: + // Don't need to do anything special + _waitState = MavlinkFTP::kCmdNone; + break; + } + } + + // Move past the incoming sequence number for next request + _lastOutgoingRequest.hdr.seqNumber = incomingSeqNumber; + + if (request->hdr.opcode == MavlinkFTP::kRspAck) { + switch (request->hdr.req_opcode) { + case MavlinkFTP::kCmdOpenFileRO: + _handlOpenFileROAck(request); + break; + case MavlinkFTP::kCmdReadFile: + _handleReadFileAck(request, false /* burstReadFile */); + break; + case MavlinkFTP::kCmdBurstReadFile: + _handleReadFileAck(request, true /*burstReadFile */); + break; + +#if 0 + case MavlinkFTP::kCmdListDirectory: + _listAckResponse(request); + break; + + case MavlinkFTP::kCmdOpenFileRO: + case MavlinkFTP::kCmdOpenFileWO: + _handlOpenFileROAck(request); + break; + + case MavlinkFTP::kCmdCreateFile: + _createAckResponse(request); + break; + + case MavlinkFTP::kCmdWriteFile: + _writeAckResponse(request); + break; +#endif + default: + // Ack back from operation which does not require additional work + _waitState = MavlinkFTP::kCmdNone; + break; + } + } else if (request->hdr.opcode == MavlinkFTP::kRspNak) { + QString errorMsg; + MavlinkFTP::OpCode_t requestOpCode = static_cast(request->hdr.req_opcode); + MavlinkFTP::ErrorCode_t errorCode = static_cast(request->data[0]); + + if (requestOpCode == MavlinkFTP::kCmdReadFile && errorCode == MavlinkFTP::kErrEOF && _readFileAccumulator.size() == _downloadFileSize) { + _downloadComplete(QString()); + } else { + // Nak's normally have 1 byte of data for error code, except for MavlinkFTP::kErrFailErrno which has additional byte for errno + if ((errorCode == MavlinkFTP::kErrFailErrno && request->hdr.size != 2) || ((errorCode != MavlinkFTP::kErrFailErrno) && request->hdr.size != 1)) { + errorMsg = tr("Invalid Nak format"); + } else if (errorCode == MavlinkFTP::kErrFailErrno) { + errorMsg = tr("errno %1").arg(request->data[1]); + } else { + errorMsg = MavlinkFTP::errorCodeToString(errorCode); + } + + _waitState = MavlinkFTP::kCmdNone; + + switch (request->hdr.req_opcode) { + case MavlinkFTP::kCmdOpenFileRO: + case MavlinkFTP::kCmdReadFile: + case MavlinkFTP::kCmdBurstReadFile: + _downloadComplete(tr("Download failed: %1").arg(errorMsg)); + break; + default: + // FIXME: Rest is NYI + break; + } + } + } +} + +void FTPManager::listDirectory(const QString& dirPath) +{ + if (_waitState != MavlinkFTP::kCmdNone) { + _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete.")); + return; + } + + _dedicatedLink = _vehicle->priorityLink(); + if (!_dedicatedLink) { + _emitErrorMessage(tr("Command not sent. No Vehicle links.")); + return; + } + + // initialise the lister + _listPath = dirPath; + _listOffset = 0; + _waitState = MavlinkFTP::kCmdListDirectory; + + // and send the initial request + _sendListCommand(); +} + +void FTPManager::_fillRequestWithString(MavlinkFTP::Request* request, const QString& str) +{ + strncpy((char *)&request->data[0], str.toStdString().c_str(), sizeof(request->data)); + request->hdr.size = static_cast(strnlen((const char *)&request->data[0], sizeof(request->data))); +} + +void FTPManager::_sendListCommand(void) +{ + MavlinkFTP::Request request; + + request.hdr.session = 0; + request.hdr.opcode = MavlinkFTP::kCmdListDirectory; + request.hdr.offset = _listOffset; + request.hdr.size = 0; + + _fillRequestWithString(&request, _listPath); + + qCDebug(FTPManagerLog) << "listDirectory: path:" << _listPath << "offset:" << _listOffset; + + _sendRequestExpectAck(&request); +} + +bool FTPManager::download(const QString& from, const QString& toDir) +{ + qCDebug(FTPManagerLog) << "download from:" << from << "to:" << toDir; + return _downloadWorker(from, toDir, false /* burstReadFile */); +} + +bool FTPManager::burstDownload(const QString& from, const QString& toDir) +{ + qCDebug(FTPManagerLog) << "burstDownload from:" << from << "to:" << toDir; + return _downloadWorker(from, toDir, true /* burstReadFile */); +} + +bool FTPManager::_downloadWorker(const QString& from, const QString& toDir, bool burstReadFile) +{ + if (_waitState != MavlinkFTP::kCmdNone) { + qCDebug(FTPManagerLog) << "Cannot download. Already in another operation"; + return false; + } + + _dedicatedLink = _vehicle->priorityLink(); + if (!_dedicatedLink) { + qCDebug(FTPManagerLog) << "Cannot download. Vehicle has no priority link"; + return false; + } + + _readFileDownloadDir.setPath(toDir); + + QString strippedFrom; + QString ftpPrefix("mavlinkftp://"); + if (from.startsWith(ftpPrefix, Qt::CaseInsensitive)) { + strippedFrom = from.right(from.length() - ftpPrefix.length() + 1); + } else { + strippedFrom = from; + } + + // We need to strip off the file name from the fully qualified path. We can't use the usual QDir + // routines because this path does not exist locally. + int lastDirSlashIndex; + for (lastDirSlashIndex=strippedFrom.size()-1; lastDirSlashIndex>=0; lastDirSlashIndex--) { + if (strippedFrom[lastDirSlashIndex] == '/') { + break; + } + } + lastDirSlashIndex++; // move past slash + _readFileDownloadFilename = strippedFrom.right(strippedFrom.size() - lastDirSlashIndex); + + _waitState = MavlinkFTP::kCmdOpenFileRO; + _openFileType = burstReadFile ? MavlinkFTP::kCmdBurstReadFile : MavlinkFTP::kCmdReadFile; + + MavlinkFTP::Request request; + request.hdr.session = 0; + request.hdr.opcode = MavlinkFTP::kCmdOpenFileRO; + request.hdr.offset = 0; + request.hdr.size = 0; + _fillRequestWithString(&request, strippedFrom); + _sendRequestExpectAck(&request); + + return true; +} + +/// @brief Uploads the specified file. +/// @param toPath File in UAS to upload to, fully qualified path +/// @param uploadFile Local file to upload from +void FTPManager::upload(const QString& toPath, const QFileInfo& uploadFile) +{ +#if 0 + if(_currentOperation != kCOIdle){ + _emitErrorMessage(tr("UAS File manager busy. Try again later")); + return; + } + + _dedicatedLink = _vehicle->priorityLink(); + if (!_dedicatedLink) { + _emitErrorMessage(tr("Command not sent. No Vehicle links.")); + return; + } + + if (toPath.isEmpty()) { + return; + } + + if (!uploadFile.isReadable()){ + _emitErrorMessage(tr("File (%1) is not readable for upload").arg(uploadFile.path())); + return; + } + + QFile file(uploadFile.absoluteFilePath()); + if (!file.open(QIODevice::ReadOnly)) { + _emitErrorMessage(tr("Unable to open local file for upload (%1)").arg(uploadFile.absoluteFilePath())); + return; + } + + _writeFileAccumulator = file.readAll(); + _writeFileSize = _writeFileAccumulator.size(); + + file.close(); + + if (_writeFileAccumulator.size() == 0) { + _emitErrorMessage(tr("Unable to read data from local file (%1)").arg(uploadFile.absoluteFilePath())); + return; + } + + _currentOperation = kCOCreate; + + MavlinkFTP::Request request; + request.hdr.session = 0; + request.hdr.opcode = MavlinkFTP::kCmdCreateFile; + request.hdr.offset = 0; + request.hdr.size = 0; + _fillRequestWithString(&request, toPath + "/" + uploadFile.fileName()); + _sendRequestExpectAck(&request); +#endif +} + +void FTPManager::createDirectory(const QString& directory) +{ +#if 0 + if(_currentOperation != kCOIdle){ + _emitErrorMessage(tr("UAS File manager busy. Try again later")); + return; + } + + _currentOperation = kCOCreateDir; + + MavlinkFTP::Request request; + request.hdr.session = 0; + request.hdr.opcode = MavlinkFTP::kCmdCreateDirectory; + request.hdr.offset = 0; + request.hdr.size = 0; + _fillRequestWithString(&request, directory); + _sendRequestExpectAck(&request); +#endif +} + +/// @brief Sends a command which only requires an opcode and no additional data +/// @param opcode Opcode to send +/// @param newOpState State to put state machine into +/// @return TRUE: command sent, FALSE: command not sent, waiting for previous command to finish +bool FTPManager::_sendOpcodeOnlyCmd(MavlinkFTP::OpCode_t opcode, MavlinkFTP::OpCode_t newWaitState) +{ + if (_waitState != MavlinkFTP::kCmdNone) { + // Can't have multiple commands in play at the same time + return false; + } + + _waitState = newWaitState; + + MavlinkFTP::Request request; + request.hdr.session = 0; + request.hdr.opcode = opcode; + request.hdr.offset = 0; + request.hdr.size = 0; + _sendRequestExpectAck(&request); + + return true; +} + +/// @brief Starts the ack timeout timer +void FTPManager::_setupAckTimeout(void) +{ + qCDebug(FTPManagerLog) << "_setupAckTimeout"; + + Q_ASSERT(!_ackTimer.isActive()); + + _ackNumTries = 0; + _ackTimer.setSingleShot(false); + _ackTimer.start(_ackTimerTimeoutMsecs); +} + +/// @brief Clears the ack timeout timer +void FTPManager::_clearAckTimeout(void) +{ + qCDebug(FTPManagerLog) << "_clearAckTimeout"; + _ackTimer.stop(); +} + +void FTPManager::_ackTimeout(void) +{ + qCDebug(FTPManagerLog) << "_ackTimeout"; + +#if 0 + // FIXME: retry NYI + if (++_ackNumTries <= _ackTimerMaxRetries) { + qCDebug(FTPManagerLog) << "ack timeout - retrying"; + if (_currentOperation == kCOBurst) { + // for burst downloads try to initiate a new burst + MavlinkFTP::Request request; + request.hdr.session = _activeSession; + request.hdr.opcode = MavlinkFTP::kCmdBurstReadFile; + request.hdr.offset = _downloadOffset; + request.hdr.size = 0; + request.hdr.seqNumber = ++_lastOutgoingRequest.hdr.seqNumber; + + _sendRequestNoAck(&request); + } else { + _sendRequestNoAck(&_lastOutgoingRequest); + } + return; + } +#endif + + _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. + + switch (_waitState) { + case MavlinkFTP::kCmdOpenFileRO: + case MavlinkFTP::kCmdReadFile: + case MavlinkFTP::kCmdBurstReadFile: + _downloadComplete(tr("Download failed: Vehicle did not response to %1").arg(MavlinkFTP::opCodeToString(_waitState))); + break; +#if 0 + // FIXME: NYI + case kCOOpenRead: + case kCOOpenBurst: + _currentOperation = kCOIdle; + _emitErrorMessage(tr("Timeout waiting for ack: Download failed")); + _sendResetCommand(); + break; + + case kCOCreate: + _currentOperation = kCOIdle; + _emitErrorMessage(tr("Timeout waiting for ack: Upload failed")); + _sendResetCommand(); + break; + + case kCOWrite: + _closeUploadSession(false /* failure */); + _emitErrorMessage(tr("Timeout waiting for ack: Upload failed")); + break; +#endif + default: + { + MavlinkFTP::OpCode_t _lastCommand = _waitState; + _waitState = MavlinkFTP::kCmdNone; + _emitErrorMessage(QString("Timeout waiting for ack: Command failed (%1)").arg(MavlinkFTP::opCodeToString(_lastCommand))); + } + break; + } +} + +void FTPManager::_sendResetCommand(void) +{ + MavlinkFTP::Request request; + request.hdr.opcode = MavlinkFTP::kCmdResetSessions; + request.hdr.size = 0; + _sendRequestExpectAck(&request); +} + +void FTPManager::_emitErrorMessage(const QString& msg) +{ + qCDebug(FTPManagerLog) << "Error:" << msg; + emit commandError(msg); +} + +void FTPManager::_emitListEntry(const QString& entry) +{ + qCDebug(FTPManagerLog) << "_emitListEntry" << entry; + emit listEntry(entry); +} + +void FTPManager::_sendRequestExpectAck(MavlinkFTP::Request* request) +{ + _setupAckTimeout(); + + request->hdr.seqNumber = ++_lastOutgoingRequest.hdr.seqNumber; + qCDebug(FTPManagerLog) << "_sendRequestExpectAck opcode:" << MavlinkFTP::opCodeToString(static_cast(request->hdr.opcode)) << "seqNumber:" << request->hdr.seqNumber; + + if (request->hdr.size <= sizeof(request->data)) { + memcpy(&_lastOutgoingRequest, request, sizeof(MavlinkFTP::RequestHeader) + request->hdr.size); + } else { + // FIXME: Shouldn't this fail something? + qCCritical(FTPManagerLog) << "request length too long:" << request->hdr.size; + } + + _sendRequestNoAck(request); +} + +void FTPManager::_sendRequestNoAck(MavlinkFTP::Request* request) +{ + qCDebug(FTPManagerLog) << "_sendRequestNoAck opcode:" << MavlinkFTP::opCodeToString(static_cast(request->hdr.opcode)); + + mavlink_message_t message; + mavlink_msg_file_transfer_protocol_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + 0, // Target network, 0=broadcast? + _vehicle->id(), + MAV_COMP_ID_AUTOPILOT1, + (uint8_t*)request); // Payload + _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, message); +} diff --git a/src/Vehicle/FTPManager.h b/src/Vehicle/FTPManager.h new file mode 100644 index 000000000..06968fae4 --- /dev/null +++ b/src/Vehicle/FTPManager.h @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +#pragma once + +#include +#include +#include +#include + +#include "UASInterface.h" +#include "QGCLoggingCategory.h" +#include "QGCMAVLink.h" + +Q_DECLARE_LOGGING_CATEGORY(FTPManagerLog) + +class Vehicle; + +class FTPManager : public QObject +{ + Q_OBJECT + +public: + FTPManager(Vehicle* vehicle); + + /// Downloads the specified file. + /// @param from File to download from vehicle, fully qualified path. May be in the format mavlinkftp://... + /// @param toDir Local directory to download file to + /// @return true: download has started, false: error, no download + /// Signals downloadComplete, commandError, commandProgress + bool download(const QString& from, const QString& toDir); + + /// Stream downloads the specified file. + /// @param from File to download from UAS, fully qualified path. May be in the format mavlinkftp://... + /// @param toDir Local directory to download file to + /// @return true: download has started, false: error, no download + /// Signals downloadComplete, commandError, commandProgress + bool burstDownload(const QString& from, const QString& toDir); + + /// Lists the specified directory. Emits listEntry signal for each entry, followed by listComplete signal. + /// @param dirPath Fully qualified path to list + void listDirectory(const QString& dirPath); + + /// Upload the specified file to the specified location + void upload(const QString& toPath, const QFileInfo& uploadFile); + + /// Create a remote directory + void createDirectory(const QString& directory); + + void mavlinkMessageReceived(mavlink_message_t message); + +signals: + void downloadComplete (const QString& file, const QString& errorMsg); + void uploadComplete (const QString& errorMsg); + + /// Signalled to indicate a new directory entry was received. + void listEntry(const QString& entry); + + // Signals associated with all commands + + /// Signalled after a command has completed + void commandComplete(void); + + void commandError(const QString& msg); + + /// Signalled during a lengthy command to show progress + /// @param value Amount of progress: 0.0 = none, 1.0 = complete + void commandProgress(int value); + +private slots: + void _ackTimeout(void); + +private: + bool _sendOpcodeOnlyCmd (MavlinkFTP::OpCode_t opcode, MavlinkFTP::OpCode_t newWaitState); + void _setupAckTimeout (void); + void _clearAckTimeout (void); + void _emitErrorMessage (const QString& msg); + void _emitListEntry (const QString& entry); + void _sendRequestExpectAck (MavlinkFTP::Request* request); + void _sendRequestNoAck (MavlinkFTP::Request* request); + void _sendMessageOnLink (LinkInterface* link, mavlink_message_t message); + void _fillRequestWithString (MavlinkFTP::Request* request, const QString& str); + void _handlOpenFileROAck (MavlinkFTP::Request* ack); + void _handleReadFileAck (MavlinkFTP::Request* ack, bool burstReadFile); + void _listAckResponse (MavlinkFTP::Request* listAck); + void _createAckResponse (MavlinkFTP::Request* createAck); + void _writeAckResponse (MavlinkFTP::Request* writeAck); + void _writeFileDatablock (void); + void _sendListCommand (void); + void _sendResetCommand (void); + void _downloadComplete (const QString& errorMsg); + void _uploadComplete (const QString& errorMsg); + bool _downloadWorker (const QString& from, const QString& toDir, bool burstReadFile); + void _requestMissingData (void); + + MavlinkFTP::OpCode_t _waitState = MavlinkFTP::kCmdNone; ///< Current operation of state machine + MavlinkFTP::OpCode_t _openFileType = MavlinkFTP::kCmdReadFile; ///< Type of read to use after open file succeeds + QTimer _ackTimer; ///< Used to signal a timeout waiting for an ack + int _ackNumTries; ///< current number of tries + Vehicle* _vehicle; + LinkInterface* _dedicatedLink = nullptr; ///< Link to use for communication + MavlinkFTP::Request _lastOutgoingRequest; ///< contains the last outgoing packet + unsigned _listOffset; ///< offset for the current List operation + QString _listPath; ///< path for the current List operation + uint8_t _activeSession = 0; ///< currently active session, 0 for none + uint32_t _readOffset; ///< current read offset + + uint32_t _writeOffset; ///< current write offset + uint32_t _writeSize; ///< current write data size + uint32_t _writeFileSize; ///< Size of file being uploaded + QByteArray _writeFileAccumulator; ///< Holds file being uploaded + + struct MissingData { + uint32_t offset; + uint32_t size; + }; + uint32_t _requestedDownloadOffset; ///< current download offset + QByteArray _readFileAccumulator; ///< Holds file being downloaded + QDir _readFileDownloadDir; ///< Directory to download file to + QString _readFileDownloadFilename; ///< Filename (no path) for download file + int _downloadFileSize; ///< Size of file being downloaded + + static const int _ackTimerTimeoutMsecs = 5000; + static const int _ackTimerMaxRetries = 6; +}; + diff --git a/src/Vehicle/FTPManagerTest.cc b/src/Vehicle/FTPManagerTest.cc new file mode 100644 index 000000000..39b35660f --- /dev/null +++ b/src/Vehicle/FTPManagerTest.cc @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "FTPManagerTest.h" +#include "MultiVehicleManager.h" +#include "QGCApplication.h" +#include "MockLink.h" +#include "FTPManager.h" + +const FTPManagerTest::TestCase_t FTPManagerTest::_rgTestCases[] = { + { "/version.json" }, +}; + +void FTPManagerTest::_testCaseWorker(const TestCase_t& testCase) +{ + _connectMockLinkNoInitialConnectSequence(); + + MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + Vehicle* vehicle = vehicleMgr->activeVehicle(); + FTPManager* ftpManager = vehicle->ftpManager(); + + QSignalSpy spyDownloadComplete(ftpManager, &FTPManager::downloadComplete); + + // void downloadComplete (const QString& file, const QString& errorMsg); + ftpManager->download(testCase.file, QStandardPaths::writableLocation(QStandardPaths::TempLocation)); + + QCOMPARE(spyDownloadComplete.wait(10000), true); + QCOMPARE(spyDownloadComplete.count(), 1); + QList arguments = spyDownloadComplete.takeFirst(); + qDebug() << arguments[0].toString(); + QVERIFY(arguments[1].toString().isEmpty()); + + _disconnectMockLink(); +} + +void FTPManagerTest::_performTestCases(void) +{ + int index = 0; + for (const TestCase_t& testCase: _rgTestCases) { + qDebug() << "Testing case" << index++; + _testCaseWorker(testCase); + } +} diff --git a/src/Vehicle/FTPManagerTest.h b/src/Vehicle/FTPManagerTest.h new file mode 100644 index 000000000..91c06087e --- /dev/null +++ b/src/Vehicle/FTPManagerTest.h @@ -0,0 +1,29 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "UnitTest.h" + +class FTPManagerTest : public UnitTest +{ + Q_OBJECT + +private slots: + void _performTestCases(void); + +private: + typedef struct { + const char* file; + } TestCase_t; + + void _testCaseWorker(const TestCase_t& testCase); + + static const TestCase_t _rgTestCases[]; +}; diff --git a/src/Vehicle/InitialConnectTest.cc b/src/Vehicle/InitialConnectTest.cc new file mode 100644 index 000000000..38a515f2e --- /dev/null +++ b/src/Vehicle/InitialConnectTest.cc @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "InitialConnectTest.h" +#include "MultiVehicleManager.h" +#include "QGCApplication.h" +#include "MockLink.h" + +InitialConnectTest::TestCase_t InitialConnectTest::_rgTestCases[] = { + { MockLink::FailRequestMessageNone, MAV_RESULT_ACCEPTED, Vehicle::RequestMessageNoFailure, false }, + { MockLink::FailRequestMessageCommandAcceptedMsgNotSent, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureMessageNotReceived, false }, + { MockLink::FailRequestMessageCommandUnsupported, MAV_RESULT_UNSUPPORTED, Vehicle::RequestMessageFailureCommandError, false }, + { MockLink::FailRequestMessageCommandNoResponse, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureCommandNotAcked, false }, + //{ MockLink::FailRequestMessageCommandAcceptedSecondAttempMsgSent, MAV_RESULT_FAILED, Vehicle::RequestMessageNoFailure, false }, +}; + +void InitialConnectTest::_requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message) +{ + TestCase_t* testCase = static_cast(resultHandlerData); + + testCase->resultHandlerCalled = true; + QCOMPARE((int)testCase->expectedCommandResult, (int)commandResult); + QCOMPARE((int)testCase->expectedFailureCode, (int)failureCode); + if (testCase->expectedFailureCode == Vehicle::RequestMessageNoFailure) { + QVERIFY(message.msgid == MAVLINK_MSG_ID_DEBUG); + } +} + +void InitialConnectTest::_testCaseWorker(TestCase_t& testCase) +{ + _connectMockLink(); + + MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + Vehicle* vehicle = vehicleMgr->activeVehicle(); + + _mockLink->setRequestMessageFailureMode(testCase.failureMode); + vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_ALL, MAVLINK_MSG_ID_DEBUG); + for (int i=0; i<100; i++) { + QTest::qWait(100); + if (testCase.resultHandlerCalled) { + break; + } + } + QVERIFY(testCase.resultHandlerCalled); + + _disconnectMockLink(); +} + +void InitialConnectTest::_test(void) +{ + _connectMockLink(); + + //MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + //Vehicle* vehicle = vehicleMgr->activeVehicle(); + + _disconnectMockLink(); +} + +void InitialConnectTest::_performTestCases(void) +{ + int index = 0; + for (TestCase_t& testCase: _rgTestCases) { + qDebug() << "Testing case" << index++; + _testCaseWorker(testCase); + } +} diff --git a/src/Vehicle/InitialConnectTest.h b/src/Vehicle/InitialConnectTest.h new file mode 100644 index 000000000..ab70af457 --- /dev/null +++ b/src/Vehicle/InitialConnectTest.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "UnitTest.h" +#include "MockLink.h" +#include "Vehicle.h" + +class InitialConnectTest : public UnitTest +{ + Q_OBJECT + +signals: + void resultHandlerCalled(void); + +private slots: + void _test(void); + +private: + void _performTestCases(void); + typedef struct { + MockLink::RequestMessageFailureMode_t failureMode; + MAV_RESULT expectedCommandResult; + Vehicle::RequestMessageResultHandlerFailureCode_t expectedFailureCode; + bool resultHandlerCalled; + } TestCase_t; + + void _testCaseWorker(TestCase_t& testCase); + + static void _requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message); + + bool _resultHandlerCalled; + + static TestCase_t _rgTestCases[]; +}; diff --git a/src/Vehicle/MAVLinkFTPManager.cc b/src/Vehicle/MAVLinkFTPManager.cc deleted file mode 100644 index 537e13e00..000000000 --- a/src/Vehicle/MAVLinkFTPManager.cc +++ /dev/null @@ -1,887 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -#include "MAVLinkFTPManager.h" -#include "Vehicle.h" -#include "QGCApplication.h" - -#include -#include -#include - -QGC_LOGGING_CATEGORY(MAVLinkFTPManagerLog, "MAVLinkFTPManagerLog") - -MAVLinkFTPManager::MAVLinkFTPManager(Vehicle* vehicle) - : QObject (vehicle) - , _vehicle (vehicle) -{ - connect(&_ackTimer, &QTimer::timeout, this, &MAVLinkFTPManager::_ackTimeout); - - _lastOutgoingRequest.hdr.seqNumber = 0; - - _systemIdServer = _vehicle->id(); - - // Make sure we don't have bad structure packing - Q_ASSERT(sizeof(RequestHeader) == 12); -} - -/// Respond to the Ack associated with the Open command with the next read command. -void MAVLinkFTPManager::_openAckResponse(Request* openAck) -{ - qCDebug(MAVLinkFTPManagerLog) << QString("_openAckResponse: _currentOperation(%1) _readFileLength(%2)").arg(_currentOperation).arg(openAck->openFileLength); - - Q_ASSERT(_currentOperation == kCOOpenRead || _currentOperation == kCOOpenBurst); - _currentOperation = _currentOperation == kCOOpenRead ? kCORead : kCOBurst; - _activeSession = openAck->hdr.session; - - // File length comes back in data - Q_ASSERT(openAck->hdr.size == sizeof(uint32_t)); - _downloadFileSize = openAck->openFileLength; - - // Start the sequence of read commands - - _downloadOffset = 0; // Start reading at beginning of file - _readFileAccumulator.clear(); // Start with an empty file - _missingDownloadedBytes = 0; - _downloadingMissingParts = false; - _missingData.clear(); - - Request request; - request.hdr.session = _activeSession; - Q_ASSERT(_currentOperation == kCORead || _currentOperation == kCOBurst); - request.hdr.opcode = _currentOperation == kCORead ? kCmdReadFile : kCmdBurstReadFile; - request.hdr.offset = _downloadOffset; - request.hdr.size = sizeof(request.data); - - _sendRequest(&request); -} - -/// request the next missing part of a (partially) downloaded file -void MAVLinkFTPManager::_requestMissingData() -{ - if (_missingData.empty()) { - _downloadingMissingParts = false; - _missingDownloadedBytes = 0; - // there might be more data missing at the end - if ((uint32_t)_readFileAccumulator.length() != _downloadFileSize) { - _downloadOffset = _readFileAccumulator.length(); - qCDebug(MAVLinkFTPManagerLog) << QString("_requestMissingData: missing parts done, downloadOffset(%1) downloadFileSize(%2)") - .arg(_downloadOffset).arg(_downloadFileSize); - } else { - _closeDownloadSession(true); - return; - } - } else { - qCDebug(MAVLinkFTPManagerLog) << QString("_requestMissingData: offset(%1) size(%2)").arg(_missingData.head().offset).arg(_missingData.head().size); - - _downloadOffset = _missingData.head().offset; - } - - Request request; - _currentOperation = kCORead; - request.hdr.session = _activeSession; - request.hdr.opcode = kCmdReadFile; - request.hdr.offset = _downloadOffset; - request.hdr.size = sizeof(request.data); - - _sendRequest(&request); -} - -/// Closes out a download session by writing the file and doing cleanup. -/// @param success true: successful download completion, false: error during download -void MAVLinkFTPManager::_closeDownloadSession(bool success) -{ - qCDebug(MAVLinkFTPManagerLog) << QString("_closeDownloadSession: success(%1) missingBytes(%2)").arg(success).arg(_missingDownloadedBytes); - - _currentOperation = kCOIdle; - - if (success) { - if (_missingDownloadedBytes > 0 || (uint32_t)_readFileAccumulator.length() < _downloadFileSize) { - // we're not done yet: request the missing parts individually (either we had missing parts or - // the last (few) packets right before the EOF got dropped) - _downloadingMissingParts = true; - _requestMissingData(); - return; - } - - QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename); - - QFile file(downloadFilePath); - if (!file.open(QIODevice::WriteOnly | QIODevice::Truncate)) { - _emitErrorMessage(tr("Unable to open local file for writing (%1)").arg(downloadFilePath)); - return; - } - - qint64 bytesWritten = file.write((const char *)_readFileAccumulator, _readFileAccumulator.length()); - if (bytesWritten != _readFileAccumulator.length()) { - file.close(); - _emitErrorMessage(tr("Unable to write data to local file (%1)").arg(downloadFilePath)); - return; - } - file.close(); - - emit commandComplete(); - } - - _readFileAccumulator.clear(); - - // Close the open session - _sendResetCommand(); -} - -/// Closes out an upload session doing cleanup. -/// @param success true: successful upload completion, false: error during download -void MAVLinkFTPManager::_closeUploadSession(bool success) -{ - qCDebug(MAVLinkFTPManagerLog) << QString("_closeUploadSession: success(%1)").arg(success); - - _currentOperation = kCOIdle; - _writeFileAccumulator.clear(); - _writeFileSize = 0; - - if (success) { - emit commandComplete(); - } - - // Close the open session - _sendResetCommand(); -} - -/// Respond to the Ack associated with the Read or Stream commands. -/// @param readFile: true: read file, false: stream file -void MAVLinkFTPManager::_downloadAckResponse(Request* readAck, bool readFile) -{ - if (readAck->hdr.session != _activeSession) { - _closeDownloadSession(false /* failure */); - _emitErrorMessage(tr("Download: Incorrect session returned")); - return; - } - - if (readAck->hdr.offset != _downloadOffset) { - if (readFile) { - _closeDownloadSession(false /* failure */); - _emitErrorMessage(tr("Download: Offset returned (%1) differs from offset requested/expected (%2)").arg(readAck->hdr.offset).arg(_downloadOffset)); - return; - } else { // burst - if (readAck->hdr.offset < _downloadOffset) { // old data: ignore it - _setupAckTimeout(); - return; - } - // keep track of missing data chunks - MissingData missingData; - missingData.offset = _downloadOffset; - missingData.size = readAck->hdr.offset - _downloadOffset; - _missingData.push_back(missingData); - _missingDownloadedBytes += readAck->hdr.offset - _downloadOffset; - qCDebug(MAVLinkFTPManagerLog) << QString("_downloadAckResponse: missing data: offset(%1) size(%2)").arg(missingData.offset).arg(missingData.size); - _downloadOffset = readAck->hdr.offset; - _readFileAccumulator.resize(_downloadOffset); // placeholder for the missing data - } - } - - qCDebug(MAVLinkFTPManagerLog) << QString("_downloadAckResponse: offset(%1) size(%2) burstComplete(%3)").arg(readAck->hdr.offset).arg(readAck->hdr.size).arg(readAck->hdr.burstComplete); - - if (_downloadingMissingParts) { - Q_ASSERT(_missingData.head().offset == _downloadOffset); - _missingDownloadedBytes -= readAck->hdr.size; - _readFileAccumulator.replace(_downloadOffset, readAck->hdr.size, (const char*)readAck->data, readAck->hdr.size); - if (_missingData.head().size <= readAck->hdr.size) { - _missingData.pop_front(); - } else { - _missingData.head().size -= readAck->hdr.size; - _missingData.head().offset += readAck->hdr.size; - } - } else { - _downloadOffset += readAck->hdr.size; - _readFileAccumulator.append((const char*)readAck->data, readAck->hdr.size); - } - - if (_downloadFileSize != 0) { - emit commandProgress(100 * ((float)(_readFileAccumulator.length() - _missingDownloadedBytes) / (float)_downloadFileSize)); - } - - if (_downloadingMissingParts) { - _requestMissingData(); - } else if (readFile || readAck->hdr.burstComplete) { - // Possibly still more data to read, send next read request - - Request request; - request.hdr.session = _activeSession; - request.hdr.opcode = readFile ? kCmdReadFile : kCmdBurstReadFile; - request.hdr.offset = _downloadOffset; - request.hdr.size = 0; - - _sendRequest(&request); - } else if (!readFile) { - // Streaming, so next ack should come automatically - _setupAckTimeout(); - } -} - -/// @brief Respond to the Ack associated with the List command. -void MAVLinkFTPManager::_listAckResponse(Request* listAck) -{ - if (listAck->hdr.offset != _listOffset) { - // this is a real error (directory listing is synchronous), no need to retransmit - _currentOperation = kCOIdle; - _emitErrorMessage(tr("List: Offset returned (%1) differs from offset requested (%2)").arg(listAck->hdr.offset).arg(_listOffset)); - return; - } - - uint8_t offset = 0; - uint8_t cListEntries = 0; - uint8_t cBytes = listAck->hdr.size; - - // parse filenames out of the buffer - while (offset < cBytes) { - const char * ptr = ((const char *)listAck->data) + offset; - - // get the length of the name - uint8_t cBytesLeft = cBytes - offset; - uint8_t nlen = static_cast(strnlen(ptr, cBytesLeft)); - if ((*ptr == 'S' && nlen > 1) || (*ptr != 'S' && nlen < 2)) { - _currentOperation = kCOIdle; - _emitErrorMessage(tr("Incorrectly formed list entry: '%1'").arg(ptr)); - return; - } else if (nlen == cBytesLeft) { - _currentOperation = kCOIdle; - _emitErrorMessage(tr("Missing NULL termination in list entry")); - return; - } - - // Returned names are prepended with D for directory, F for file, S for skip - if (*ptr == 'F' || *ptr == 'D') { - // put it in the view - _emitListEntry(ptr); - } else if (*ptr == 'S') { - // do nothing - } else { - qDebug() << "unknown entry" << ptr; - } - - // account for the name + NUL - offset += nlen + 1; - - cListEntries++; - } - - if (listAck->hdr.size == 0 || cListEntries == 0) { - // Directory is empty, we're done - Q_ASSERT(listAck->hdr.opcode == kRspAck); - _currentOperation = kCOIdle; - emit commandComplete(); - } else { - // Possibly more entries to come, need to keep trying till we get EOF - _currentOperation = kCOList; - _listOffset += cListEntries; - _sendListCommand(); - } -} - -/// @brief Respond to the Ack associated with the create command. -void MAVLinkFTPManager::_createAckResponse(Request* createAck) -{ - qCDebug(MAVLinkFTPManagerLog) << "_createAckResponse"; - - _currentOperation = kCOWrite; - _activeSession = createAck->hdr.session; - - // Start the sequence of write commands from the beginning of the file - - _writeOffset = 0; - _writeSize = 0; - - _writeFileDatablock(); -} - -/// @brief Respond to the Ack associated with the write command. -void MAVLinkFTPManager::_writeAckResponse(Request* writeAck) -{ - if(_writeOffset + _writeSize >= _writeFileSize){ - _closeUploadSession(true /* success */); - return; - } - - if (writeAck->hdr.session != _activeSession) { - _closeUploadSession(false /* failure */); - _emitErrorMessage(tr("Write: Incorrect session returned")); - return; - } - - if (writeAck->hdr.offset != _writeOffset) { - _closeUploadSession(false /* failure */); - _emitErrorMessage(tr("Write: Offset returned (%1) differs from offset requested (%2)").arg(writeAck->hdr.offset).arg(_writeOffset)); - return; - } - - if (writeAck->hdr.size != sizeof(uint32_t)) { - _closeUploadSession(false /* failure */); - _emitErrorMessage(tr("Write: Returned invalid size of write size data")); - return; - } - - - if( writeAck->writeFileLength !=_writeSize) { - _closeUploadSession(false /* failure */); - _emitErrorMessage(tr("Write: Size returned (%1) differs from size requested (%2)").arg(writeAck->writeFileLength).arg(_writeSize)); - return; - } - - _writeFileDatablock(); -} - -/// @brief Send next write file data block. -void MAVLinkFTPManager::_writeFileDatablock(void) -{ - if (_writeOffset + _writeSize >= _writeFileSize){ - _closeUploadSession(true /* success */); - return; - } - - _writeOffset += _writeSize; - - Request request; - request.hdr.session = _activeSession; - request.hdr.opcode = kCmdWriteFile; - request.hdr.offset = _writeOffset; - - if(_writeFileSize -_writeOffset > sizeof(request.data) ) - _writeSize = sizeof(request.data); - else - _writeSize = _writeFileSize - _writeOffset; - - request.hdr.size = _writeSize; - - memcpy(request.data, &_writeFileAccumulator.data()[_writeOffset], _writeSize); - - _sendRequest(&request); -} - -void MAVLinkFTPManager::receiveMessage(mavlink_message_t message) -{ - // 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_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 incorrect target_system:" << data.target_system << "expected:" << _systemIdQGC; - return; - } - - 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::max()/2)) { - qDebug() << "Received old packet: expected seq:" << expectedSeqNumber << "got:" << incomingSeqNumber; - return; - } - - _clearAckTimeout(); - - qCDebug(MAVLinkFTPManagerLog) << "receiveMessage" << request->hdr.opcode; - - if (incomingSeqNumber != expectedSeqNumber) { - bool doAbort = true; - switch (_currentOperation) { - case kCOBurst: // burst download drops are handled in _downloadAckResponse() - doAbort = false; - break; - case kCORead: - _closeDownloadSession(false /* failure */); - break; - - case kCOWrite: - _closeUploadSession(false /* failure */); - break; - - case kCOOpenRead: - case kCOOpenBurst: - case kCOCreate: - // We could have an open session hanging around - _currentOperation = kCOIdle; - _sendResetCommand(); - break; - - default: - // Don't need to do anything special - _currentOperation = kCOIdle; - break; - } - - if (doAbort) { - _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 - _lastOutgoingRequest.hdr.seqNumber = incomingSeqNumber; - - if (request->hdr.opcode == kRspAck) { - switch (request->hdr.req_opcode) { - case kCmdListDirectory: - _listAckResponse(request); - break; - - case kCmdOpenFileRO: - case kCmdOpenFileWO: - _openAckResponse(request); - break; - - case kCmdReadFile: - _downloadAckResponse(request, true /* read file */); - break; - - case kCmdBurstReadFile: - _downloadAckResponse(request, false /* stream file */); - break; - - case kCmdCreateFile: - _createAckResponse(request); - break; - - case kCmdWriteFile: - _writeAckResponse(request); - break; - - default: - // Ack back from operation which does not require additional work - _currentOperation = kCOIdle; - break; - } - } else if (request->hdr.opcode == kRspNak) { - uint8_t errorCode = request->data[0]; - - // Nak's normally have 1 byte of data for error code, except for kErrFailErrno which has additional byte for errno - Q_ASSERT((errorCode == kErrFailErrno && request->hdr.size == 2) || request->hdr.size == 1); - - _currentOperation = kCOIdle; - - if (request->hdr.req_opcode == kCmdListDirectory && errorCode == kErrEOF) { - // This is not an error, just the end of the list loop - emit commandComplete(); - return; - } else if ((request->hdr.req_opcode == kCmdReadFile || request->hdr.req_opcode == kCmdBurstReadFile) && errorCode == kErrEOF) { - // This is not an error, just the end of the download loop - _closeDownloadSession(true /* success */); - return; - } else if (request->hdr.req_opcode == kCmdCreateFile) { - _emitErrorMessage(tr("Nak received creating file, error: %1").arg(errorString(request->data[0]))); - return; - } else if (request->hdr.req_opcode == kCmdCreateDirectory) { - _emitErrorMessage(tr("Nak received creating directory, error: %1").arg(errorString(request->data[0]))); - return; - } else { - // Generic Nak handling - if (request->hdr.req_opcode == kCmdReadFile || request->hdr.req_opcode == kCmdBurstReadFile) { - // Nak error during download loop, download failed - _closeDownloadSession(false /* failure */); - } else if (request->hdr.req_opcode == kCmdWriteFile) { - // Nak error during upload loop, upload failed - _closeUploadSession(false /* failure */); - } - _emitErrorMessage(tr("Nak received, error: %1").arg(errorString(request->data[0]))); - } - } else { - // Note that we don't change our operation state. If something goes wrong beyond this, the operation - // will time out. - _emitErrorMessage(tr("Unknown opcode returned from server: %1").arg(request->hdr.opcode)); - } -} - -void MAVLinkFTPManager::listDirectory(const QString& dirPath) -{ - if (_currentOperation != kCOIdle) { - _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete.")); - return; - } - - _dedicatedLink = _vehicle->priorityLink(); - if (!_dedicatedLink) { - _emitErrorMessage(tr("Command not sent. No Vehicle links.")); - return; - } - - // initialise the lister - _listPath = dirPath; - _listOffset = 0; - _currentOperation = kCOList; - - // and send the initial request - _sendListCommand(); -} - -void MAVLinkFTPManager::_fillRequestWithString(Request* request, const QString& str) -{ - strncpy((char *)&request->data[0], str.toStdString().c_str(), sizeof(request->data)); - request->hdr.size = static_cast(strnlen((const char *)&request->data[0], sizeof(request->data))); -} - -void MAVLinkFTPManager::_sendListCommand(void) -{ - Request request; - - request.hdr.session = 0; - request.hdr.opcode = kCmdListDirectory; - request.hdr.offset = _listOffset; - request.hdr.size = 0; - - _fillRequestWithString(&request, _listPath); - - qCDebug(MAVLinkFTPManagerLog) << "listDirectory: path:" << _listPath << "offset:" << _listOffset; - - _sendRequest(&request); -} - -void MAVLinkFTPManager::downloadPath(const QString& vehicleFilePath, const QDir& downloadDir) -{ - if (_currentOperation != kCOIdle) { - _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete.")); - return; - } - - _dedicatedLink = _vehicle->priorityLink(); - if (!_dedicatedLink) { - _emitErrorMessage(tr("Command not sent. No Vehicle links.")); - return; - } - - qCDebug(MAVLinkFTPManagerLog) << "downloadPath vehicleFilePath:" << vehicleFilePath << "to:" << downloadDir; - _downloadWorker(vehicleFilePath, downloadDir, true /* read file */); -} - -void MAVLinkFTPManager::streamPath(const QString& from, const QDir& downloadDir) -{ - if (_currentOperation != kCOIdle) { - _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete.")); - return; - } - - _dedicatedLink = _vehicle->priorityLink(); - if (!_dedicatedLink) { - _emitErrorMessage(tr("Command not sent. No Vehicle links.")); - return; - } - - qCDebug(MAVLinkFTPManagerLog) << "streamPath from:" << from << "to:" << downloadDir; - _downloadWorker(from, downloadDir, false /* stream file */); -} - -void MAVLinkFTPManager::_downloadWorker(const QString& vehicleFilePath, const QDir& downloadDir, bool readFile) -{ - if (vehicleFilePath.isEmpty()) { - return; - } - - _readFileDownloadDir.setPath(downloadDir.absolutePath()); - - // We need to strip off the file name from the fully qualified path. We can't use the usual QDir - // routines because this path does not exist locally. - int i; - for (i=vehicleFilePath.size()-1; i>=0; i--) { - if (vehicleFilePath[i] == '/') { - break; - } - } - i++; // move past slash - _readFileDownloadFilename = vehicleFilePath.right(vehicleFilePath.size() - i); - - _currentOperation = readFile ? kCOOpenRead : kCOOpenBurst; - - Request request; - request.hdr.session = 0; - request.hdr.opcode = kCmdOpenFileRO; - request.hdr.offset = 0; - request.hdr.size = 0; - _fillRequestWithString(&request, vehicleFilePath); - _sendRequest(&request); -} - -/// @brief Uploads the specified file. -/// @param toPath File in UAS to upload to, fully qualified path -/// @param uploadFile Local file to upload from -void MAVLinkFTPManager::uploadPath(const QString& toPath, const QFileInfo& uploadFile) -{ - if(_currentOperation != kCOIdle){ - _emitErrorMessage(tr("UAS File manager busy. Try again later")); - return; - } - - _dedicatedLink = _vehicle->priorityLink(); - if (!_dedicatedLink) { - _emitErrorMessage(tr("Command not sent. No Vehicle links.")); - return; - } - - if (toPath.isEmpty()) { - return; - } - - if (!uploadFile.isReadable()){ - _emitErrorMessage(tr("File (%1) is not readable for upload").arg(uploadFile.path())); - return; - } - - QFile file(uploadFile.absoluteFilePath()); - if (!file.open(QIODevice::ReadOnly)) { - _emitErrorMessage(tr("Unable to open local file for upload (%1)").arg(uploadFile.absoluteFilePath())); - return; - } - - _writeFileAccumulator = file.readAll(); - _writeFileSize = _writeFileAccumulator.size(); - - file.close(); - - if (_writeFileAccumulator.size() == 0) { - _emitErrorMessage(tr("Unable to read data from local file (%1)").arg(uploadFile.absoluteFilePath())); - return; - } - - _currentOperation = kCOCreate; - - Request request; - request.hdr.session = 0; - request.hdr.opcode = kCmdCreateFile; - request.hdr.offset = 0; - request.hdr.size = 0; - _fillRequestWithString(&request, toPath + "/" + uploadFile.fileName()); - _sendRequest(&request); -} - -void MAVLinkFTPManager::createDirectory(const QString& directory) -{ - if(_currentOperation != kCOIdle){ - _emitErrorMessage(tr("UAS File manager busy. Try again later")); - return; - } - - _currentOperation = kCOCreateDir; - - Request request; - request.hdr.session = 0; - request.hdr.opcode = kCmdCreateDirectory; - request.hdr.offset = 0; - request.hdr.size = 0; - _fillRequestWithString(&request, directory); - _sendRequest(&request); -} - -QString MAVLinkFTPManager::errorString(uint8_t errorCode) -{ - switch(errorCode) { - case kErrNone: - return QString("no error"); - case kErrFail: - return QString("unknown error"); - case kErrEOF: - return QString("read beyond end of file"); - case kErrUnknownCommand: - return QString("unknown command"); - case kErrFailErrno: - return QString("command failed"); - case kErrInvalidDataSize: - return QString("invalid data size"); - case kErrInvalidSession: - return QString("invalid session"); - case kErrNoSessionsAvailable: - return QString("no sessions available"); - case kErrFailFileExists: - return QString("File already exists on target"); - case kErrFailFileProtected: - return QString("File is write protected"); - default: - return QString("unknown error code"); - } -} - -/// @brief Sends a command which only requires an opcode and no additional data -/// @param opcode Opcode to send -/// @param newOpState State to put state machine into -/// @return TRUE: command sent, FALSE: command not sent, waiting for previous command to finish -bool MAVLinkFTPManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpState) -{ - if (_currentOperation != kCOIdle) { - // Can't have multiple commands in play at the same time - return false; - } - - Request request; - request.hdr.session = 0; - request.hdr.opcode = opcode; - request.hdr.offset = 0; - request.hdr.size = 0; - - _currentOperation = newOpState; - - _sendRequest(&request); - - return true; -} - -/// @brief Starts the ack timeout timer -void MAVLinkFTPManager::_setupAckTimeout(void) -{ - qCDebug(MAVLinkFTPManagerLog) << "_setupAckTimeout"; - - Q_ASSERT(!_ackTimer.isActive()); - - _ackNumTries = 0; - _ackTimer.setSingleShot(false); - _ackTimer.start(ackTimerTimeoutMsecs); -} - -/// @brief Clears the ack timeout timer -void MAVLinkFTPManager::_clearAckTimeout(void) -{ - qCDebug(MAVLinkFTPManagerLog) << "_clearAckTimeout"; - _ackTimer.stop(); -} - -/// @brief Called when ack timeout timer fires -void MAVLinkFTPManager::_ackTimeout(void) -{ - qCDebug(MAVLinkFTPManagerLog) << "_ackTimeout"; - - if (++_ackNumTries <= ackTimerMaxRetries) { - qCDebug(MAVLinkFTPManagerLog) << "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. - - switch (_currentOperation) { - case kCORead: - case kCOBurst: - _closeDownloadSession(false /* failure */); - _emitErrorMessage(tr("Timeout waiting for ack: Download failed")); - break; - - case kCOOpenRead: - case kCOOpenBurst: - _currentOperation = kCOIdle; - _emitErrorMessage(tr("Timeout waiting for ack: Download failed")); - _sendResetCommand(); - break; - - case kCOCreate: - _currentOperation = kCOIdle; - _emitErrorMessage(tr("Timeout waiting for ack: Upload failed")); - _sendResetCommand(); - break; - - case kCOWrite: - _closeUploadSession(false /* failure */); - _emitErrorMessage(tr("Timeout waiting for ack: Upload failed")); - break; - - default: - { - OperationState currentOperation = _currentOperation; - _currentOperation = kCOIdle; - _emitErrorMessage(QString("Timeout waiting for ack: Command failed (%1)").arg(currentOperation)); - } - break; - } -} - -void MAVLinkFTPManager::_sendResetCommand(void) -{ - Request request; - request.hdr.opcode = kCmdResetSessions; - request.hdr.size = 0; - _sendRequest(&request); -} - -void MAVLinkFTPManager::_emitErrorMessage(const QString& msg) -{ - qCDebug(MAVLinkFTPManagerLog) << "Error:" << msg; - emit commandError(msg); -} - -void MAVLinkFTPManager::_emitListEntry(const QString& entry) -{ - qCDebug(MAVLinkFTPManagerLog) << "_emitListEntry" << entry; - emit listEntry(entry); -} - -/// @brief Sends the specified Request out to the UAS. -void MAVLinkFTPManager::_sendRequest(Request* request) -{ - - _setupAckTimeout(); - - 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(MAVLinkFTPManagerLog) << "request length too long:" << request->hdr.size; - } - - qCDebug(MAVLinkFTPManagerLog) << "_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 MAVLinkFTPManager::_sendRequestNoAck(Request* request) -{ - mavlink_message_t message; - - // Unit testing code can end up here without _dedicateLink set since it tests inidividual commands. - LinkInterface* link; - if (_dedicatedLink) { - link = _dedicatedLink; - } else { - link = _vehicle->priorityLink(); - } - - mavlink_msg_file_transfer_protocol_pack_chan(_systemIdQGC, // QGC System ID - 0, // QGC Component ID - link->mavlinkChannel(), - &message, // Mavlink Message to pack into - 0, // Target network - _systemIdServer, // Target system - 0, // Target component - (uint8_t*)request); // Payload - - _vehicle->sendMessageOnLinkThreadSafe(link, message); -} diff --git a/src/Vehicle/MAVLinkFTPManager.h b/src/Vehicle/MAVLinkFTPManager.h deleted file mode 100644 index 4d971bc0b..000000000 --- a/src/Vehicle/MAVLinkFTPManager.h +++ /dev/null @@ -1,249 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2018 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -#pragma once - -#include -#include -#include -#include - -#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(MAVLinkFTPManagerLog) - -class Vehicle; - -class MAVLinkFTPManager : public QObject -{ - Q_OBJECT - -public: - MAVLinkFTPManager(Vehicle* vehicle); - - /// These methods are only used for testing purposes. - bool _sendCmdTestAck(void) { return _sendOpcodeOnlyCmd(kCmdNone, kCOAck); }; - bool _sendCmdTestNoAck(void) { return _sendOpcodeOnlyCmd(kCmdTestNoAck, kCOAck); }; - - /// 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 MAVLinkFTPManager to timeout. - static const int ackTimerTimeoutMsecs = 50; - - static const int ackTimerMaxRetries = 6; - - /// Downloads the specified file. - /// @param vehicleFilePath File to download from UAS, fully qualified path - /// @param downloadDir Local directory to download file to - void downloadPath(const QString& vehicleFilePath, const QDir& downloadDir); - - /// Stream downloads the specified file. - /// @param from File to download from UAS, fully qualified path - /// @param downloadDir Local directory to download file to - void streamPath(const QString& from, const QDir& downloadDir); - - /// Lists the specified directory. Emits listEntry signal for each entry, followed by listComplete signal. - /// @param dirPath Fully qualified path to list - void listDirectory(const QString& dirPath); - - /// 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 - - /// Signalled to indicate a new directory entry was received. - void listEntry(const QString& entry); - - // Signals associated with all commands - - /// Signalled after a command has completed - void commandComplete(void); - - /// Signalled when an error occurs during a command. In this case a commandComplete signal will - /// not be sent. - void commandError(const QString& msg); - - /// Signalled during a lengthy command to show progress - /// @param value Amount of progress: 0.0 = none, 1.0 = complete - void commandProgress(int value); - -public slots: - void receiveMessage(mavlink_message_t message); - -private slots: - void _ackTimeout(void); - -private: - /// @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 - uint8_t opcode; ///< Command opcode - uint8_t size; ///< Size of data - uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - 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; - - PACKED_STRUCT( - typedef struct _Request - { - RequestHeader hdr; - - // 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 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_file_transfer_protocol_t*)0)->payload) - sizeof(RequestHeader)]; - - // File length returned by Open command - uint32_t openFileLength; - - // Length of file chunk written by write command - uint32_t writeFileLength; - }; - }) Request; - - enum Opcode - { - kCmdNone, ///< ignored, always acked - kCmdTerminateSession, ///< Terminates open Read session - kCmdResetSessions, ///< Terminates all open Read sessions - kCmdListDirectory, ///< List files in from - kCmdOpenFileRO, ///< Opens file at for reading, returns - kCmdReadFile, ///< Reads bytes from in - kCmdCreateFile, ///< Creates file at for writing, returns - kCmdWriteFile, ///< Writes bytes to in - kCmdRemoveFile, ///< Remove file at - kCmdCreateDirectory, ///< Creates directory at - kCmdRemoveDirectory, ///< Removes Directory at , must be empty - kCmdOpenFileWO, ///< Opens file at for writing, returns - kCmdTruncateFile, ///< Truncate file at to length - kCmdRename, ///< Rename to - kCmdCalcFileCRC32, ///< Calculate CRC32 for file at - kCmdBurstReadFile, ///< Burst download session file - - kRspAck = 128, ///< Ack response - kRspNak, ///< Nak response - - // Used for testing only, not part of protocol - kCmdTestNoAck, ///< ignored, ack not sent back, should timeout waiting for ack - }; - - /// @brief Error codes returned in Nak response PayloadHeader.data[0]. - enum ErrorCode - { - kErrNone, - kErrFail, ///< Unknown failure - kErrFailErrno, ///< errno sent back in PayloadHeader.data[1] - kErrInvalidDataSize, ///< PayloadHeader.size is invalid - kErrInvalidSession, ///< Session is not currently open - kErrNoSessionsAvailable, ///< All available Sessions in use - kErrEOF, ///< Offset past end of file for List and Read commands - kErrUnknownCommand, ///< Unknown command opcode - kErrFailFileExists, ///< File exists already - kErrFailFileProtected ///< File is write protected - }; - - enum OperationState - { - kCOIdle, // not doing anything - kCOAck, // waiting for an Ack - kCOList, // waiting for List response - kCOOpenRead, // waiting for Open response followed by Read download - kCOOpenBurst, // waiting for Open response, followed by Burst download - kCORead, // waiting for Read response - 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); - void _setupAckTimeout(void); - void _clearAckTimeout(void); - 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); - void _listAckResponse(Request* listAck); - void _createAckResponse(Request* createAck); - void _writeAckResponse(Request* writeAck); - void _writeFileDatablock(void); - void _sendListCommand(void); - void _sendResetCommand(void); - void _closeDownloadSession(bool success); - void _closeUploadSession(bool success); - void _downloadWorker(const QString& vehicleFilePath, const QDir& downloadDir, bool readFile); - void _requestMissingData(); - - static QString errorString(uint8_t errorCode); - - OperationState _currentOperation = kCOIdle; ///< Current operation of state machine - QTimer _ackTimer; ///< Used to signal a timeout waiting for an ack - int _ackNumTries = 0; ///< current number of tries - - Vehicle* _vehicle = nullptr; - LinkInterface* _dedicatedLink = nullptr; ///< Link to use for communication - - Request _lastOutgoingRequest; ///< contains the last outgoing packet - - unsigned _listOffset; ///< offset for the current List operation - QString _listPath; ///< path for the current List operation - - uint8_t _activeSession = 0; ///< currently active session, 0 for none - - uint32_t _readOffset; ///< current read offset - - uint32_t _writeOffset; ///< current write offset - uint32_t _writeSize; ///< current write data size - 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 = 0; ///< number of missing bytes for burst download - QQueue _missingData; ///< missing chunks of downloaded file (for burst downloads) - bool _downloadingMissingParts = false; ///< 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 - uint32_t _downloadFileSize; ///< Size of file being downloaded - - uint8_t _systemIdQGC = 0; ///< System ID for QGC - uint8_t _systemIdServer = 0; ///< System ID for server - - // We give MockLinkFileServer friend access so that it can use the data structures and opcodes - // to build a mock mavlink file server for testing. - friend class MockLinkFileServer; -}; - diff --git a/src/Vehicle/StateMachine.h b/src/Vehicle/StateMachine.h index 1fda1ccfd..738c19576 100644 --- a/src/Vehicle/StateMachine.h +++ b/src/Vehicle/StateMachine.h @@ -9,8 +9,12 @@ #pragma once -class StateMachine +#include + +class StateMachine : public QObject { + Q_OBJECT + public: typedef void (*StateFn)(StateMachine* stateMachine); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index c70a1830a..2a7d1b493 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -45,7 +45,7 @@ #include "QGCGeo.h" #include "TerrainProtocolHandler.h" #include "ParameterManager.h" -#include "MAVLinkFTPManager.h" +#include "FTPManager.h" #include "ComponentInformationManager.h" #include "InitialConnectStateMachine.h" @@ -465,9 +465,9 @@ void Vehicle::_commonInit() _parameterManager = new ParameterManager(this); connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady); - _componentInformationManager = new ComponentInformationManager(this); - _initialConnectStateMachine = new InitialConnectStateMachine(this); - _mavlinkFTPManager = new MAVLinkFTPManager(this); + _componentInformationManager = new ComponentInformationManager (this); + _initialConnectStateMachine = new InitialConnectStateMachine (this); + _ftpManager = new FTPManager (this); _objectAvoidance = new VehicleObjectAvoidance(this, this); @@ -715,6 +715,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes if (!_terrainProtocolHandler->mavlinkMessageReceived(message)) { return; } + _ftpManager->mavlinkMessageReceived(message); _waitForMavlinkMessageMessageReceived(message); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 36124d1d0..1229c67da 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -44,7 +44,7 @@ class VehicleObjectAvoidance; class TrajectoryPoints; class TerrainProtocolHandler; class ComponentInformationManager; -class MAVLinkFTPManager; +class FTPManager; class InitialConnectStateMachine; #if defined(QGC_AIRMAP_ENABLED) @@ -1009,7 +1009,7 @@ public: ParameterManager* parameterManager () { return _parameterManager; } ParameterManager* parameterManager () const { return _parameterManager; } - MAVLinkFTPManager* mavlinkFTPManager () { return _mavlinkFTPManager; } + FTPManager* ftpManager () { return _ftpManager; } VehicleObjectAvoidance* objectAvoidance () { return _objectAvoidance; } static const int cMaxRcChannels = 18; @@ -1460,7 +1460,7 @@ private: RallyPointManager* _rallyPointManager; ParameterManager* _parameterManager = nullptr; - MAVLinkFTPManager* _mavlinkFTPManager = nullptr; + FTPManager* _ftpManager = nullptr; ComponentInformationManager* _componentInformationManager = nullptr; InitialConnectStateMachine* _initialConnectStateMachine = nullptr; VehicleObjectAvoidance* _objectAvoidance = nullptr; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 41b9043e2..c19dc3f96 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -96,7 +96,7 @@ MockLink::MockLink(SharedLinkConfigurationPointer& config) px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; _mavCustomMode = px4_cm.data; - _fileServer = new MockLinkFileServer(_vehicleSystemId, _vehicleComponentId, this); + _fileServer = new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this); Q_CHECK_PTR(_fileServer); moveToThread(this); @@ -1532,7 +1532,7 @@ bool MockLink::_handleRequestMessage(const mavlink_command_long_t& request, bool void MockLink::_sendVersionMetaData(void) { mavlink_message_t responseMsg; - char metaDataURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN] = "mavlinkftp://version.json"; + char metaDataURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN] = "mavlinkftp://version.json"; char translationURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_TRANSLATION_URI_LEN] = ""; mavlink_msg_component_information_pack_chan(_vehicleSystemId, @@ -1551,7 +1551,7 @@ void MockLink::_sendVersionMetaData(void) void MockLink::_sendParameterMetaData(void) { mavlink_message_t responseMsg; - char metaDataURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN] = "mavlinkftp://parameter.json"; + char metaDataURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN] = "mavlinkftp://parameter.json"; char translationURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_TRANSLATION_URI_LEN] = ""; mavlink_msg_component_information_pack_chan(_vehicleSystemId, diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 226e417ed..608055d4b 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -16,7 +16,7 @@ #include #include "MockLinkMissionItemHandler.h" -#include "MockLinkFileServer.h" +#include "MockLinkFTP.h" #include "LinkManager.h" #include "QGCMAVLink.h" @@ -119,7 +119,7 @@ public: /// Sends the specified mavlink message to QGC void respondWithMavlinkMessage(const mavlink_message_t& msg); - MockLinkFileServer* getFileServer(void) { return _fileServer; } + MockLinkFTP* getFileServer(void) { return _fileServer; } // Overrides from LinkInterface QString getName (void) const override { return _name; } @@ -266,7 +266,7 @@ private: double _vehicleLongitude; double _vehicleAltitude; - MockLinkFileServer* _fileServer; + MockLinkFTP* _fileServer; bool _sendStatusText; bool _apmSendHomePositionOnEmptyList; diff --git a/src/comm/MockLinkFileServer.cc b/src/comm/MockLinkFTP.cc similarity index 51% rename from src/comm/MockLinkFileServer.cc rename to src/comm/MockLinkFTP.cc index a72abd871..52ffafa05 100644 --- a/src/comm/MockLinkFileServer.cc +++ b/src/comm/MockLinkFTP.cc @@ -8,43 +8,36 @@ ****************************************************************************/ -#include "MockLinkFileServer.h" +#include "MockLinkFTP.h" #include "MockLink.h" -const MockLinkFileServer::ErrorMode_t MockLinkFileServer::rgFailureModes[] = { - MockLinkFileServer::errModeNoResponse, - MockLinkFileServer::errModeNakResponse, - MockLinkFileServer::errModeNoSecondResponse, - MockLinkFileServer::errModeNakSecondResponse, - MockLinkFileServer::errModeBadSequence, +const MockLinkFTP::ErrorMode_t MockLinkFTP::rgFailureModes[] = { + MockLinkFTP::errModeNoResponse, + MockLinkFTP::errModeNakResponse, + MockLinkFTP::errModeNoSecondResponse, + MockLinkFTP::errModeNakSecondResponse, + MockLinkFTP::errModeBadSequence, }; -const size_t MockLinkFileServer::cFailureModes = sizeof(MockLinkFileServer::rgFailureModes) / sizeof(MockLinkFileServer::rgFailureModes[0]); +const size_t MockLinkFTP::cFailureModes = sizeof(MockLinkFTP::rgFailureModes) / sizeof(MockLinkFTP::rgFailureModes[0]); -const MockLinkFileServer::FileTestCase MockLinkFileServer::rgFileTestCases[MockLinkFileServer::cFileTestCases] = { +const MockLinkFTP::FileTestCase MockLinkFTP::rgFileTestCases[MockLinkFTP::cFileTestCases] = { // File fits one Read Ack packet, partially filling data - { "partial.qgc", sizeof(((FileManager::Request*)0)->data) - 1, 1, false}, + { "partial.qgc", sizeof(((MavlinkFTP::Request*)0)->data) - 1, 1, false}, // File fits one Read Ack packet, exactly filling all data - { "exact.qgc", sizeof(((FileManager::Request*)0)->data), 1, true }, + { "exact.qgc", sizeof(((MavlinkFTP::Request*)0)->data), 1, true }, // File is larger than a single Read Ack packets, requires multiple Reads - { "multi.qgc", sizeof(((FileManager::Request*)0)->data) + 1, 2, false }, + { "multi.qgc", sizeof(((MavlinkFTP::Request*)0)->data) + 1, 2, false }, }; -// We only support a single fixed session -const uint8_t MockLinkFileServer::_sessionId = 1; - -MockLinkFileServer::MockLinkFileServer(uint8_t systemIdServer, uint8_t componentIdServer, MockLink* mockLink) : - _errMode(errModeNone), - _systemIdServer(systemIdServer), - _componentIdServer(componentIdServer), - _mockLink(mockLink), - _lastReplyValid(false), - _lastReplySequence(0), - _randomDropsEnabled(false) +MockLinkFTP::MockLinkFTP(uint8_t systemIdServer, uint8_t componentIdServer, MockLink* mockLink) + : _systemIdServer (systemIdServer) + , _componentIdServer(componentIdServer) + , _mockLink (mockLink) { srand(0); // make sure unit tests are deterministic } -void MockLinkFileServer::ensureNullTemination(FileManager::Request* request) +void MockLinkFTP::ensureNullTemination(MavlinkFTP::Request* request) { if (request->hdr.size < sizeof(request->data)) { request->data[request->hdr.size] = '\0'; @@ -56,11 +49,11 @@ void MockLinkFileServer::ensureNullTemination(FileManager::Request* request) /// @brief Handles List command requests. Only supports root folder paths. /// File list returned is set using the setFileList method. -void MockLinkFileServer::_listCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber) +void MockLinkFTP::_listCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) { // FIXME: Does not support directories that span multiple packets - FileManager::Request ackResponse; + MavlinkFTP::Request ackResponse; QString path; uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); @@ -69,18 +62,18 @@ void MockLinkFileServer::_listCommand(uint8_t senderSystemId, uint8_t senderComp // We only support root path path = (char *)&request->data[0]; if (!path.isEmpty() && path != "/") { - _sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdListDirectory); + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdListDirectory); return; } // Offset requested is past the end of the list if (request->hdr.offset > (uint32_t)_fileList.size()) { - _sendNak(senderSystemId, senderComponentId, FileManager::kErrEOF, outgoingSeqNumber, FileManager::kCmdListDirectory); + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrEOF, outgoingSeqNumber, MavlinkFTP::kCmdListDirectory); return; } - ackResponse.hdr.opcode = FileManager::kRspAck; - ackResponse.hdr.req_opcode = FileManager::kCmdListDirectory; + ackResponse.hdr.opcode = MavlinkFTP::kRspAck; + ackResponse.hdr.req_opcode = MavlinkFTP::kCmdListDirectory; ackResponse.hdr.session = 0; ackResponse.hdr.offset = request->hdr.offset; ackResponse.hdr.size = 0; @@ -100,23 +93,23 @@ void MockLinkFileServer::_listCommand(uint8_t senderSystemId, uint8_t senderComp _sendResponse(senderSystemId, senderComponentId, &ackResponse, outgoingSeqNumber); } else if (_errMode == errModeNakSecondResponse) { // Nak error all subsequent requests - _sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdListDirectory); + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdListDirectory); return; } else if (_errMode == errModeNoSecondResponse) { // No response for all subsequent requests return; } else { // FIXME: Does not support directories that span multiple packets - _sendNak(senderSystemId, senderComponentId, FileManager::kErrEOF, outgoingSeqNumber, FileManager::kCmdListDirectory); + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrEOF, outgoingSeqNumber, MavlinkFTP::kCmdListDirectory); } } -/// @brief Handles Open command requests. -void MockLinkFileServer::_openCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber) +void MockLinkFTP::_openCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) { - FileManager::Request response; - QString path; - uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); + MavlinkFTP::Request response; + QString path; + uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); + QString tmpFilename; ensureNullTemination(request); @@ -124,58 +117,62 @@ void MockLinkFileServer::_openCommand(uint8_t senderSystemId, uint8_t senderComp Q_ASSERT(cchPath != sizeof(request->data)); Q_UNUSED(cchPath); // Fix initialized-but-not-referenced warning on release builds path = (char *)request->data; + + _currentFile.close(); // Check path against one of our known test cases - - bool found = false; - for (size_t i=0; ihdr.session != _sessionId) { - _sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdReadFile); + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrInvalidSession, outgoingSeqNumber, MavlinkFTP::kCmdReadFile); return; } uint32_t readOffset = request->hdr.offset; // offset into file for reading - uint8_t cDataBytes = 0; // current number of data bytes used - + if (readOffset != 0) { // 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(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdReadFile); + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdReadFile); return; } else if (_errMode == errModeNoSecondResponse) { // No rsponse for all subsequent requests @@ -183,35 +180,35 @@ void MockLinkFileServer::_readCommand(uint8_t senderSystemId, uint8_t senderComp } } - if (readOffset >= _readFileLength) { - _sendNak(senderSystemId, senderComponentId, FileManager::kErrEOF, outgoingSeqNumber, FileManager::kCmdReadFile); + if (readOffset >= _currentFile.size()) { + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrEOF, outgoingSeqNumber, MavlinkFTP::kCmdReadFile); return; } - // Write file bytes. Data is a repeating sequence of 0x00, 0x01, .. 0xFF. - for (; cDataBytes < sizeof(response.data) && readOffset < _readFileLength; readOffset++, cDataBytes++) { - response.data[cDataBytes] = readOffset & 0xFF; - } + uint8_t cBytesToRead = (uint8_t)qMin((qint64)sizeof(response.data), _currentFile.size() - readOffset); + _currentFile.seek(readOffset); + QByteArray bytes = _currentFile.read(cBytesToRead); + memcpy(response.data, bytes.constData(), cBytesToRead); // We should always have written something, otherwise there is something wrong with the code above - Q_ASSERT(cDataBytes); + Q_ASSERT(cBytesToRead); - response.hdr.session = _sessionId; - response.hdr.size = cDataBytes; - response.hdr.offset = request->hdr.offset; - response.hdr.opcode = FileManager::kRspAck; - response.hdr.req_opcode = FileManager::kCmdReadFile; + response.hdr.session = _sessionId; + response.hdr.size = cBytesToRead; + response.hdr.offset = request->hdr.offset; + response.hdr.opcode = MavlinkFTP::kRspAck; + response.hdr.req_opcode = MavlinkFTP::kCmdReadFile; _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber); } -void MockLinkFileServer::_streamCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber) +void MockLinkFTP::_burstReadCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) { uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); - FileManager::Request response; + MavlinkFTP::Request response; if (request->hdr.session != _sessionId) { - _sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdBurstReadFile); + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdBurstReadFile); return; } @@ -219,14 +216,14 @@ void MockLinkFileServer::_streamCommand(uint8_t senderSystemId, uint8_t senderCo uint32_t ackOffset = 0; // offset for ack uint8_t cDataAck; // number of bytes in ack - while (readOffset < _readFileLength) { + while (readOffset < _currentFile.size()) { cDataAck = 0; if (readOffset != 0) { // 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(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdBurstReadFile); + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdBurstReadFile); return; } else if (_errMode == errModeNoSecondResponse) { // No response for all subsequent requests @@ -234,19 +231,19 @@ void MockLinkFileServer::_streamCommand(uint8_t senderSystemId, uint8_t senderCo } } - // Write file bytes. Data is a repeating sequence of 0x00, 0x01, .. 0xFF. - for (; cDataAck < sizeof(response.data) && readOffset < _readFileLength; readOffset++, cDataAck++) { - response.data[cDataAck] = readOffset & 0xFF; - } - + uint8_t cBytesToRead = (uint8_t)qMin((qint64)sizeof(response.data), _currentFile.size()); + _currentFile.seek(readOffset); + QByteArray bytes = _currentFile.read(cBytesToRead); + memcpy(response.data, bytes.constData(), cBytesToRead); + // We should always have written something, otherwise there is something wrong with the code above - Q_ASSERT(cDataAck); + Q_ASSERT(cBytesToRead); response.hdr.session = _sessionId; response.hdr.size = cDataAck; - response.hdr.offset = ackOffset; - response.hdr.opcode = FileManager::kRspAck; - response.hdr.req_opcode = FileManager::kCmdBurstReadFile; + response.hdr.offset = cBytesToRead; + response.hdr.opcode = MavlinkFTP::kRspAck; + response.hdr.req_opcode = MavlinkFTP::kCmdBurstReadFile; _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber); @@ -254,39 +251,39 @@ void MockLinkFileServer::_streamCommand(uint8_t senderSystemId, uint8_t senderCo ackOffset += cDataAck; } - _sendNak(senderSystemId, senderComponentId, FileManager::kErrEOF, outgoingSeqNumber, FileManager::kCmdBurstReadFile); + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrEOF, outgoingSeqNumber, MavlinkFTP::kCmdBurstReadFile); } -void MockLinkFileServer::_terminateCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber) +void MockLinkFTP::_terminateCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) { uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); if (request->hdr.session != _sessionId) { - _sendNak(senderSystemId, senderComponentId, FileManager::kErrInvalidSession, outgoingSeqNumber, FileManager::kCmdTerminateSession); + _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrInvalidSession, outgoingSeqNumber, MavlinkFTP::kCmdTerminateSession); return; } - _sendAck(senderSystemId, senderComponentId, outgoingSeqNumber, FileManager::kCmdTerminateSession); + _sendAck(senderSystemId, senderComponentId, outgoingSeqNumber, MavlinkFTP::kCmdTerminateSession); emit terminateCommandReceived(); } -void MockLinkFileServer::_resetCommand(uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber) +void MockLinkFTP::_resetCommand(uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber) { uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); - _sendAck(senderSystemId, senderComponentId, outgoingSeqNumber, FileManager::kCmdResetSessions); + _sendAck(senderSystemId, senderComponentId, outgoingSeqNumber, MavlinkFTP::kCmdResetSessions); emit resetCommandReceived(); } -void MockLinkFileServer::handleFTPMessage(const mavlink_message_t& message) +void MockLinkFTP::handleFTPMessage(const mavlink_message_t& message) { if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { return; } - FileManager::Request ackResponse; + MavlinkFTP::Request ackResponse; mavlink_file_transfer_protocol_t requestFTP; mavlink_msg_file_transfer_protocol_decode(&message, &requestFTP); @@ -295,7 +292,7 @@ void MockLinkFileServer::handleFTPMessage(const mavlink_message_t& message) return; } - FileManager::Request* request = (FileManager::Request*)&requestFTP.payload[0]; + MavlinkFTP::Request* request = (MavlinkFTP::Request*)&requestFTP.payload[0]; if (_randomDropsEnabled) { if (rand() % 3 == 0) { @@ -315,67 +312,63 @@ void MockLinkFileServer::handleFTPMessage(const mavlink_message_t& message) uint16_t incomingSeqNumber = request->hdr.seqNumber; uint16_t outgoingSeqNumber = _nextSeqNumber(incomingSeqNumber); - if (request->hdr.opcode != FileManager::kCmdResetSessions && request->hdr.opcode != FileManager::kCmdTerminateSession) { + if (request->hdr.opcode != MavlinkFTP::kCmdResetSessions && request->hdr.opcode != MavlinkFTP::kCmdTerminateSession) { 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(message.sysid, message.compid, FileManager::kErrFail, outgoingSeqNumber, (FileManager::Opcode)request->hdr.opcode); + _sendNak(message.sysid, message.compid, MavlinkFTP::kErrFail, outgoingSeqNumber, (MavlinkFTP::OpCode_t)request->hdr.opcode); return; } } switch (request->hdr.opcode) { - case FileManager::kCmdTestNoAck: - // ignored, ack not sent back, for testing only - break; - - case FileManager::kCmdNone: + case MavlinkFTP::kCmdNone: // ignored, always acked - ackResponse.hdr.opcode = FileManager::kRspAck; + ackResponse.hdr.opcode = MavlinkFTP::kRspAck; ackResponse.hdr.session = 0; ackResponse.hdr.size = 0; _sendResponse(message.sysid, message.compid, &ackResponse, outgoingSeqNumber); break; - case FileManager::kCmdListDirectory: + case MavlinkFTP::kCmdListDirectory: _listCommand(message.sysid, message.compid, request, incomingSeqNumber); break; - case FileManager::kCmdOpenFileRO: + case MavlinkFTP::kCmdOpenFileRO: _openCommand(message.sysid, message.compid, request, incomingSeqNumber); break; - case FileManager::kCmdReadFile: + case MavlinkFTP::kCmdReadFile: _readCommand(message.sysid, message.compid, request, incomingSeqNumber); break; - case FileManager::kCmdBurstReadFile: - _streamCommand(message.sysid, message.compid, request, incomingSeqNumber); + case MavlinkFTP::kCmdBurstReadFile: + _burstReadCommand(message.sysid, message.compid, request, incomingSeqNumber); break; - case FileManager::kCmdTerminateSession: + case MavlinkFTP::kCmdTerminateSession: _terminateCommand(message.sysid, message.compid, request, incomingSeqNumber); break; - case FileManager::kCmdResetSessions: + case MavlinkFTP::kCmdResetSessions: _resetCommand(message.sysid, message.compid, incomingSeqNumber); break; default: // nack for all NYI opcodes - _sendNak(message.sysid, message.compid, FileManager::kErrUnknownCommand, outgoingSeqNumber, (FileManager::Opcode)request->hdr.opcode); + _sendNak(message.sysid, message.compid, MavlinkFTP::kErrUnknownCommand, outgoingSeqNumber, (MavlinkFTP::OpCode_t)request->hdr.opcode); break; } } /// @brief Sends an Ack -void MockLinkFileServer::_sendAck(uint8_t targetSystemId, uint8_t targetComponentId, uint16_t seqNumber, FileManager::Opcode reqOpcode) +void MockLinkFTP::_sendAck(uint8_t targetSystemId, uint8_t targetComponentId, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpcode) { - FileManager::Request ackResponse; + MavlinkFTP::Request ackResponse; - ackResponse.hdr.opcode = FileManager::kRspAck; + ackResponse.hdr.opcode = MavlinkFTP::kRspAck; ackResponse.hdr.req_opcode = reqOpcode; ackResponse.hdr.session = 0; ackResponse.hdr.size = 0; @@ -383,35 +376,48 @@ void MockLinkFileServer::_sendAck(uint8_t targetSystemId, uint8_t targetComponen _sendResponse(targetSystemId, targetComponentId, &ackResponse, seqNumber); } -/// @brief Sends a Nak with the specified error code. -void MockLinkFileServer::_sendNak(uint8_t targetSystemId, uint8_t targetComponentId, FileManager::ErrorCode error, uint16_t seqNumber, FileManager::Opcode reqOpcode) +void MockLinkFTP::_sendNak(uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::ErrorCode_t error, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpcode) { - FileManager::Request nakResponse; + MavlinkFTP::Request nakResponse; - nakResponse.hdr.opcode = FileManager::kRspNak; - nakResponse.hdr.req_opcode = reqOpcode; - nakResponse.hdr.session = 0; - nakResponse.hdr.size = 1; - nakResponse.data[0] = error; + nakResponse.hdr.opcode = MavlinkFTP::kRspNak; + nakResponse.hdr.req_opcode = reqOpcode; + nakResponse.hdr.session = 0; + nakResponse.hdr.size = 1; + nakResponse.data[0] = error; _sendResponse(targetSystemId, targetComponentId, &nakResponse, seqNumber); } +void MockLinkFTP::_sendNakErrno(uint8_t targetSystemId, uint8_t targetComponentId, uint8_t nakErrno, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpcode) +{ + MavlinkFTP::Request nakResponse; + + nakResponse.hdr.opcode = MavlinkFTP::kRspNak; + nakResponse.hdr.req_opcode = reqOpcode; + nakResponse.hdr.session = 0; + nakResponse.hdr.size = 2; + nakResponse.data[0] = MavlinkFTP::kErrFailErrno; + nakResponse.data[1] = nakErrno; + + _sendResponse(targetSystemId, targetComponentId, &nakResponse, seqNumber); +} + /// @brief Emits a Request through the messageReceived signal. -void MockLinkFileServer::_sendResponse(uint8_t targetSystemId, uint8_t targetComponentId, FileManager::Request* request, uint16_t seqNumber) +void MockLinkFTP::_sendResponse(uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) { request->hdr.seqNumber = seqNumber; _lastReplySequence = seqNumber; _lastReplyValid = true; - mavlink_msg_file_transfer_protocol_pack_chan(_systemIdServer, // System ID - 0, // Component ID + mavlink_msg_file_transfer_protocol_pack_chan(_systemIdServer, // System ID + _componentIdServer, // Component ID _mockLink->mavlinkChannel(), - &_lastReply, // Mavlink Message to pack into - 0, // Target network + &_lastReply, // Mavlink Message to pack into + 0, // Target network targetSystemId, targetComponentId, - (uint8_t*)request); // Payload + (uint8_t*)request); // Payload if (_randomDropsEnabled) { if (rand() % 3 == 0) { @@ -425,7 +431,7 @@ void MockLinkFileServer::_sendResponse(uint8_t targetSystemId, uint8_t targetCom /// @brief Generates the next sequence number given an incoming sequence number. Handles generating /// bad sequence numbers when errModeBadSequence is set. -uint16_t MockLinkFileServer::_nextSeqNumber(uint16_t seqNumber) +uint16_t MockLinkFTP::_nextSeqNumber(uint16_t seqNumber) { uint16_t outgoingSeqNumber = seqNumber + 1; @@ -434,3 +440,14 @@ uint16_t MockLinkFileServer::_nextSeqNumber(uint16_t seqNumber) } return outgoingSeqNumber; } + +QString MockLinkFTP::_createTestCaseTempFile(const FileTestCase& testCase) +{ + QGCTemporaryFile tmpFile("MockLinkFTPTestCase"); + tmpFile.open(QIODevice::WriteOnly | QIODevice::Truncate); + for (int i=0; i - #pragma once -#include "FileManager.h" +#include "QGCMAVLink.h" #include +#include class MockLink; /// Mock implementation of Mavlink FTP server. -class MockLinkFileServer : public QObject +class MockLinkFTP : public QObject { Q_OBJECT public: - MockLinkFileServer(uint8_t systemIdServer, uint8_t componentIdServer, MockLink* mockLink); + MockLinkFTP(uint8_t systemIdServer, uint8_t componentIdServer, MockLink* mockLink); /// @brief Sets the list of files returned by the List command. Prepend names with F or D /// to indicate (F)ile or (D)irectory. @@ -54,7 +52,7 @@ public: /// Called to handle an FTP message void handleFTPMessage(const mavlink_message_t& message); - + /// @brief Used to represent a single test case for download testing. struct FileTestCase { const char* filename; ///< Filename to download @@ -79,33 +77,34 @@ signals: void resetCommandReceived(void); private: - void _sendAck(uint8_t targetSystemId, uint8_t targetComponentId, uint16_t seqNumber, FileManager::Opcode reqOpcode); - void _sendNak(uint8_t targetSystemId, uint8_t targetComponentId, FileManager::ErrorCode error, uint16_t seqNumber, FileManager::Opcode reqOpcode); - void _sendResponse(uint8_t targetSystemId, uint8_t targetComponentId, FileManager::Request* request, uint16_t seqNumber); - void _listCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber); - void _openCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber); - void _readCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber); - void _streamCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber); - void _terminateCommand(uint8_t senderSystemId, uint8_t senderComponentId, FileManager::Request* request, uint16_t seqNumber); - void _resetCommand(uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber); - uint16_t _nextSeqNumber(uint16_t seqNumber); + void _sendAck (uint8_t targetSystemId, uint8_t targetComponentId, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpCode); + void _sendNak (uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::ErrorCode_t error, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpCode); + void _sendNakErrno (uint8_t targetSystemId, uint8_t targetComponentId, uint8_t nakErrno, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpCode); + void _sendResponse (uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); + void _listCommand (uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); + void _openCommand (uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); + void _readCommand (uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); + void _burstReadCommand (uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); + void _terminateCommand (uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); + void _resetCommand (uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber); + uint16_t _nextSeqNumber (uint16_t seqNumber); + QString _createTestCaseTempFile (const FileTestCase& testCase); /// if request is a string, this ensures it's null-terminated - static void ensureNullTemination(FileManager::Request* request); + static void ensureNullTemination(MavlinkFTP::Request* request); QStringList _fileList; ///< List of files returned by List command - 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 _componentIdServer; ///< Component ID for server - MockLink* _mockLink; ///< MockLink to communicate through - - bool _lastReplyValid; - uint16_t _lastReplySequence; - mavlink_message_t _lastReply; + QFile _currentFile; + ErrorMode_t _errMode = errModeNone; ///< Currently set error mode, as specified by setErrorMode + const uint8_t _systemIdServer; ///< System ID for server + const uint8_t _componentIdServer; ///< Component ID for server + MockLink* _mockLink; ///< MockLink to communicate through + bool _lastReplyValid = false; + uint16_t _lastReplySequence = 0; + mavlink_message_t _lastReply; + bool _randomDropsEnabled = false; - bool _randomDropsEnabled; + static const uint8_t _sessionId = 1; ///< We only support a single fixed session }; diff --git a/src/comm/QGCMAVLink.cc b/src/comm/QGCMAVLink.cc index c791df4e6..33201be49 100644 --- a/src/comm/QGCMAVLink.cc +++ b/src/comm/QGCMAVLink.cc @@ -80,3 +80,77 @@ QString QGCMAVLink::mavResultToString(MAV_RESULT result) return QStringLiteral("MAV_RESULT unknown %1").arg(result); } } + +QString MavlinkFTP::opCodeToString(OpCode_t opCode) +{ + switch (opCode) { + case kCmdNone: + return "None"; + case kCmdTerminateSession: + return "Terminate Session"; + case kCmdResetSessions: + return "Reset Sessions"; + case kCmdListDirectory: + return "List Directory"; + case kCmdOpenFileRO: + return "Open File RO"; + case kCmdReadFile: + return "Read File"; + case kCmdCreateFile: + return "Create File"; + case kCmdWriteFile: + return "Write File"; + case kCmdRemoveFile: + return "Remove File"; + case kCmdCreateDirectory: + return "Create Directory"; + case kCmdRemoveDirectory: + return "Remove Directory"; + case kCmdOpenFileWO: + return "Open File WO"; + case kCmdTruncateFile: + return "Truncate File"; + case kCmdRename: + return "Rename"; + case kCmdCalcFileCRC32: + return "Calc File CRC32"; + case kCmdBurstReadFile: + return "Burst Read File"; + case kRspAck: + return "Ack"; + case kRspNak: + return "Nak"; + } + + return "Unknown OpCode"; +} + +QString MavlinkFTP::errorCodeToString(ErrorCode_t errorCode) +{ + switch (errorCode) { + case kErrNone: + return "None"; + case kErrFail: + return "Fail"; + case kErrFailErrno: + return "Fail Errorno"; + case kErrInvalidDataSize: + return "Invalid Data Size"; + case kErrInvalidSession: + return "Invalid Session"; + case kErrNoSessionsAvailable: + return "No Sessions Available"; + case kErrEOF: + return "EOF"; + case kErrUnknownCommand: + return "Unknown Command"; + case kErrFailFileExists: + return "File Already Exists"; + case kErrFailFileProtected: + return "File Protected"; + case kErrFailFileNotFound: + return "File Not Found"; + } + + return "Unknown Error"; +} diff --git a/src/comm/QGCMAVLink.h b/src/comm/QGCMAVLink.h index e23eea483..608bf3f14 100644 --- a/src/comm/QGCMAVLink.h +++ b/src/comm/QGCMAVLink.h @@ -40,6 +40,12 @@ extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; #pragma warning(pop, 0) #endif +#ifdef __GNUC__ +#define PACKED_STRUCT( __Declaration__ ) __Declaration__ __attribute__((packed)) +#else +#define PACKED_STRUCT( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) ) +#endif + class QGCMAVLink { public: static bool isFixedWing (MAV_TYPE mavType); @@ -50,4 +56,78 @@ public: static QString mavResultToString (MAV_RESULT result); }; +class MavlinkFTP { +public: + /// 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 + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message + uint8_t burstComplete; ///< Only used if req_opcode=kCmdBurstReadFile - 1: set of burst packets complete, 0: More burst packets coming. + uint8_t paddng; ///< 32 bit aligment padding + uint32_t offset; ///< Offsets for List and Read commands + }) RequestHeader; + + PACKED_STRUCT( + typedef struct Request{ + RequestHeader hdr; + + // 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 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_file_transfer_protocol_t*)0)->payload) - sizeof(RequestHeader)]; + // File length returned by Open command + uint32_t openFileLength; + + // Length of file chunk written by write command + uint32_t writeFileLength; + }; + }) Request; + + typedef enum { + kCmdNone = 0, ///< ignored, always acked + kCmdTerminateSession, ///< Terminates open Read session + kCmdResetSessions, ///< Terminates all open Read sessions + kCmdListDirectory, ///< List files in from + kCmdOpenFileRO, ///< Opens file at for reading, returns + kCmdReadFile, ///< Reads bytes from in + kCmdCreateFile, ///< Creates file at for writing, returns + kCmdWriteFile, ///< Writes bytes to in + kCmdRemoveFile, ///< Remove file at + kCmdCreateDirectory, ///< Creates directory at + kCmdRemoveDirectory, ///< Removes Directory at , must be empty + kCmdOpenFileWO, ///< Opens file at for writing, returns + kCmdTruncateFile, ///< Truncate file at to length + kCmdRename, ///< Rename to + kCmdCalcFileCRC32, ///< Calculate CRC32 for file at + kCmdBurstReadFile, ///< Burst download session file + + kRspAck = 128, ///< Ack response + kRspNak, ///< Nak response + } OpCode_t; + + /// @brief Error codes returned in Nak response PayloadHeader.data[0]. + typedef enum { + kErrNone = 0, + kErrFail, ///< Unknown failure + kErrFailErrno, ///< errno sent back in PayloadHeader.data[1] + kErrInvalidDataSize, ///< PayloadHeader.size is invalid + kErrInvalidSession, ///< Session is not currently open + kErrNoSessionsAvailable, ///< All available Sessions in use + kErrEOF, ///< Offset past end of file for List and Read commands + kErrUnknownCommand, ///< Unknown command opcode + kErrFailFileExists, ///< File exists already + kErrFailFileProtected, ///< File is write protected + kErrFailFileNotFound + } ErrorCode_t; + + static QString opCodeToString (OpCode_t opCode); + static QString errorCodeToString(ErrorCode_t errorCode); +}; diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc index b915e544e..86139c251 100644 --- a/src/qgcunittest/UnitTestList.cc +++ b/src/qgcunittest/UnitTestList.cc @@ -46,6 +46,8 @@ #include "CameraCalcTest.h" #include "FWLandingPatternTest.h" #include "RequestMessageTest.h" +#include "InitialConnectTest.h" +#include "FTPManagerTest.h" UT_REGISTER_TEST(FactSystemTestGeneric) UT_REGISTER_TEST(FactSystemTestPX4) @@ -56,6 +58,8 @@ UT_REGISTER_TEST(LinkManagerTest) UT_REGISTER_TEST(SendMavCommandWithSignallingTest) UT_REGISTER_TEST(SendMavCommandWithHandlerTest) UT_REGISTER_TEST(RequestMessageTest) +UT_REGISTER_TEST(FTPManagerTest) +UT_REGISTER_TEST(InitialConnectTest) UT_REGISTER_TEST(MissionItemTest) UT_REGISTER_TEST(SimpleMissionItemTest) UT_REGISTER_TEST(MissionControllerTest) diff --git a/src/uas/FileManager.cc b/src/uas/FileManager.cc deleted file mode 100644 index 84b94a8ed..000000000 --- a/src/uas/FileManager.cc +++ /dev/null @@ -1,895 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -#include "FileManager.h" -#include "QGC.h" -#include "MAVLinkProtocol.h" -#include "Vehicle.h" -#include "QGCApplication.h" - -#include -#include -#include - -QGC_LOGGING_CATEGORY(FileManagerLog, "FileManagerLog") - -FileManager::FileManager(QObject* parent, Vehicle* vehicle) - : QObject(parent) - , _currentOperation(kCOIdle) - , _vehicle(vehicle) - , _dedicatedLink(nullptr) - , _activeSession(0) - , _missingDownloadedBytes(0) - , _downloadingMissingParts(false) - , _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 - Q_ASSERT(sizeof(RequestHeader) == 12); -} - -/// Respond to the Ack associated with the Open command with the next read command. -void FileManager::_openAckResponse(Request* openAck) -{ - qCDebug(FileManagerLog) << QString("_openAckResponse: _currentOperation(%1) _readFileLength(%2)").arg(_currentOperation).arg(openAck->openFileLength); - - Q_ASSERT(_currentOperation == kCOOpenRead || _currentOperation == kCOOpenBurst); - _currentOperation = _currentOperation == kCOOpenRead ? kCORead : kCOBurst; - _activeSession = openAck->hdr.session; - - // File length comes back in data - Q_ASSERT(openAck->hdr.size == sizeof(uint32_t)); - _downloadFileSize = openAck->openFileLength; - - // Start the sequence of read commands - - _downloadOffset = 0; // Start reading at beginning of file - _readFileAccumulator.clear(); // Start with an empty file - _missingDownloadedBytes = 0; - _downloadingMissingParts = false; - _missingData.clear(); - - Request request; - request.hdr.session = _activeSession; - Q_ASSERT(_currentOperation == kCORead || _currentOperation == kCOBurst); - request.hdr.opcode = _currentOperation == kCORead ? kCmdReadFile : kCmdBurstReadFile; - request.hdr.offset = _downloadOffset; - request.hdr.size = sizeof(request.data); - - _sendRequest(&request); -} - -/// request the next missing part of a (partially) downloaded file -void FileManager::_requestMissingData() -{ - if (_missingData.empty()) { - _downloadingMissingParts = false; - _missingDownloadedBytes = 0; - // there might be more data missing at the end - if ((uint32_t)_readFileAccumulator.length() != _downloadFileSize) { - _downloadOffset = _readFileAccumulator.length(); - qCDebug(FileManagerLog) << QString("_requestMissingData: missing parts done, downloadOffset(%1) downloadFileSize(%2)") - .arg(_downloadOffset).arg(_downloadFileSize); - } else { - _closeDownloadSession(true); - return; - } - } else { - qCDebug(FileManagerLog) << QString("_requestMissingData: offset(%1) size(%2)").arg(_missingData.head().offset).arg(_missingData.head().size); - - _downloadOffset = _missingData.head().offset; - } - - Request request; - _currentOperation = kCORead; - request.hdr.session = _activeSession; - request.hdr.opcode = kCmdReadFile; - request.hdr.offset = _downloadOffset; - request.hdr.size = sizeof(request.data); - - _sendRequest(&request); -} - -/// Closes out a download session by writing the file and doing cleanup. -/// @param success true: successful download completion, false: error during download -void FileManager::_closeDownloadSession(bool success) -{ - qCDebug(FileManagerLog) << QString("_closeDownloadSession: success(%1) missingBytes(%2)").arg(success).arg(_missingDownloadedBytes); - - _currentOperation = kCOIdle; - - if (success) { - if (_missingDownloadedBytes > 0 || (uint32_t)_readFileAccumulator.length() < _downloadFileSize) { - // we're not done yet: request the missing parts individually (either we had missing parts or - // the last (few) packets right before the EOF got dropped) - _downloadingMissingParts = true; - _requestMissingData(); - return; - } - - QString downloadFilePath = _readFileDownloadDir.absoluteFilePath(_readFileDownloadFilename); - - QFile file(downloadFilePath); - if (!file.open(QIODevice::WriteOnly | QIODevice::Truncate)) { - _emitErrorMessage(tr("Unable to open local file for writing (%1)").arg(downloadFilePath)); - return; - } - - qint64 bytesWritten = file.write((const char *)_readFileAccumulator, _readFileAccumulator.length()); - if (bytesWritten != _readFileAccumulator.length()) { - file.close(); - _emitErrorMessage(tr("Unable to write data to local file (%1)").arg(downloadFilePath)); - return; - } - file.close(); - - emit commandComplete(); - } - - _readFileAccumulator.clear(); - - // Close the open session - _sendResetCommand(); -} - -/// Closes out an upload session doing cleanup. -/// @param success true: successful upload completion, false: error during download -void FileManager::_closeUploadSession(bool success) -{ - qCDebug(FileManagerLog) << QString("_closeUploadSession: success(%1)").arg(success); - - _currentOperation = kCOIdle; - _writeFileAccumulator.clear(); - _writeFileSize = 0; - - if (success) { - emit commandComplete(); - } - - // Close the open session - _sendResetCommand(); -} - -/// Respond to the Ack associated with the Read or Stream commands. -/// @param readFile: true: read file, false: stream file -void FileManager::_downloadAckResponse(Request* readAck, bool readFile) -{ - if (readAck->hdr.session != _activeSession) { - _closeDownloadSession(false /* failure */); - _emitErrorMessage(tr("Download: Incorrect session returned")); - return; - } - - if (readAck->hdr.offset != _downloadOffset) { - if (readFile) { - _closeDownloadSession(false /* failure */); - _emitErrorMessage(tr("Download: Offset returned (%1) differs from offset requested/expected (%2)").arg(readAck->hdr.offset).arg(_downloadOffset)); - return; - } else { // burst - if (readAck->hdr.offset < _downloadOffset) { // old data: ignore it - _setupAckTimeout(); - return; - } - // keep track of missing data chunks - MissingData missingData; - missingData.offset = _downloadOffset; - missingData.size = readAck->hdr.offset - _downloadOffset; - _missingData.push_back(missingData); - _missingDownloadedBytes += readAck->hdr.offset - _downloadOffset; - qCDebug(FileManagerLog) << QString("_downloadAckResponse: missing data: offset(%1) size(%2)").arg(missingData.offset).arg(missingData.size); - _downloadOffset = readAck->hdr.offset; - _readFileAccumulator.resize(_downloadOffset); // placeholder for the missing data - } - } - - qCDebug(FileManagerLog) << QString("_downloadAckResponse: offset(%1) size(%2) burstComplete(%3)").arg(readAck->hdr.offset).arg(readAck->hdr.size).arg(readAck->hdr.burstComplete); - - if (_downloadingMissingParts) { - Q_ASSERT(_missingData.head().offset == _downloadOffset); - _missingDownloadedBytes -= readAck->hdr.size; - _readFileAccumulator.replace(_downloadOffset, readAck->hdr.size, (const char*)readAck->data, readAck->hdr.size); - if (_missingData.head().size <= readAck->hdr.size) { - _missingData.pop_front(); - } else { - _missingData.head().size -= readAck->hdr.size; - _missingData.head().offset += readAck->hdr.size; - } - } else { - _downloadOffset += readAck->hdr.size; - _readFileAccumulator.append((const char*)readAck->data, readAck->hdr.size); - } - - if (_downloadFileSize != 0) { - emit commandProgress(100 * ((float)(_readFileAccumulator.length() - _missingDownloadedBytes) / (float)_downloadFileSize)); - } - - if (_downloadingMissingParts) { - _requestMissingData(); - } else if (readFile || readAck->hdr.burstComplete) { - // Possibly still more data to read, send next read request - - Request request; - request.hdr.session = _activeSession; - request.hdr.opcode = readFile ? kCmdReadFile : kCmdBurstReadFile; - request.hdr.offset = _downloadOffset; - request.hdr.size = 0; - - _sendRequest(&request); - } else if (!readFile) { - // Streaming, so next ack should come automatically - _setupAckTimeout(); - } -} - -/// @brief Respond to the Ack associated with the List command. -void FileManager::_listAckResponse(Request* listAck) -{ - if (listAck->hdr.offset != _listOffset) { - // this is a real error (directory listing is synchronous), no need to retransmit - _currentOperation = kCOIdle; - _emitErrorMessage(tr("List: Offset returned (%1) differs from offset requested (%2)").arg(listAck->hdr.offset).arg(_listOffset)); - return; - } - - uint8_t offset = 0; - uint8_t cListEntries = 0; - uint8_t cBytes = listAck->hdr.size; - - // parse filenames out of the buffer - while (offset < cBytes) { - const char * ptr = ((const char *)listAck->data) + offset; - - // get the length of the name - uint8_t cBytesLeft = cBytes - offset; - uint8_t nlen = static_cast(strnlen(ptr, cBytesLeft)); - if ((*ptr == 'S' && nlen > 1) || (*ptr != 'S' && nlen < 2)) { - _currentOperation = kCOIdle; - _emitErrorMessage(tr("Incorrectly formed list entry: '%1'").arg(ptr)); - return; - } else if (nlen == cBytesLeft) { - _currentOperation = kCOIdle; - _emitErrorMessage(tr("Missing NULL termination in list entry")); - return; - } - - // Returned names are prepended with D for directory, F for file, S for skip - if (*ptr == 'F' || *ptr == 'D') { - // put it in the view - _emitListEntry(ptr); - } else if (*ptr == 'S') { - // do nothing - } else { - qDebug() << "unknown entry" << ptr; - } - - // account for the name + NUL - offset += nlen + 1; - - cListEntries++; - } - - if (listAck->hdr.size == 0 || cListEntries == 0) { - // Directory is empty, we're done - Q_ASSERT(listAck->hdr.opcode == kRspAck); - _currentOperation = kCOIdle; - emit commandComplete(); - } else { - // Possibly more entries to come, need to keep trying till we get EOF - _currentOperation = kCOList; - _listOffset += cListEntries; - _sendListCommand(); - } -} - -/// @brief Respond to the Ack associated with the create command. -void FileManager::_createAckResponse(Request* createAck) -{ - qCDebug(FileManagerLog) << "_createAckResponse"; - - _currentOperation = kCOWrite; - _activeSession = createAck->hdr.session; - - // Start the sequence of write commands from the beginning of the file - - _writeOffset = 0; - _writeSize = 0; - - _writeFileDatablock(); -} - -/// @brief Respond to the Ack associated with the write command. -void FileManager::_writeAckResponse(Request* writeAck) -{ - if(_writeOffset + _writeSize >= _writeFileSize){ - _closeUploadSession(true /* success */); - return; - } - - if (writeAck->hdr.session != _activeSession) { - _closeUploadSession(false /* failure */); - _emitErrorMessage(tr("Write: Incorrect session returned")); - return; - } - - if (writeAck->hdr.offset != _writeOffset) { - _closeUploadSession(false /* failure */); - _emitErrorMessage(tr("Write: Offset returned (%1) differs from offset requested (%2)").arg(writeAck->hdr.offset).arg(_writeOffset)); - return; - } - - if (writeAck->hdr.size != sizeof(uint32_t)) { - _closeUploadSession(false /* failure */); - _emitErrorMessage(tr("Write: Returned invalid size of write size data")); - return; - } - - - if( writeAck->writeFileLength !=_writeSize) { - _closeUploadSession(false /* failure */); - _emitErrorMessage(tr("Write: Size returned (%1) differs from size requested (%2)").arg(writeAck->writeFileLength).arg(_writeSize)); - return; - } - - _writeFileDatablock(); -} - -/// @brief Send next write file data block. -void FileManager::_writeFileDatablock(void) -{ - if (_writeOffset + _writeSize >= _writeFileSize){ - _closeUploadSession(true /* success */); - return; - } - - _writeOffset += _writeSize; - - Request request; - request.hdr.session = _activeSession; - request.hdr.opcode = kCmdWriteFile; - request.hdr.offset = _writeOffset; - - if(_writeFileSize -_writeOffset > sizeof(request.data) ) - _writeSize = sizeof(request.data); - else - _writeSize = _writeFileSize - _writeOffset; - - request.hdr.size = _writeSize; - - memcpy(request.data, &_writeFileAccumulator.data()[_writeOffset], _writeSize); - - _sendRequest(&request); -} - -void FileManager::receiveMessage(mavlink_message_t message) -{ - // 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_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 incorrect target_system:" << data.target_system << "expected:" << _systemIdQGC; - return; - } - - 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::max()/2)) { - qDebug() << "Received old packet: expected seq:" << expectedSeqNumber << "got:" << incomingSeqNumber; - return; - } - - _clearAckTimeout(); - - qCDebug(FileManagerLog) << "receiveMessage" << request->hdr.opcode; - - if (incomingSeqNumber != expectedSeqNumber) { - bool doAbort = true; - switch (_currentOperation) { - case kCOBurst: // burst download drops are handled in _downloadAckResponse() - doAbort = false; - break; - case kCORead: - _closeDownloadSession(false /* failure */); - break; - - case kCOWrite: - _closeUploadSession(false /* failure */); - break; - - case kCOOpenRead: - case kCOOpenBurst: - case kCOCreate: - // We could have an open session hanging around - _currentOperation = kCOIdle; - _sendResetCommand(); - break; - - default: - // Don't need to do anything special - _currentOperation = kCOIdle; - break; - } - - if (doAbort) { - _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 - _lastOutgoingRequest.hdr.seqNumber = incomingSeqNumber; - - if (request->hdr.opcode == kRspAck) { - switch (request->hdr.req_opcode) { - case kCmdListDirectory: - _listAckResponse(request); - break; - - case kCmdOpenFileRO: - case kCmdOpenFileWO: - _openAckResponse(request); - break; - - case kCmdReadFile: - _downloadAckResponse(request, true /* read file */); - break; - - case kCmdBurstReadFile: - _downloadAckResponse(request, false /* stream file */); - break; - - case kCmdCreateFile: - _createAckResponse(request); - break; - - case kCmdWriteFile: - _writeAckResponse(request); - break; - - default: - // Ack back from operation which does not require additional work - _currentOperation = kCOIdle; - break; - } - } else if (request->hdr.opcode == kRspNak) { - uint8_t errorCode = request->data[0]; - - // Nak's normally have 1 byte of data for error code, except for kErrFailErrno which has additional byte for errno - Q_ASSERT((errorCode == kErrFailErrno && request->hdr.size == 2) || request->hdr.size == 1); - - _currentOperation = kCOIdle; - - if (request->hdr.req_opcode == kCmdListDirectory && errorCode == kErrEOF) { - // This is not an error, just the end of the list loop - emit commandComplete(); - return; - } else if ((request->hdr.req_opcode == kCmdReadFile || request->hdr.req_opcode == kCmdBurstReadFile) && errorCode == kErrEOF) { - // This is not an error, just the end of the download loop - _closeDownloadSession(true /* success */); - return; - } else if (request->hdr.req_opcode == kCmdCreateFile) { - _emitErrorMessage(tr("Nak received creating file, error: %1").arg(errorString(request->data[0]))); - return; - } else if (request->hdr.req_opcode == kCmdCreateDirectory) { - _emitErrorMessage(tr("Nak received creating directory, error: %1").arg(errorString(request->data[0]))); - return; - } else { - // Generic Nak handling - if (request->hdr.req_opcode == kCmdReadFile || request->hdr.req_opcode == kCmdBurstReadFile) { - // Nak error during download loop, download failed - _closeDownloadSession(false /* failure */); - } else if (request->hdr.req_opcode == kCmdWriteFile) { - // Nak error during upload loop, upload failed - _closeUploadSession(false /* failure */); - } - _emitErrorMessage(tr("Nak received, error: %1").arg(errorString(request->data[0]))); - } - } else { - // Note that we don't change our operation state. If something goes wrong beyond this, the operation - // will time out. - _emitErrorMessage(tr("Unknown opcode returned from server: %1").arg(request->hdr.opcode)); - } -} - -void FileManager::listDirectory(const QString& dirPath) -{ - if (_currentOperation != kCOIdle) { - _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete.")); - return; - } - - _dedicatedLink = _vehicle->priorityLink(); - if (!_dedicatedLink) { - _emitErrorMessage(tr("Command not sent. No Vehicle links.")); - return; - } - - // initialise the lister - _listPath = dirPath; - _listOffset = 0; - _currentOperation = kCOList; - - // and send the initial request - _sendListCommand(); -} - -void FileManager::_fillRequestWithString(Request* request, const QString& str) -{ - strncpy((char *)&request->data[0], str.toStdString().c_str(), sizeof(request->data)); - request->hdr.size = static_cast(strnlen((const char *)&request->data[0], sizeof(request->data))); -} - -void FileManager::_sendListCommand(void) -{ - Request request; - - request.hdr.session = 0; - request.hdr.opcode = kCmdListDirectory; - request.hdr.offset = _listOffset; - request.hdr.size = 0; - - _fillRequestWithString(&request, _listPath); - - qCDebug(FileManagerLog) << "listDirectory: path:" << _listPath << "offset:" << _listOffset; - - _sendRequest(&request); -} - -void FileManager::downloadPath(const QString& from, const QDir& downloadDir) -{ - if (_currentOperation != kCOIdle) { - _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete.")); - return; - } - - _dedicatedLink = _vehicle->priorityLink(); - if (!_dedicatedLink) { - _emitErrorMessage(tr("Command not sent. No Vehicle links.")); - return; - } - - qCDebug(FileManagerLog) << "downloadPath from:" << from << "to:" << downloadDir; - _downloadWorker(from, downloadDir, true /* read file */); -} - -void FileManager::streamPath(const QString& from, const QDir& downloadDir) -{ - if (_currentOperation != kCOIdle) { - _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete.")); - return; - } - - _dedicatedLink = _vehicle->priorityLink(); - if (!_dedicatedLink) { - _emitErrorMessage(tr("Command not sent. No Vehicle links.")); - return; - } - - qCDebug(FileManagerLog) << "streamPath from:" << from << "to:" << downloadDir; - _downloadWorker(from, downloadDir, false /* stream file */); -} - -void FileManager::_downloadWorker(const QString& from, const QDir& downloadDir, bool readFile) -{ - if (from.isEmpty()) { - return; - } - - _readFileDownloadDir.setPath(downloadDir.absolutePath()); - - // We need to strip off the file name from the fully qualified path. We can't use the usual QDir - // routines because this path does not exist locally. - int i; - for (i=from.size()-1; i>=0; i--) { - if (from[i] == '/') { - break; - } - } - i++; // move past slash - _readFileDownloadFilename = from.right(from.size() - i); - - _currentOperation = readFile ? kCOOpenRead : kCOOpenBurst; - - Request request; - request.hdr.session = 0; - request.hdr.opcode = kCmdOpenFileRO; - request.hdr.offset = 0; - request.hdr.size = 0; - _fillRequestWithString(&request, from); - _sendRequest(&request); -} - -/// @brief Uploads the specified file. -/// @param toPath File in UAS to upload to, fully qualified path -/// @param uploadFile Local file to upload from -void FileManager::uploadPath(const QString& toPath, const QFileInfo& uploadFile) -{ - if(_currentOperation != kCOIdle){ - _emitErrorMessage(tr("UAS File manager busy. Try again later")); - return; - } - - _dedicatedLink = _vehicle->priorityLink(); - if (!_dedicatedLink) { - _emitErrorMessage(tr("Command not sent. No Vehicle links.")); - return; - } - - if (toPath.isEmpty()) { - return; - } - - if (!uploadFile.isReadable()){ - _emitErrorMessage(tr("File (%1) is not readable for upload").arg(uploadFile.path())); - return; - } - - QFile file(uploadFile.absoluteFilePath()); - if (!file.open(QIODevice::ReadOnly)) { - _emitErrorMessage(tr("Unable to open local file for upload (%1)").arg(uploadFile.absoluteFilePath())); - return; - } - - _writeFileAccumulator = file.readAll(); - _writeFileSize = _writeFileAccumulator.size(); - - file.close(); - - if (_writeFileAccumulator.size() == 0) { - _emitErrorMessage(tr("Unable to read data from local file (%1)").arg(uploadFile.absoluteFilePath())); - return; - } - - _currentOperation = kCOCreate; - - Request request; - request.hdr.session = 0; - request.hdr.opcode = kCmdCreateFile; - request.hdr.offset = 0; - request.hdr.size = 0; - _fillRequestWithString(&request, toPath + "/" + uploadFile.fileName()); - _sendRequest(&request); -} - -void FileManager::createDirectory(const QString& directory) -{ - if(_currentOperation != kCOIdle){ - _emitErrorMessage(tr("UAS File manager busy. Try again later")); - return; - } - - _currentOperation = kCOCreateDir; - - Request request; - request.hdr.session = 0; - request.hdr.opcode = kCmdCreateDirectory; - request.hdr.offset = 0; - request.hdr.size = 0; - _fillRequestWithString(&request, directory); - _sendRequest(&request); -} - -QString FileManager::errorString(uint8_t errorCode) -{ - switch(errorCode) { - case kErrNone: - return QString("no error"); - case kErrFail: - return QString("unknown error"); - case kErrEOF: - return QString("read beyond end of file"); - case kErrUnknownCommand: - return QString("unknown command"); - case kErrFailErrno: - return QString("command failed"); - case kErrInvalidDataSize: - return QString("invalid data size"); - case kErrInvalidSession: - return QString("invalid session"); - case kErrNoSessionsAvailable: - return QString("no sessions available"); - case kErrFailFileExists: - return QString("File already exists on target"); - case kErrFailFileProtected: - return QString("File is write protected"); - default: - return QString("unknown error code"); - } -} - -/// @brief Sends a command which only requires an opcode and no additional data -/// @param opcode Opcode to send -/// @param newOpState State to put state machine into -/// @return TRUE: command sent, FALSE: command not sent, waiting for previous command to finish -bool FileManager::_sendOpcodeOnlyCmd(uint8_t opcode, OperationState newOpState) -{ - if (_currentOperation != kCOIdle) { - // Can't have multiple commands in play at the same time - return false; - } - - Request request; - request.hdr.session = 0; - request.hdr.opcode = opcode; - request.hdr.offset = 0; - request.hdr.size = 0; - - _currentOperation = newOpState; - - _sendRequest(&request); - - return true; -} - -/// @brief Starts the ack timeout timer -void FileManager::_setupAckTimeout(void) -{ - qCDebug(FileManagerLog) << "_setupAckTimeout"; - - Q_ASSERT(!_ackTimer.isActive()); - - _ackNumTries = 0; - _ackTimer.setSingleShot(false); - _ackTimer.start(ackTimerTimeoutMsecs); -} - -/// @brief Clears the ack timeout timer -void FileManager::_clearAckTimeout(void) -{ - qCDebug(FileManagerLog) << "_clearAckTimeout"; - _ackTimer.stop(); -} - -/// @brief Called when ack timeout timer fires -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. - - switch (_currentOperation) { - case kCORead: - case kCOBurst: - _closeDownloadSession(false /* failure */); - _emitErrorMessage(tr("Timeout waiting for ack: Download failed")); - break; - - case kCOOpenRead: - case kCOOpenBurst: - _currentOperation = kCOIdle; - _emitErrorMessage(tr("Timeout waiting for ack: Download failed")); - _sendResetCommand(); - break; - - case kCOCreate: - _currentOperation = kCOIdle; - _emitErrorMessage(tr("Timeout waiting for ack: Upload failed")); - _sendResetCommand(); - break; - - case kCOWrite: - _closeUploadSession(false /* failure */); - _emitErrorMessage(tr("Timeout waiting for ack: Upload failed")); - break; - - default: - { - OperationState currentOperation = _currentOperation; - _currentOperation = kCOIdle; - _emitErrorMessage(QString("Timeout waiting for ack: Command failed (%1)").arg(currentOperation)); - } - break; - } -} - -void FileManager::_sendResetCommand(void) -{ - Request request; - request.hdr.opcode = kCmdResetSessions; - request.hdr.size = 0; - _sendRequest(&request); -} - -void FileManager::_emitErrorMessage(const QString& msg) -{ - qCDebug(FileManagerLog) << "Error:" << msg; - emit commandError(msg); -} - -void FileManager::_emitListEntry(const QString& entry) -{ - qCDebug(FileManagerLog) << "_emitListEntry" << entry; - emit listEntry(entry); -} - -/// @brief Sends the specified Request out to the UAS. -void FileManager::_sendRequest(Request* request) -{ - - _setupAckTimeout(); - - 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; - if (_dedicatedLink) { - link = _dedicatedLink; - } else { - link = _vehicle->priorityLink(); - } - - mavlink_msg_file_transfer_protocol_pack_chan(_systemIdQGC, // QGC System ID - 0, // QGC Component ID - link->mavlinkChannel(), - &message, // Mavlink Message to pack into - 0, // Target network - _systemIdServer, // Target system - 0, // Target component - (uint8_t*)request); // Payload - - _vehicle->sendMessageOnLinkThreadSafe(link, message); -} diff --git a/src/uas/FileManager.h b/src/uas/FileManager.h deleted file mode 100644 index f7dc29d2a..000000000 --- a/src/uas/FileManager.h +++ /dev/null @@ -1,259 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -#pragma once - -#include -#include -#include -#include - -#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; - -class FileManager : public QObject -{ - Q_OBJECT - -public: - FileManager(QObject* parent, Vehicle* vehicle); - FileManager(QObject* parent, LinkInterface *link, uint8_t systemIdQGC, uint8_t systemIdServer); - - /// These methods are only used for testing purposes. - bool _sendCmdTestAck(void) { return _sendOpcodeOnlyCmd(kCmdNone, kCOAck); }; - bool _sendCmdTestNoAck(void) { return _sendOpcodeOnlyCmd(kCmdTestNoAck, kCOAck); }; - - /// 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 = 5000; - - static const int ackTimerMaxRetries = 6; - - int _ackTimerTimeoutMsecs = ackTimerTimeoutMsecs; - - int _ackTimerMaxRetries = ackTimerMaxRetries; - - - /// Downloads the specified file. - /// @param from File to download from UAS, fully qualified path - /// @param downloadDir Local directory to download file to - void downloadPath(const QString& from, const QDir& downloadDir); - - /// Stream downloads the specified file. - /// @param from File to download from UAS, fully qualified path - /// @param downloadDir Local directory to download file to - void streamPath(const QString& from, const QDir& downloadDir); - - /// Lists the specified directory. Emits listEntry signal for each entry, followed by listComplete signal. - /// @param dirPath Fully qualified path to list - void listDirectory(const QString& dirPath); - - /// 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 - - /// Signalled to indicate a new directory entry was received. - void listEntry(const QString& entry); - - // Signals associated with all commands - - /// Signalled after a command has completed - void commandComplete(void); - - /// Signalled when an error occurs during a command. In this case a commandComplete signal will - /// not be sent. - void commandError(const QString& msg); - - /// Signalled during a lengthy command to show progress - /// @param value Amount of progress: 0.0 = none, 1.0 = complete - void commandProgress(int value); - - /// Used internally to move sendMessage call to main thread - void _sendMessageOnLinkOnThread(LinkInterface* link, mavlink_message_t message); - -public slots: - void receiveMessage(mavlink_message_t message); - -private slots: - void _ackTimeout(void); - -private: - /// @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 - uint8_t opcode; ///< Command opcode - uint8_t size; ///< Size of data - uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - 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; - - PACKED_STRUCT( - typedef struct _Request - { - RequestHeader hdr; - - // 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 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_file_transfer_protocol_t*)0)->payload) - sizeof(RequestHeader)]; - - // File length returned by Open command - uint32_t openFileLength; - - // Length of file chunk written by write command - uint32_t writeFileLength; - }; - }) Request; - - enum Opcode - { - kCmdNone, ///< ignored, always acked - kCmdTerminateSession, ///< Terminates open Read session - kCmdResetSessions, ///< Terminates all open Read sessions - kCmdListDirectory, ///< List files in from - kCmdOpenFileRO, ///< Opens file at for reading, returns - kCmdReadFile, ///< Reads bytes from in - kCmdCreateFile, ///< Creates file at for writing, returns - kCmdWriteFile, ///< Writes bytes to in - kCmdRemoveFile, ///< Remove file at - kCmdCreateDirectory, ///< Creates directory at - kCmdRemoveDirectory, ///< Removes Directory at , must be empty - kCmdOpenFileWO, ///< Opens file at for writing, returns - kCmdTruncateFile, ///< Truncate file at to length - kCmdRename, ///< Rename to - kCmdCalcFileCRC32, ///< Calculate CRC32 for file at - kCmdBurstReadFile, ///< Burst download session file - - kRspAck = 128, ///< Ack response - kRspNak, ///< Nak response - - // Used for testing only, not part of protocol - kCmdTestNoAck, ///< ignored, ack not sent back, should timeout waiting for ack - }; - - /// @brief Error codes returned in Nak response PayloadHeader.data[0]. - enum ErrorCode - { - kErrNone, - kErrFail, ///< Unknown failure - kErrFailErrno, ///< errno sent back in PayloadHeader.data[1] - kErrInvalidDataSize, ///< PayloadHeader.size is invalid - kErrInvalidSession, ///< Session is not currently open - kErrNoSessionsAvailable, ///< All available Sessions in use - kErrEOF, ///< Offset past end of file for List and Read commands - kErrUnknownCommand, ///< Unknown command opcode - kErrFailFileExists, ///< File exists already - kErrFailFileProtected ///< File is write protected - }; - - enum OperationState - { - kCOIdle, // not doing anything - kCOAck, // waiting for an Ack - kCOList, // waiting for List response - kCOOpenRead, // waiting for Open response followed by Read download - kCOOpenBurst, // waiting for Open response, followed by Burst download - kCORead, // waiting for Read response - 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); - void _setupAckTimeout(void); - void _clearAckTimeout(void); - void _emitErrorMessage(const QString& msg); - void _emitListEntry(const QString& entry); - void _sendRequest(Request* request); - void _sendRequestNoAck(Request* request); - void _sendMessageOnLink(LinkInterface* link, mavlink_message_t message); - void _fillRequestWithString(Request* request, const QString& str); - void _openAckResponse(Request* openAck); - void _downloadAckResponse(Request* readAck, bool readFile); - void _listAckResponse(Request* listAck); - void _createAckResponse(Request* createAck); - void _writeAckResponse(Request* writeAck); - void _writeFileDatablock(void); - void _sendListCommand(void); - void _sendResetCommand(void); - 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 - - Request _lastOutgoingRequest; ///< contains the last outgoing packet - - 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 - - uint32_t _writeOffset; ///< current write offset - uint32_t _writeSize; ///< current write data size - 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; ///< 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 - uint32_t _downloadFileSize; ///< Size of file being downloaded - - uint8_t _systemIdQGC; ///< System ID for QGC - uint8_t _systemIdServer; ///< System ID for server - - // We give MockLinkFileServer friend access so that it can use the data structures and opcodes - // to build a mock mavlink file server for testing. - friend class MockLinkFileServer; -}; - diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index b9af1a20f..f7934933d 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -60,10 +60,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi controlYawManual(true), controlThrustManual(true), -#ifndef __mobile__ - fileManager(this, vehicle), -#endif - attitudeKnown(false), attitudeStamped(false), lastAttitude(0), @@ -113,10 +109,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi _firmwarePluginManager(firmwarePluginManager) { -#ifndef __mobile__ - connect(_vehicle, &Vehicle::mavlinkMessageReceived, &fileManager, &FileManager::receiveMessage); -#endif - } /** diff --git a/src/uas/UAS.h b/src/uas/UAS.h index ec6915431..b6ff6a58e 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -20,10 +20,6 @@ #include "Vehicle.h" #include "FirmwarePluginManager.h" -#ifndef __mobile__ -#include "FileManager.h" -#endif - Q_DECLARE_LOGGING_CATEGORY(UASLog) class Vehicle; @@ -109,10 +105,6 @@ public: temperature_var = var; } -#ifndef __mobile__ - friend class FileManager; -#endif - protected: /// LINK ID AND STATUS int uasId; ///< Unique system ID @@ -138,10 +130,6 @@ protected: /// POSITION bool isGlobalPositionKnown; ///< If the global position has been received for this MAV -#ifndef __mobile__ - FileManager fileManager; -#endif - /// ATTITUDE bool attitudeKnown; ///< True if attitude was received, false else bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV @@ -182,10 +170,6 @@ public: /** @brief Get the human-readable status message for this code */ void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription); -#ifndef __mobile__ - virtual FileManager* getFileManager() { return &fileManager; } -#endif - QImage getImage(); void requestImage(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index e4e141a2a..0d31064c0 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -21,10 +21,6 @@ #include "LinkInterface.h" -#ifndef __mobile__ -class FileManager; -#endif - /** * @brief Interface for all robots. * @@ -43,10 +39,6 @@ public: /** @brief The time interval the robot is switched on **/ virtual quint64 getUptime() const = 0; -#ifndef __mobile__ - virtual FileManager* getFileManager() = 0; -#endif - enum StartCalibrationType { StartCalibrationRadio, StartCalibrationGyro, -- 2.22.0