From 54107e96c6f4ac017ae9b5ea9e6c3f2d2b1523cc Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Tue, 5 Apr 2016 09:58:16 -0700
Subject: [PATCH] Hack fix for multi-vehicle disconnect on same link

---
 src/Vehicle/MultiVehicleManager.cc | 15 +++++++++++++++
 src/Vehicle/MultiVehicleManager.h  |  6 ++++++
 src/Vehicle/Vehicle.cc             |  7 ++++++-
 3 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc
index 9e2cd70ed..e7b59bb87 100644
--- a/src/Vehicle/MultiVehicleManager.cc
+++ b/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;
+}
diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h
index 214e13631..63d50fff6 100644
--- a/src/Vehicle/MultiVehicleManager.h
+++ b/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);
 
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index a69beaf28..6429f4046 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/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]);
+        }
     }
 }
 
-- 
2.22.0