Fix multiple vehicles on same link

- Also fix vehicle selector dropdown in toolbar

src/Vehicle/MultiVehicleManager.cc

@@ -69,25 +69,37 @@ void MultiVehicleManager::setToolbox(QGCToolbox *toolbox)
    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
    qmlRegisterUncreatableType<MultiVehicleManager>("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)) {
+    if (_ignoreVehicleIds.contains(vehicleId) || getVehicleById(vehicleId)) {
-        qCDebug(MultiVehicleManagerLog) << "Adding new vehicle linkName:vehicleId:vehicleFirmwareType:vehicleType"
+        return;
+    }
+
+    qCDebug(MultiVehicleManagerLog()) << "Adding new vehicle link:vehicleId:vehicleMavlinkVersion:vehicleFirmwareType:vehicleType "
                                       << link->getName()
                                       << vehicleId
+                                      << vehicleMavlinkVersion
                                       << vehicleFirmwareType
                                       << vehicleType;
 
-        Vehicle* vehicle = new Vehicle(link, vehicleId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _autopilotPluginManager, _joystickManager);
+    if (vehicleId == _mavlinkProtocol->getSystemId()) {
+        _app->showMessage(QString("Warning: A vehicle is using the same system id as QGroundControl: %1").arg(vehicleId));
+    }
 
-        if (!vehicle) {
+    QSettings settings;
-            qWarning() << "New Vehicle allocation failed";
+    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);
 
@@ -97,7 +109,6 @@ void MultiVehicleManager::_linkActive(LinkInterface* link, int vehicleId, int ve
 
     setActiveVehicle(vehicle);
 }
-}
 
 /// This slot is connected to the Vehicle::allLinksDestroyed signal such that the Vehicle is deleted
 /// and all other right things happen when the Vehicle goes away.

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);

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();

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();
+    return link->_connect();
-
-    if (link->_connect()) {
-        if (!previousAnyConnectedLinks) {
-            emit anyConnectedLinksChanged(true);
-        }
-        return true;
-    } else {
-        return false;
-    }
 }
 
 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<LinkInterface*>(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<LinkInterface*>(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");

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<int>          _ignoreVehicleIds;  ///< List of vehicle id for which we ignore further communication
 
     QmlObjectListModel  _links;
     QmlObjectListModel  _linkConfigurations;

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;
     }

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";
     }
 

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;
     }

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()
         }
     }