Merge pull request #3147 from DonLakeFlyer/MultiVehicleDisconnect

Hack fix for multi-vehicle disconnect on same link

src/Vehicle/MultiVehicleManager.cc

@@ -326,3 +326,18 @@ void MultiVehicleManager::_sendGCSHeartbeat(void)
         vehicle->sendMessage(message);
     }
 }
+
+bool MultiVehicleManager::linkInUse(LinkInterface* link, Vehicle* skipVehicle)
+{
+    for (int i=0; i< _vehicles.count(); i++) {
+        Vehicle* vehicle = qobject_cast<Vehicle*>(_vehicles[i]);
+
+        if (vehicle != skipVehicle) {
+            if (vehicle->containsLink(link)) {
+                return true;
+            }
+        }
+    }
+
+    return false;
+}

src/Vehicle/MultiVehicleManager.h

@@ -81,6 +81,12 @@ public:
     bool gcsHeartbeatEnabled(void) const { return _gcsHeartbeatEnabled; }
     void setGcsHeartbeatEnabled(bool gcsHeartBeatEnabled);
 
+    /// Determines if the link is in use by a Vehicle
+    ///     @param link Link to test against
+    ///     @param skipVehicle Don't consider this Vehicle as part of the test
+    /// @return true: link is in use by one or more Vehicles
+    bool linkInUse(LinkInterface* link, Vehicle* skipVehicle);
+
     // Override from QGCTool
     virtual void setToolbox(QGCToolbox *toolbox);
 

src/Vehicle/Vehicle.cc

@@ -1262,9 +1262,14 @@ void Vehicle::disconnectInactiveVehicle(void)
 {
     // Vehicle is no longer communicating with us, disconnect all links
 
+
     LinkManager* linkMgr = qgcApp()->toolbox()->linkManager();
     for (int i=0; i<_links.count(); i++) {
-        linkMgr->disconnectLink(_links[i]);
+        // FIXME: This linkInUse check is a hack fix for multiple vehicles on the same link.
+        // The real fix requires significant restructuring which will come later.
+        if (!qgcApp()->toolbox()->multiVehicleManager()->linkInUse(_links[i], this)) {
+            linkMgr->disconnectLink(_links[i]);
+        }
     }
 }