From 55200937f63e43af8138f120901dc3e8e29adfc8 Mon Sep 17 00:00:00 2001 From: olliw42 <6089567+olliw42@users.noreply.github.com> Date: Thu, 17 Nov 2022 08:04:31 +0100 Subject: [PATCH] MAVFtp: Allow proper compId as default (#10488) --- src/Camera/QGCCameraControl.cc | 2 +- src/FactSystem/ParameterManager.cc | 2 +- src/Vehicle/ComponentInformationManager.cc | 2 +- src/Vehicle/FTPManager.cc | 13 +++++++------ src/Vehicle/FTPManager.h | 23 ++++++++++++----------- src/Vehicle/FTPManagerTest.cc | 6 +++--- 6 files changed, 25 insertions(+), 23 deletions(-) diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index 5378d1e..9481c45 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -1970,7 +1970,7 @@ QGCCameraControl::_handleDefinitionFile(const QString &url) ver, ext.toStdString().c_str()); connect(_vehicle->ftpManager(), &FTPManager::downloadComplete, this, &QGCCameraControl::_ftpDownloadComplete); - _vehicle->ftpManager()->download(url, + _vehicle->ftpManager()->download(_compID, url, qgcApp()->toolbox()->settingsManager()->appSettings()->parameterSavePath().toStdString().c_str(), fileName); return; diff --git a/src/FactSystem/ParameterManager.cc b/src/FactSystem/ParameterManager.cc index 6a2c52e..1e97133 100644 --- a/src/FactSystem/ParameterManager.cc +++ b/src/FactSystem/ParameterManager.cc @@ -531,7 +531,7 @@ void ParameterManager::refreshAllParameters(uint8_t componentId) FTPManager* ftpManager = _vehicle->ftpManager(); connect(ftpManager, &FTPManager::downloadComplete, this, &ParameterManager::_ftpDownloadComplete); _waitingParamTimeoutTimer.stop(); - if (ftpManager->download("@PARAM/param.pck", + if (ftpManager->download(MAV_COMP_ID_AUTOPILOT1, "@PARAM/param.pck", QStandardPaths::writableLocation(QStandardPaths::TempLocation), "", false /* No filesize check */)) { connect(ftpManager, &FTPManager::commandProgress, this, &ParameterManager::_ftpDownloadProgress); diff --git a/src/Vehicle/ComponentInformationManager.cc b/src/Vehicle/ComponentInformationManager.cc index 03bfac1..19220f0 100644 --- a/src/Vehicle/ComponentInformationManager.cc +++ b/src/Vehicle/ComponentInformationManager.cc @@ -420,7 +420,7 @@ void RequestMetaDataTypeStateMachine::_requestFile(const QString& cacheFileTag, qCDebug(ComponentInformationManagerLog) << "Downloading json" << uri; if (_uriIsMAVLinkFTP(uri)) { connect(ftpManager, &FTPManager::downloadComplete, this, &RequestMetaDataTypeStateMachine::_ftpDownloadComplete); - if (ftpManager->download(uri, QStandardPaths::writableLocation(QStandardPaths::TempLocation))) { + if (ftpManager->download(MAV_COMP_ID_AUTOPILOT1, uri, QStandardPaths::writableLocation(QStandardPaths::TempLocation))) { _downloadStartTime.start(); connect(ftpManager, &FTPManager::commandProgress, this, &RequestMetaDataTypeStateMachine::_ftpDownloadProgress); } else { diff --git a/src/Vehicle/FTPManager.cc b/src/Vehicle/FTPManager.cc index 8b92e07..4b3d3c2 100644 --- a/src/Vehicle/FTPManager.cc +++ b/src/Vehicle/FTPManager.cc @@ -34,9 +34,9 @@ FTPManager::FTPManager(Vehicle* vehicle) Q_ASSERT(sizeof(MavlinkFTP::RequestHeader) == 12); } -bool FTPManager::download(const QString& fromURI, const QString& toDir, const QString& fileName, bool checksize) +bool FTPManager::download(uint8_t fromCompId, const QString& fromURI, const QString& toDir, const QString& fileName, bool checksize) { - qCDebug(FTPManagerLog) << "download fromURI:" << fromURI << "to:" << toDir; + qCDebug(FTPManagerLog) << "download fromURI:" << fromURI << "to:" << toDir << "fromCompId:" << fromCompId; if (!_rgStateMachine.isEmpty()) { qCDebug(FTPManagerLog) << "Cannot download. Already in another operation"; @@ -58,7 +58,7 @@ bool FTPManager::download(const QString& fromURI, const QString& toDir, const QS _downloadState.toDir.setPath(toDir); _downloadState.checksize = checksize; - if (!_parseURI(fromURI, _downloadState.fullPathOnVehicle, _ftpCompId)) { + if (!_parseURI(fromCompId, fromURI, _downloadState.fullPathOnVehicle, _ftpCompId)) { qCWarning(FTPManagerLog) << "_parseURI failed"; return false; } @@ -171,7 +171,8 @@ void FTPManager::_downloadComplete(const QString& errorMsg) void FTPManager::_mavlinkMessageReceived(const mavlink_message_t& message) { - if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL || message.compid != _ftpCompId) { + if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL || + message.sysid != _vehicle->id() || message.compid != _ftpCompId) { return; } @@ -626,10 +627,10 @@ void FTPManager::_sendRequestExpectAck(MavlinkFTP::Request* request) } } -bool FTPManager::_parseURI(const QString& uri, QString& parsedURI, uint8_t& compId) +bool FTPManager::_parseURI(uint8_t fromCompId, const QString& uri, QString& parsedURI, uint8_t& compId) { parsedURI = uri; - compId = MAV_COMP_ID_AUTOPILOT1; + compId = (fromCompId == MAV_COMP_ID_ALL) ? (uint8_t)MAV_COMP_ID_AUTOPILOT1 : fromCompId; // Pull scheme off the front if there QString ftpPrefix(QStringLiteral("%1://").arg(mavlinkFTPScheme)); diff --git a/src/Vehicle/FTPManager.h b/src/Vehicle/FTPManager.h index e10ef39..10e9095 100644 --- a/src/Vehicle/FTPManager.h +++ b/src/Vehicle/FTPManager.h @@ -32,18 +32,19 @@ public: FTPManager(Vehicle* vehicle); /// Downloads the specified file. - /// @param fromURI File to download from vehicle, fully qualified path. May be in the format "mftp://[;comp=]..." where the component id is specified. - /// If component id is not specified MAV_COMP_ID_AUTOPILOT1 is used. - /// @param toDir Local directory to download file to - /// @param filename (optional) - /// @param checksize (optional, default true) If true compare the filesize indicated in the open - /// response with the transmitted filesize. If false the transmission is tftp style - /// and the indicated filesize from MAVFTP fileopen response is ignored. - /// This is used for the APM parameterdownload where the filesize is wrong due to - /// a dynamic file creation on the vehicle. + /// @param fromCompId Component id of the component to download from. If fromCompId is MAV_COMP_ID_ALL, then MAV_COMP_ID_AUTOPILOT1 is used. + /// @param fromURI File to download from component, fully qualified path. May be in the format "mftp://[;comp=]..." where the component id + /// is specified. If component id is not specified, then the id set via fromCompId is used. + /// @param toDir Local directory to download file to + /// @param filename (optional) + /// @param checksize (optional, default true) If true compare the filesize indicated in the open + /// response with the transmitted filesize. If false the transmission is tftp style + /// and the indicated filesize from MAVFTP fileopen response is ignored. + /// This is used for the APM parameter download where the filesize is wrong due to + /// a dynamic file creation on the vehicle. /// @return true: download has started, false: error, no download /// Signals downloadComplete, commandError, commandProgress - bool download(const QString& fromURI, const QString& toDir, const QString& fileName="", bool checksize = true); + bool download(uint8_t fromCompId, const QString& fromURI, const QString& toDir, const QString& fileName="", bool checksize = true); /// Cancel the current operation /// This will emit downloadComplete() when done, and if there's currently a download in progress @@ -136,7 +137,7 @@ private: void _fillRequestDataWithString(MavlinkFTP::Request* request, const QString& str); void _fillMissingBlocksWorker (bool firstRequest); void _burstReadFileWorker (bool firstRequest); - bool _parseURI (const QString& uri, QString& parsedURI, uint8_t& compId); + bool _parseURI (uint8_t fromCompId, const QString& uri, QString& parsedURI, uint8_t& compId); void _terminateSessionBegin (void); void _terminateSessionAckOrNak (const MavlinkFTP::Request* ackOrNak); diff --git a/src/Vehicle/FTPManagerTest.cc b/src/Vehicle/FTPManagerTest.cc index 8233cd6..e1b4a0e 100644 --- a/src/Vehicle/FTPManagerTest.cc +++ b/src/Vehicle/FTPManagerTest.cc @@ -33,7 +33,7 @@ void FTPManagerTest::_testCaseWorker(const TestCase_t& testCase) QSignalSpy spyDownloadComplete(ftpManager, &FTPManager::downloadComplete); // void downloadComplete (const QString& file, const QString& errorMsg); - ftpManager->download(testCase.file, QStandardPaths::writableLocation(QStandardPaths::TempLocation)); + ftpManager->download(MAV_COMP_ID_AUTOPILOT1, testCase.file, QStandardPaths::writableLocation(QStandardPaths::TempLocation)); QCOMPARE(spyDownloadComplete.wait(10000), true); QCOMPARE(spyDownloadComplete.count(), 1); @@ -53,7 +53,7 @@ void FTPManagerTest::_sizeTestCaseWorker(int fileSize) QSignalSpy spyDownloadComplete(ftpManager, &FTPManager::downloadComplete); - ftpManager->download(filename, QStandardPaths::writableLocation(QStandardPaths::TempLocation)); + ftpManager->download(MAV_COMP_ID_AUTOPILOT1, filename, QStandardPaths::writableLocation(QStandardPaths::TempLocation)); QCOMPARE(spyDownloadComplete.wait(10000), true); QCOMPARE(spyDownloadComplete.count(), 1); @@ -106,7 +106,7 @@ void FTPManagerTest::_testLostPackets(void) QSignalSpy spyDownloadComplete(ftpManager, &FTPManager::downloadComplete); _mockLink->mockLinkFTP()->enableRandromDrops(true); - ftpManager->download(filename, QStandardPaths::writableLocation(QStandardPaths::TempLocation)); + ftpManager->download(MAV_COMP_ID_AUTOPILOT1, filename, QStandardPaths::writableLocation(QStandardPaths::TempLocation)); QCOMPARE(spyDownloadComplete.wait(10000), true); QCOMPARE(spyDownloadComplete.count(), 1);