diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 9e2cd70..e7b59bb 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(_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 214e136..63d50ff 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 a69beaf..6429f40 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]); + } } }