From 181e69f74de27602504d05cab4d7610b37826648 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 12 Jan 2016 13:06:34 -0800 Subject: [PATCH] Mini-protocols must only communicate over single link --- src/FactSystem/ParameterLoader.cc | 28 +++++++-------- src/MissionManager/MissionManager.cc | 15 ++++---- src/MissionManager/MissionManager.h | 2 ++ src/ViewWidgets/LogDownloadController.cc | 6 ++-- src/uas/FileManager.cc | 45 ++++++++++++++++++------ src/uas/FileManager.h | 5 +-- src/uas/UAS.cc | 2 +- 7 files changed, 66 insertions(+), 37 deletions(-) diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index 7fc937e61..dab06c209 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -45,15 +45,15 @@ QGC_LOGGING_CATEGORY(ParameterLoaderVerboseLog, "ParameterLoaderVerboseLog") Fact ParameterLoader::_defaultFact; -ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, QObject* parent) : - QObject(parent), - _autopilot(autopilot), - _vehicle(vehicle), - _mavlink(qgcApp()->toolbox()->mavlinkProtocol()), - _parametersReady(false), - _initialLoadComplete(false), - _defaultComponentId(FactSystem::defaultComponentId), - _totalParamCount(0) +ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, QObject* parent) + : QObject(parent) + , _autopilot(autopilot) + , _vehicle(vehicle) + , _mavlink(qgcApp()->toolbox()->mavlinkProtocol()) + , _parametersReady(false) + , _initialLoadComplete(false) + , _defaultComponentId(FactSystem::defaultComponentId) + , _totalParamCount(0) { Q_ASSERT(_autopilot); Q_ASSERT(_vehicle); @@ -321,7 +321,7 @@ void ParameterLoader::refreshAllParameters(void) mavlink_message_t msg; mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), MAV_COMP_ID_ALL); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); qCDebug(ParameterLoaderLog) << "Request to refresh all parameters"; } @@ -522,7 +522,7 @@ void ParameterLoader::_tryCacheLookup() mavlink_message_t msg; mavlink_msg_param_request_read_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), MAV_COMP_ID_ALL, "_HASH_CHECK", -1); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } void ParameterLoader::_readParameterRaw(int componentId, const QString& paramName, int paramIndex) @@ -538,7 +538,7 @@ void ParameterLoader::_readParameterRaw(int componentId, const QString& paramNam componentId, // Target component id fixedParamName, // Named parameter being requested paramIndex); // Parameter index being requested, -1 for named - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value) @@ -591,7 +591,7 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa mavlink_message_t msg; mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } void ParameterLoader::_writeLocalParamCache() @@ -663,7 +663,7 @@ void ParameterLoader::_saveToEEPROM(void) if (_vehicle->firmwarePlugin()->isCapable(FirmwarePlugin::MavCmdPreflightStorageCapability)) { mavlink_message_t msg; mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); qCDebug(ParameterLoaderLog) << "_saveToEEPROM"; } else { qCDebug(ParameterLoaderLog) << "_saveToEEPROM skipped due to FirmwarePlugin::isCapable"; diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 80907cf3f..dc24d6baf 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -34,6 +34,7 @@ QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") MissionManager::MissionManager(Vehicle* vehicle) : _vehicle(vehicle) + , _dedicatedLink(NULL) , _ackTimeoutTimer(NULL) , _retryAck(AckNone) , _readTransactionInProgress(false) @@ -98,7 +99,8 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) mavlink_msg_mission_count_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionCount); - _vehicle->sendMessage(message); + _dedicatedLink = _vehicle->priorityLink(); + _vehicle->sendMessageOnLink(_dedicatedLink, message); _startAckTimeout(AckMissionRequest); emit inProgressChanged(true); } @@ -120,7 +122,8 @@ void MissionManager::requestMissionItems(void) mavlink_msg_mission_request_list_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &request); - _vehicle->sendMessage(message); + _dedicatedLink = _vehicle->priorityLink(); + _vehicle->sendMessageOnLink(_dedicatedLink, message); _startAckTimeout(AckMissionCount); emit inProgressChanged(true); } @@ -180,8 +183,8 @@ void MissionManager::_readTransactionComplete(void) mavlink_msg_mission_ack_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionAck); - _vehicle->sendMessage(message); - + _vehicle->sendMessageOnLink(_dedicatedLink, message); + _finishTransaction(true); emit newMissionItemsAvailable(); } @@ -228,7 +231,7 @@ void MissionManager::_requestNextMissionItem(void) mavlink_msg_mission_request_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionRequest); - _vehicle->sendMessage(message); + _vehicle->sendMessageOnLink(_dedicatedLink, message); _startAckTimeout(AckMissionItem); } @@ -330,7 +333,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message) mavlink_msg_mission_item_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &messageOut, &missionItem); - _vehicle->sendMessage(messageOut); + _vehicle->sendMessageOnLink(_dedicatedLink, messageOut); _startAckTimeout(AckMissionRequest); } diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 1c09f7dbf..3c80333b4 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -33,6 +33,7 @@ #include "QmlObjectListModel.h" #include "QGCMAVLink.h" #include "QGCLoggingCategory.h" +#include "LinkInterface.h" class Vehicle; @@ -116,6 +117,7 @@ private: private: Vehicle* _vehicle; + LinkInterface* _dedicatedLink; QTimer* _ackTimeoutTimer; AckType_t _retryAck; diff --git a/src/ViewWidgets/LogDownloadController.cc b/src/ViewWidgets/LogDownloadController.cc index 2aace6f7b..1affbea7f 100644 --- a/src/ViewWidgets/LogDownloadController.cc +++ b/src/ViewWidgets/LogDownloadController.cc @@ -390,7 +390,7 @@ LogDownloadController::_requestLogData(uint8_t id, uint32_t offset, uint32_t cou &msg, qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), MAV_COMP_ID_ALL, id, offset, count); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } } @@ -419,7 +419,7 @@ LogDownloadController::_requestLogList(uint32_t start, uint32_t end) MAV_COMP_ID_ALL, start, end); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); //-- Wait 2 seconds before bitching about not getting anything _timer.start(2000); } @@ -556,7 +556,7 @@ LogDownloadController::eraseAll(void) qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &msg, qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), MAV_COMP_ID_ALL); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); refresh(); } } diff --git a/src/uas/FileManager.cc b/src/uas/FileManager.cc index 4c34284ed..90de16dd4 100644 --- a/src/uas/FileManager.cc +++ b/src/uas/FileManager.cc @@ -34,13 +34,14 @@ QGC_LOGGING_CATEGORY(FileManagerLog, "FileManagerLog") -FileManager::FileManager(QObject* parent, Vehicle* vehicle) : - QObject(parent), - _currentOperation(kCOIdle), - _vehicle(vehicle), - _lastOutgoingSeqNumber(0), - _activeSession(0), - _systemIdQGC(0) +FileManager::FileManager(QObject* parent, Vehicle* vehicle) + : QObject(parent) + , _currentOperation(kCOIdle) + , _vehicle(vehicle) + , _dedicatedLink(NULL) + , _lastOutgoingSeqNumber(0) + , _activeSession(0) + , _systemIdQGC(0) { connect(&_ackTimer, &QTimer::timeout, this, &FileManager::_ackTimeout); @@ -308,10 +309,8 @@ void FileManager::_writeFileDatablock(void) _sendRequest(&request); } -void FileManager::receiveMessage(LinkInterface* link, mavlink_message_t message) +void FileManager::receiveMessage(mavlink_message_t message) { - Q_UNUSED(link); - // 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; @@ -444,6 +443,12 @@ void FileManager::listDirectory(const QString& dirPath) return; } + _dedicatedLink = _vehicle->priorityLink(); + if (!_dedicatedLink) { + _emitErrorMessage(tr("Command not sent. No Vehicle links.")); + return; + } + // initialise the lister _listPath = dirPath; _listOffset = 0; @@ -481,6 +486,12 @@ void FileManager::downloadPath(const QString& from, const QDir& downloadDir) _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 */); @@ -492,6 +503,12 @@ void FileManager::streamPath(const QString& from, const QDir& downloadDir) _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 */); @@ -537,6 +554,12 @@ void FileManager::uploadPath(const QString& toPath, const QFileInfo& uploadFile) return; } + _dedicatedLink = _vehicle->priorityLink(); + if (!_dedicatedLink) { + _emitErrorMessage(tr("Command not sent. No Vehicle links.")); + return; + } + if (toPath.isEmpty()) { return; } @@ -731,5 +754,5 @@ void FileManager::_sendRequest(Request* request) 0, // Target component (uint8_t*)request); // Payload - _vehicle->sendMessage(message); + _vehicle->sendMessageOnLink(_dedicatedLink, message); } diff --git a/src/uas/FileManager.h b/src/uas/FileManager.h index 696358961..ad4dda930 100644 --- a/src/uas/FileManager.h +++ b/src/uas/FileManager.h @@ -87,7 +87,7 @@ signals: void commandProgress(int value); public slots: - void receiveMessage(LinkInterface* link, mavlink_message_t message); + void receiveMessage(mavlink_message_t message); private slots: void _ackTimeout(void); @@ -203,7 +203,8 @@ private: OperationState _currentOperation; ///< Current operation of state machine QTimer _ackTimer; ///< Used to signal a timeout waiting for an ack - Vehicle* _vehicle; + Vehicle* _vehicle; + LinkInterface* _dedicatedLink; ///< Link to use for communication uint16_t _lastOutgoingSeqNumber; ///< Sequence number sent in last outgoing packet diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 0eee3ea94..e7d2780f6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -185,7 +185,7 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi } #ifndef __mobile__ - connect(mavlink, &MAVLinkProtocol::messageReceived, &fileManager, &FileManager::receiveMessage); + connect(_vehicle, &Vehicle::mavlinkMessageReceived, &fileManager, &FileManager::receiveMessage); #endif color = UASInterface::getNextColor(); -- 2.22.0