diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 35bb23b977e2666750e21c6a9503c2fbfa2371da..29b8331a6fdfa5f48b5299641a2eecc30105f529 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -69,34 +69,45 @@ void MultiVehicleManager::setToolbox(QGCToolbox *toolbox) QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); qmlRegisterUncreatableType("QGroundControl.MultiVehicleManager", 1, 0, "MultiVehicleManager", "Reference only"); - connect(_toolbox->linkManager(), &LinkManager::linkActive, this, &MultiVehicleManager::_linkActive); + connect(_mavlinkProtocol, &MAVLinkProtocol::vehicleHeartbeatInfo, this, &MultiVehicleManager::_vehicleHeartbeatInfo); } -void MultiVehicleManager::_linkActive(LinkInterface* link, int vehicleId, int vehicleFirmwareType, int vehicleType) +void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType) { - if (!getVehicleById(vehicleId)) { - qCDebug(MultiVehicleManagerLog) << "Adding new vehicle linkName:vehicleId:vehicleFirmwareType:vehicleType" - << link->getName() - << vehicleId - << vehicleFirmwareType - << vehicleType; - - Vehicle* vehicle = new Vehicle(link, vehicleId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _autopilotPluginManager, _joystickManager); - - if (!vehicle) { - qWarning() << "New Vehicle allocation failed"; - return; - } - - connect(vehicle, &Vehicle::allLinksInactive, this, &MultiVehicleManager::_deleteVehiclePhase1); - connect(vehicle->autopilotPlugin(), &AutoPilotPlugin::parametersReadyChanged, this, &MultiVehicleManager::_autopilotParametersReadyChanged); + if (_ignoreVehicleIds.contains(vehicleId) || getVehicleById(vehicleId)) { + return; + } - _vehicles.append(vehicle); + qCDebug(MultiVehicleManagerLog()) << "Adding new vehicle link:vehicleId:vehicleMavlinkVersion:vehicleFirmwareType:vehicleType " + << link->getName() + << vehicleId + << vehicleMavlinkVersion + << vehicleFirmwareType + << vehicleType; - emit vehicleAdded(vehicle); + if (vehicleId == _mavlinkProtocol->getSystemId()) { + _app->showMessage(QString("Warning: A vehicle is using the same system id as QGroundControl: %1").arg(vehicleId)); + } - setActiveVehicle(vehicle); + QSettings settings; + bool mavlinkVersionCheck = settings.value("VERSION_CHECK_ENABLED", true).toBool(); + if (mavlinkVersionCheck && vehicleMavlinkVersion != MAVLINK_VERSION) { + _ignoreVehicleIds += vehicleId; + _app->showMessage(QString("The MAVLink protocol version on vehicle #%1 and QGroundControl differ! " + "It is unsafe to use different MAVLink versions. " + "QGroundControl therefore refuses to connect to vehicle #%1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(vehicleId).arg(vehicleMavlinkVersion).arg(MAVLINK_VERSION)); + return; } + + Vehicle* vehicle = new Vehicle(link, vehicleId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _autopilotPluginManager, _joystickManager); + connect(vehicle, &Vehicle::allLinksInactive, this, &MultiVehicleManager::_deleteVehiclePhase1); + connect(vehicle->autopilotPlugin(), &AutoPilotPlugin::parametersReadyChanged, this, &MultiVehicleManager::_autopilotParametersReadyChanged); + + _vehicles.append(vehicle); + + emit vehicleAdded(vehicle); + + setActiveVehicle(vehicle); } /// This slot is connected to the Vehicle::allLinksDestroyed signal such that the Vehicle is deleted diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index 6942ce5a19b33555aa5332d073c72f08c5f9c75a..c1bd65915c2241e243c2843b41d9569af8a19a7a 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -95,8 +95,8 @@ private slots: void _deleteVehiclePhase2(void); void _setActiveVehiclePhase2(void); void _autopilotParametersReadyChanged(bool parametersReady); - void _linkActive(LinkInterface* link, int vehicleId, int vehicleFirmwareType, int vehicleType); void _sendGCSHeartbeat(void); + void _vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType); private: bool _vehicleExists(int vehicleId); diff --git a/src/VehicleSetup/FirmwareUpgradeController.cc b/src/VehicleSetup/FirmwareUpgradeController.cc index 034a71836dc45073a61610b9d5ec1a75fd90ede9..a9197fd47a32a3f952a4daa89c277a840cb356e1 100644 --- a/src/VehicleSetup/FirmwareUpgradeController.cc +++ b/src/VehicleSetup/FirmwareUpgradeController.cc @@ -85,10 +85,12 @@ void FirmwareUpgradeController::startBoardSearch(void) linkMgr->setConnectionsSuspended(tr("Connect not allowed during Firmware Upgrade.")); - if (!linkMgr->anyActiveLinks()) { + // FIXME: Why did we get here with active vehicle? + if (!qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()) { // We have to disconnect any inactive links linkMgr->disconnectAll(); } + _bootloaderFound = false; _startFlashWhenBootloaderFound = false; _threadController->startFindBoardLoop(); diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index fb548edc32e34e9a6cab4b27d5d17f7adb008845..b15f4d3b958ca03dbc861eff29014c1d7836d818 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -91,9 +91,7 @@ LinkManager::LinkManager(QGCApplication* app) LinkManager::~LinkManager() { - if (anyActiveLinks()) { - qWarning() << "Why are there still active links?"; - } + } void LinkManager::setToolbox(QGCToolbox *toolbox) @@ -101,7 +99,6 @@ void LinkManager::setToolbox(QGCToolbox *toolbox) QGCTool::setToolbox(toolbox); _mavlinkProtocol = _toolbox->mavlinkProtocol(); - connect(_mavlinkProtocol, &MAVLinkProtocol::vehicleHeartbeatInfo, this, &LinkManager::_vehicleHeartbeatInfo); connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateAutoConnectLinks); _portListTimer.start(_autoconnectUpdateTimerMSecs); // timeout must be long enough to get past bootloader on second pass @@ -219,16 +216,7 @@ bool LinkManager::connectLink(LinkInterface* link) return false; } - bool previousAnyConnectedLinks = anyConnectedLinks(); - - if (link->_connect()) { - if (!previousAnyConnectedLinks) { - emit anyConnectedLinksChanged(true); - } - return true; - } else { - return false; - } + return link->_connect(); } void LinkManager::disconnectLink(LinkInterface* link) @@ -579,69 +567,6 @@ void LinkManager::_updateAutoConnectLinks(void) #endif // __ios__ } -bool LinkManager::anyConnectedLinks(void) -{ - bool found = false; - for (int i=0; i<_links.count(); i++) { - LinkInterface* link = _links.value(i); - - if (link && link->isConnected()) { - found = true; - break; - } - } - return found; -} - -bool LinkManager::anyActiveLinks(void) -{ - bool found = false; - for (int i=0; i<_links.count(); i++) { - LinkInterface* link = _links.value(i); - - if (link && link->active()) { - found = true; - break; - } - } - return found; -} - -void LinkManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType) -{ - if (!link->active() && !_ignoreVehicleIds.contains(vehicleId)) { - qCDebug(LinkManagerLog) << "New heartbeat on link:vehicleId:vehicleMavlinkVersion:vehicleFirmwareType:vehicleType " - << link->getName() - << vehicleId - << vehicleMavlinkVersion - << vehicleFirmwareType - << vehicleType; - - if (vehicleId == _mavlinkProtocol->getSystemId()) { - _app->showMessage(QString("Warning: A vehicle is using the same system id as QGroundControl: %1").arg(vehicleId)); - } - - QSettings settings; - bool mavlinkVersionCheck = settings.value("VERSION_CHECK_ENABLED", true).toBool(); - if (mavlinkVersionCheck && vehicleMavlinkVersion != MAVLINK_VERSION) { - _ignoreVehicleIds += vehicleId; - _app->showMessage(QString("The MAVLink protocol version on vehicle #%1 and QGroundControl differ! " - "It is unsafe to use different MAVLink versions. " - "QGroundControl therefore refuses to connect to vehicle #%1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(vehicleId).arg(vehicleMavlinkVersion).arg(MAVLINK_VERSION)); - return; - } - - bool previousAnyActiveLinks = anyActiveLinks(); - - link->setActive(true); - emit linkActive(link, vehicleId, vehicleFirmwareType, vehicleType); - - if (!previousAnyActiveLinks) { - emit anyActiveLinksChanged(true); - } - } -} - void LinkManager::shutdown(void) { setConnectionsSuspended("Shutdown"); diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 71e96ba2c02647565aedeb93a525c794c0764e9b..28c2f51c705e42ca57d8200feaffe8018e9e680f 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -74,8 +74,6 @@ public: LinkManager(QGCApplication* app); ~LinkManager(); - Q_PROPERTY(bool anyActiveLinks READ anyActiveLinks NOTIFY anyActiveLinksChanged) - Q_PROPERTY(bool anyConnectedLinks READ anyConnectedLinks NOTIFY anyConnectedLinksChanged) Q_PROPERTY(bool autoconnectUDP READ autoconnectUDP WRITE setAutoconnectUDP NOTIFY autoconnectUDPChanged) Q_PROPERTY(bool autoconnectPixhawk READ autoconnectPixhawk WRITE setAutoconnectPixhawk NOTIFY autoconnectPixhawkChanged) Q_PROPERTY(bool autoconnect3DRRadio READ autoconnect3DRRadio WRITE setAutoconnect3DRRadio NOTIFY autoconnect3DRRadioChanged) @@ -105,8 +103,6 @@ public: // Property accessors - bool anyConnectedLinks (void); - bool anyActiveLinks (void); bool autoconnectUDP (void) { return _autoconnectUDP; } bool autoconnectPixhawk (void) { return _autoconnectPixhawk; } bool autoconnect3DRRadio (void) { return _autoconnect3DRRadio; } @@ -185,8 +181,6 @@ public: virtual void setToolbox(QGCToolbox *toolbox); signals: - void anyActiveLinksChanged(bool anyActiveLinks); - void anyConnectedLinksChanged(bool anyConnectedLinks); void autoconnectUDPChanged(bool autoconnect); void autoconnectPixhawkChanged(bool autoconnect); void autoconnect3DRRadioChanged(bool autoconnect); @@ -216,7 +210,6 @@ signals: private slots: void _linkConnected(void); void _linkDisconnected(void); - void _vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType); private: bool _connectionsSuspendedMsg(void); @@ -236,7 +229,6 @@ private: uint32_t _mavlinkChannelsUsedBitMask; MAVLinkProtocol* _mavlinkProtocol; - QList _ignoreVehicleIds; ///< List of vehicle id for which we ignore further communication QmlObjectListModel _links; QmlObjectListModel _linkConfigurations; diff --git a/src/comm/LogReplayLink.cc b/src/comm/LogReplayLink.cc index 1ab9214f406fe69953d3407344555f59c2a7bda6..27d4f262016194fc35ae1ac7c4d4a3374c9703a9 100644 --- a/src/comm/LogReplayLink.cc +++ b/src/comm/LogReplayLink.cc @@ -102,7 +102,7 @@ LogReplayLink::~LogReplayLink(void) bool LogReplayLink::_connect(void) { // Disallow replay when any links are connected - if (qgcApp()->toolbox()->linkManager()->anyActiveLinks()) { + if (qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()) { emit communicationError(_errorTitle, "You must close all connections prior to replaying a log."); return false; } diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 63f0c2644515af2e364dfef1284811856031f0a4..49131307e5b3a789ec573e3bc8745aa59da1713e 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -425,7 +425,7 @@ void MainWindow::closeEvent(QCloseEvent *event) } // Should not be any active connections - if (qgcApp()->toolbox()->linkManager()->anyActiveLinks()) { + if (qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()) { qWarning() << "All links should be disconnected by now"; } diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index 1befe1f317b8ed75f9bf6d3520df65e8d93a23b7..b6a7bd48c63adf300f55627d162308375bae623a 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -60,7 +60,7 @@ void QGCMAVLinkLogPlayer::_pause(void) void QGCMAVLinkLogPlayer::_selectLogFileForPlayback(void) { // Disallow replay when any links are connected - if (qgcApp()->toolbox()->linkManager()->anyActiveLinks()) { + if (qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()) { QGCMessageBox::information(tr("Log Replay"), tr("You must close all connections prior to replaying a log.")); return; } diff --git a/src/ui/toolbar/MainToolBarIndicators.qml b/src/ui/toolbar/MainToolBarIndicators.qml index 96287ea0ee3d7780014b04de4e84675b58a9b8c8..11eb06b7d9730be88c646eb0543ef5bdf1fb371c 100644 --- a/src/ui/toolbar/MainToolBarIndicators.qml +++ b/src/ui/toolbar/MainToolBarIndicators.qml @@ -325,7 +325,6 @@ Row { MenuItem { checkable: true - checked: vehicle ? vehicle.active : false onTriggered: multiVehicleManager.activeVehicle = vehicle property int vehicleId: Number(text.split(" ")[1]) @@ -355,7 +354,7 @@ Row { Connections { target: multiVehicleManager.vehicles - onCountChanged: vehicleSelectorButton.updateVehicleMenu + onCountChanged: vehicleSelectorButton.updateVehicleMenu() } }