VehicleLinkManager: Major refactor on how QGC manages comm links (#9101)

New VehicleLinkManager implementation

src/Vehicle/MultiVehicleManager.h

@@ -40,16 +40,11 @@ public:
 
     Q_PROPERTY(bool                 activeVehicleAvailable          READ activeVehicleAvailable                                         NOTIFY activeVehicleAvailableChanged)
     Q_PROPERTY(bool                 parameterReadyVehicleAvailable  READ parameterReadyVehicleAvailable                                 NOTIFY parameterReadyVehicleAvailableChanged)
-    /// The current, active vehicle
     Q_PROPERTY(Vehicle*             activeVehicle                   READ activeVehicle                  WRITE setActiveVehicle          NOTIFY activeVehicleChanged)
-    /// The list of all connected vehicles
     Q_PROPERTY(QmlObjectListModel*  vehicles                        READ vehicles                                                       CONSTANT)
-    /// Enable sending heartbeats to the vehicle (defaults to true)
     Q_PROPERTY(bool                 gcsHeartBeatEnabled             READ gcsHeartbeatEnabled            WRITE setGcsHeartbeatEnabled    NOTIFY gcsHeartBeatEnabledChanged)
-    /// A disconnected vehicle used for offline editing. It will match the vehicle type specified in Settings.
     Q_PROPERTY(Vehicle*             offlineEditingVehicle           READ offlineEditingVehicle                                          CONSTANT)
-    /// The current vehicle's last known location
-    Q_PROPERTY(QGeoCoordinate       lastKnownLocation               READ lastKnownLocation                                              NOTIFY lastKnownLocationChanged)
+    Q_PROPERTY(QGeoCoordinate       lastKnownLocation               READ lastKnownLocation                                              NOTIFY lastKnownLocationChanged) //< Current vehicles last know location
 
     // Methods
 
@@ -73,12 +68,6 @@ public:
 
     Vehicle* offlineEditingVehicle(void) { return _offlineEditingVehicle; }
 
-    /// 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/TerrainProtocolHandler.cc

@@ -139,14 +139,14 @@ void TerrainProtocolHandler::_sendTerrainData(const QGeoCoordinate& swCorner, ui
             mavlink_msg_terrain_data_pack_chan(
                         qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                         qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
-                        _vehicle->priorityLink()->mavlinkChannel(),
+                        _vehicle->vehicleLinkManager()->primaryLink()->mavlinkChannel(),
                         &msg,
                         _currentTerrainRequest.lat,
                         _currentTerrainRequest.lon,
                         _currentTerrainRequest.grid_spacing,
                         gridBit,
                         terrainData);
-            _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg);
+            _vehicle->sendMessageOnLinkThreadSafe(_vehicle->vehicleLinkManager()->primaryLink(), msg);
         }
     }
 }

src/Vehicle/VehicleLinkManager.cc

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "VehicleLinkManager.h"
+#include "Vehicle.h"
+#include "QGCLoggingCategory.h"
+#include "LinkManager.h"
+#include "QGCApplication.h"
+
+QGC_LOGGING_CATEGORY(VehicleLinkManagerLog, "VehicleLinkManagerLog")
+
+VehicleLinkManager::VehicleLinkManager(Vehicle* vehicle)
+    : QObject   (vehicle)
+    , _vehicle  (vehicle)
+    , _linkMgr  (qgcApp()->toolbox()->linkManager())
+{
+    connect(this,                   &VehicleLinkManager::linkNamesChanged,  this, &VehicleLinkManager::linkStatusesChanged);
+    connect(&_commLostCheckTimer,   &QTimer::timeout,                       this, &VehicleLinkManager::_commLostCheck);
+
+    _commLostCheckTimer.setSingleShot(false);
+    _commLostCheckTimer.setInterval(_commLostCheckTimeoutMSecs);
+}
+
+void VehicleLinkManager::mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
+{
+    // Radio status messages come from Sik Radios directly. It doesn't indicate there is any life on the other end.
+    if (message.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
+        int linkIndex = _containsLinkIndex(link);
+        if (linkIndex == -1) {
+            _addLink(link);
+        } else {
+            LinkInfo_t& linkInfo = _rgLinkInfo[linkIndex];
+            linkInfo.heartbeatElapsedTimer.restart();
+            if (_rgLinkInfo[linkIndex].commLost) {
+                _commRegainedOnLink(link);
+            }
+        }
+    }
+}
+
+void VehicleLinkManager::_commRegainedOnLink(LinkInterface* link)
+{
+    QString commRegainedMessage;
+    QString primarySwitchMessage;
+
+    int linkIndex = _containsLinkIndex(link);
+    if (linkIndex == -1) {
+        return;
+    }
+
+    _rgLinkInfo[linkIndex].commLost = false;
+
+    // Notify the user of communication regained
+    bool isPrimaryLink = link == _primaryLink;
+    if (_rgLinkInfo.count() > 1) {
+        commRegainedMessage = tr("%1Communication regained on %2 link").arg(_vehicle->_vehicleIdSpeech()).arg(isPrimaryLink ? tr("primary") : tr("secondary"));
+    } else {
+        commRegainedMessage = tr("%1Communication regained").arg(_vehicle->_vehicleIdSpeech());
+    }
+
+    // Try to switch to another link
+    if (_updatePrimaryLink()) {
+        QString primarySwitchMessage = tr("%1Switching communication to new primary link").arg(_vehicle->_vehicleIdSpeech());
+    }
+
+    if (!commRegainedMessage.isEmpty()) {
+        _vehicle->_say(commRegainedMessage);
+    }
+    if (!primarySwitchMessage.isEmpty()) {
+        _vehicle->_say(primarySwitchMessage);
+    }
+    if (!commRegainedMessage.isEmpty() || !primarySwitchMessage.isEmpty()) {
+        bool showBothMessages = !commRegainedMessage.isEmpty() && !primarySwitchMessage.isEmpty();
+        qgcApp()->showAppMessage(QStringLiteral("%1%2%3").arg(commRegainedMessage).arg(showBothMessages ? " " : "").arg(primarySwitchMessage));
+    }
+
+    emit linkStatusesChanged();
+
+    // Check recovery from total communication loss
+    if (_communicationLost) {
+        bool noCommunicationLoss = true;
+        for (const LinkInfo_t& linkInfo: _rgLinkInfo) {
+            if (linkInfo.commLost) {
+                noCommunicationLoss = false;
+                break;
+            }
+        }
+        if (noCommunicationLoss) {
+            _communicationLost = false;
+            emit communicationLostChanged(false);
+        }
+    }
+}
+
+void VehicleLinkManager::_commLostCheck(void)
+{
+    QString switchingPrimaryLinkMessage;
+
+    if (!_communicationLostEnabled) {
+        return;
+    }
+
+    bool linkStatusChange = false;
+    for (LinkInfo_t& linkInfo: _rgLinkInfo) {
+        if (!linkInfo.commLost && !linkInfo.link->linkConfiguration()->isHighLatency() && linkInfo.heartbeatElapsedTimer.elapsed() > _heartbeatMaxElpasedMSecs) {
+            linkInfo.commLost = true;
+            linkStatusChange = true;
+
+            // Notify the user of individual link communication loss
+            bool isPrimaryLink = linkInfo.link.get() == _primaryLink;
+            if (_rgLinkInfo.count() > 1) {
+                QString msg = tr("%1Communication lost on %2 link.").arg(_vehicle->_vehicleIdSpeech()).arg(isPrimaryLink ? tr("primary") : tr("secondary"));
+                _vehicle->_say(msg);
+                qgcApp()->showAppMessage(msg);
+            }
+        }
+    }
+    if (linkStatusChange) {
+        emit linkStatusesChanged();
+    }
+
+    // Switch to better primary link if needed
+    if (_updatePrimaryLink()) {
+        QString msg = tr("%1Switching communication to secondary link.").arg(_vehicle->_vehicleIdSpeech());
+        _vehicle->_say(msg);
+        qgcApp()->showAppMessage(msg);
+    }
+
+    // Check for total communication loss
+    if (!_communicationLost) {
+        bool totalCommunicationLoss = true;
+        for (const LinkInfo_t& linkInfo: _rgLinkInfo) {
+            if (!linkInfo.commLost) {
+                totalCommunicationLoss = false;
+                break;
+            }
+        }
+        if (totalCommunicationLoss) {
+            if (_autoDisconnect) {
+                // There is only one link to the vehicle and we want to auto disconnect from it
+                closeVehicle();
+                return;
+            }
+            _vehicle->_say(tr("%1Communication lost").arg(_vehicle->_vehicleIdSpeech()));
+
+            _communicationLost = true;
+            emit communicationLostChanged(true);
+        }
+    }
+}
+
+int VehicleLinkManager::_containsLinkIndex(LinkInterface* link)
+{
+    for (int i=0; i<_rgLinkInfo.count(); i++) {
+        if (_rgLinkInfo[i].link.get() == link) {
+            return i;
+        }
+    }
+    return -1;
+}
+
+void VehicleLinkManager::_addLink(LinkInterface* link)
+{
+    if (_containsLinkIndex(link) != -1) {
+        qCWarning(VehicleLinkManagerLog) << "_addLink call with link which is already in the list";
+        return;
+    } else {
+        qCDebug(VehicleLinkManagerLog) << "_addLink:" << link->getName() << QString("%1").arg((qulonglong)link, 0, 16);
+
+        link->addVehicleReference();
+
+        LinkInfo_t linkInfo;
+        linkInfo.link = _linkMgr->sharedLinkInterfacePointerForLink(link);
+        if (!link->linkConfiguration()->isHighLatency()) {
+            linkInfo.heartbeatElapsedTimer.start();
+        }
+        _rgLinkInfo.append(linkInfo);
+
+        _updatePrimaryLink();
+
+        connect(link, &LinkInterface::disconnected, this, &VehicleLinkManager::_linkDisconnected);
+
+        emit linkNamesChanged();
+
+        if (_rgLinkInfo.count() == 1) {
+            _commLostCheckTimer.start();
+        }
+    }
+}
+
+void VehicleLinkManager::_removeLink(LinkInterface* link)
+{
+    int linkIndex = _containsLinkIndex(link);
+
+    if (linkIndex == -1) {
+        qCWarning(VehicleLinkManagerLog) << "_removeLink call with link which is already in the list";
+        return;
+    } else {
+        qCDebug(VehicleLinkManagerLog) << "_removeLink:" << QString("%1").arg((qulonglong)link, 0, 16);
+
+        if (link == _primaryLink) {
+            _primaryLink = nullptr;
+            emit primaryLinkChanged();
+        }
+
+        disconnect(link, &LinkInterface::disconnected, this, &VehicleLinkManager::_linkDisconnected);
+        link->removeVehicleReference();
+        emit linkNamesChanged();
+        _rgLinkInfo.removeAt(linkIndex); // Remove the link last since it may cause the link itself to be deleted
+
+        if (_rgLinkInfo.count() == 0) {
+            _commLostCheckTimer.stop();
+        }
+    }
+}
+
+void VehicleLinkManager::_linkDisconnected(void)
+{
+    qCDebug(VehicleLog) << "_linkDisconnected linkCount" << _rgLinkInfo.count();
+
+    LinkInterface* link = qobject_cast<LinkInterface*>(sender());
+    if (link) {
+        _removeLink(link);
+        _updatePrimaryLink();
+        if (_rgLinkInfo.count() == 0) {
+            qCDebug(VehicleLog) << "All links removed. Closing down Vehicle.";
+            emit allLinksRemoved(_vehicle);
+        }
+    }
+}
+
+LinkInterface* VehicleLinkManager::_bestActivePrimaryLink(void)
+{
+#ifndef NO_SERIAL_LINK
+    // Best choice is a USB connection
+    for (const LinkInfo_t& linkInfo: _rgLinkInfo) {
+        if (!linkInfo.commLost) {
+            SharedLinkInterfacePtr  link        = linkInfo.link;
+            SerialLink*             serialLink  = qobject_cast<SerialLink*>(link.get());
+            if (serialLink) {
+                SharedLinkConfigurationPtr config = serialLink->linkConfiguration();
+                if (config) {
+                    SerialConfiguration* serialConfig = qobject_cast<SerialConfiguration*>(config.get());
+                    if (serialConfig && serialConfig->usbDirect()) {
+                        return link.get();
+                    }
+                }
+            }
+        }
+    }
+#endif
+
+    // Next best is normal latency link
+    for (const LinkInfo_t& linkInfo: _rgLinkInfo) {
+        if (!linkInfo.commLost) {
+            SharedLinkInterfacePtr      link    = linkInfo.link;
+            SharedLinkConfigurationPtr  config  = link->linkConfiguration();
+            if (config && !config->isHighLatency()) {
+                return link.get();
+            }
+        }
+    }
+
+    // Last possible choice is a high latency link
+    if (_primaryLink && _primaryLink->linkConfiguration()->isHighLatency()) {
+        // Best choice continues to be the current high latency link
+        return _primaryLink;
+    } else {
+        // Pick any high latency link if one exists
+        for (const LinkInfo_t& linkInfo: _rgLinkInfo) {
+            if (!linkInfo.commLost) {
+                SharedLinkInterfacePtr      link    = linkInfo.link;
+                SharedLinkConfigurationPtr  config  = link->linkConfiguration();
+                if (config && config->isHighLatency()) {
+                    return link.get();
+                }
+            }
+        }
+    }
+
+    return nullptr;
+}
+
+bool VehicleLinkManager::_updatePrimaryLink(void)
+{
+    int linkIndex = _containsLinkIndex(_primaryLink);
+    if (linkIndex != -1 && !_rgLinkInfo[linkIndex].commLost && !_rgLinkInfo[linkIndex].link->linkConfiguration()->isHighLatency()) {
+        // Current priority link is still valid
+        return false;
+    }
+
+    LinkInterface* bestActivePrimaryLink = _bestActivePrimaryLink();
+
+    if (linkIndex != -1 && !bestActivePrimaryLink) {
+        // Nothing better available, leave things set to current primary link
+        return false;
+    } else {
+        if (bestActivePrimaryLink != _primaryLink) {
+            if (_primaryLink && _primaryLink->linkConfiguration()->isHighLatency()) {
+                _vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
+                               MAV_CMD_CONTROL_HIGH_LATENCY,
+                               true,
+                               0); // Stop transmission on this link
+            }
+
+            _primaryLink = bestActivePrimaryLink;
+            emit primaryLinkChanged();
+
+            if (bestActivePrimaryLink && bestActivePrimaryLink->linkConfiguration()->isHighLatency()) {
+                _vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
+                               MAV_CMD_CONTROL_HIGH_LATENCY,
+                               true,
+                               1); // Start transmission on this link
+            }
+            return true;
+        } else {
+            return false;
+        }
+    }
+}
+
+void VehicleLinkManager::closeVehicle(void)
+{
+    // Vehicle is no longer communicating with us. Remove all link references
+
+    QList<LinkInfo_t> rgLinkInfoCopy = _rgLinkInfo;
+    for (const LinkInfo_t& linkInfo: rgLinkInfoCopy) {
+        _removeLink(linkInfo.link.get());
+    }
+
+    _rgLinkInfo.clear();
+
+    emit allLinksRemoved(_vehicle);
+}
+
+void VehicleLinkManager::setCommunicationLostEnabled(bool communicationLostEnabled)
+{
+    if (_communicationLostEnabled != communicationLostEnabled) {
+        _communicationLostEnabled = communicationLostEnabled;
+        emit communicationLostEnabledChanged(communicationLostEnabled);
+    }
+}
+
+bool VehicleLinkManager::containsLink(LinkInterface* link)
+{
+    return _containsLinkIndex(link) != -1;
+}
+
+QString VehicleLinkManager::primaryLinkName() const
+{
+    if (_primaryLink) {
+        return _primaryLink->linkConfiguration()->name();
+    }
+
+    return QString();
+}
+void VehicleLinkManager::setPrimaryLinkByName(const QString& name)
+{
+    for (const LinkInfo_t& linkInfo: _rgLinkInfo) {
+        if (linkInfo.link->linkConfiguration()->name() == name) {
+            _primaryLink = linkInfo.link.get();
+            emit primaryLinkChanged();
+        }
+    }
+}
+
+QStringList VehicleLinkManager::linkNames(void) const
+{
+    QStringList rgNames;
+
+    for (const LinkInfo_t& linkInfo: _rgLinkInfo) {
+        rgNames.append(linkInfo.link->linkConfiguration()->name());
+    }
+
+    return rgNames;
+}
+
+QStringList VehicleLinkManager::linkStatuses(void) const
+{
+    QStringList rgStatuses;
+
+    for (const LinkInfo_t& linkInfo: _rgLinkInfo) {
+        rgStatuses.append(linkInfo.commLost ? tr("Comm Lost") : "");
+    }
+
+    return rgStatuses;
+}

src/Vehicle/VehicleLinkManager.h

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <QObject>
+#include <QTimer>
+#include <QDebug>
+#include <QLoggingCategory>
+#include <QElapsedTimer>
+
+#include "QGCMAVLink.h"
+#include "LinkInterface.h"
+
+Q_DECLARE_LOGGING_CATEGORY(VehicleLinkManagerLog)
+
+class Vehicle;
+class LinkManager;
+class VehicleLinkManagerTest;
+
+class VehicleLinkManager : public QObject
+{
+    Q_OBJECT
+
+    friend class Vehicle;
+    friend class VehicleLinkManagerTest;
+
+public:
+    VehicleLinkManager(Vehicle* vehicle);
+
+    Q_PROPERTY(LinkInterface*   primaryLink                 READ primaryLink                                                    NOTIFY primaryLinkChanged)
+    Q_PROPERTY(QString          primaryLinkName             READ primaryLinkName            WRITE setPrimaryLinkByName          NOTIFY primaryLinkChanged)
+    Q_PROPERTY(QStringList      linkNames                   READ linkNames                                                      NOTIFY linkNamesChanged)
+    Q_PROPERTY(QStringList      linkStatuses                READ linkStatuses                                                   NOTIFY linkStatusesChanged)
+    Q_PROPERTY(bool             communicationLost           READ communicationLost                                              NOTIFY communicationLostChanged)
+    Q_PROPERTY(bool             communicationLostEnabled    READ communicationLostEnabled   WRITE setCommunicationLostEnabled   NOTIFY communicationLostEnabledChanged)
+    Q_PROPERTY(bool             autoDisconnect              MEMBER _autoDisconnect                                              NOTIFY autoDisconnectChanged)
+
+    void            mavlinkMessageReceived      (LinkInterface* link, mavlink_message_t message);
+    bool            containsLink                (LinkInterface* link);
+    LinkInterface*  primaryLink                 (void) { return _primaryLink; }
+    QString         primaryLinkName             (void) const;
+    QStringList     linkNames                   (void) const;
+    QStringList     linkStatuses                (void) const;
+    bool            communicationLost           (void) const { return _communicationLost; }
+    bool            communicationLostEnabled    (void) const { return _communicationLostEnabled; }
+
+    void            setPrimaryLinkByName        (const QString& name);
+    void            setCommunicationLostEnabled (bool communicationLostEnabled);
+
+    void            closeVehicle                (void);
+
+signals:
+    void primaryLinkChanged             (void);
+    void allLinksRemoved                (Vehicle* vehicle);
+    void communicationLostChanged       (bool communicationLost);
+    void communicationLostEnabledChanged(bool communicationLostEnabled);
+    void linkNamesChanged               (void);
+    void linkStatusesChanged            (void);
+    void autoDisconnectChanged          (bool autoDisconnect);
+
+private slots:
+    void _commLostCheck(void);
+
+private:
+    int             _containsLinkIndex      (LinkInterface* link);
+    void            _addLink                (LinkInterface* link);
+    void            _removeLink             (LinkInterface* link);
+    void            _linkDisconnected       (void);
+    bool            _updatePrimaryLink      (void);
+    LinkInterface*  _bestActivePrimaryLink  (void);
+    void            _commRegainedOnLink     (LinkInterface*  link);
+
+    typedef struct {
+        SharedLinkInterfacePtr  link;
+        bool                    commLost = false;
+        QElapsedTimer           heartbeatElapsedTimer;
+    } LinkInfo_t;
+
+    Vehicle*            _vehicle                    = nullptr;
+    LinkManager*        _linkMgr                    = nullptr;
+    QTimer              _commLostCheckTimer;
+    QList<LinkInfo_t>   _rgLinkInfo;
+    LinkInterface*      _primaryLink                = nullptr;
+    bool                _communicationLost          = false;
+    bool                _communicationLostEnabled   = true;
+    bool                _autoDisconnect             = false;    ///< true: Automatically disconnect vehicle when last connection goes away or lost heartbeat
+
+    static const int _commLostCheckTimeoutMSecs     = 1000;  // Check for comm lost once a second
+    static const int _heartbeatMaxElpasedMSecs      = 3500;  // No heartbeat for longer than this indicates comm loss
+};

src/Vehicle/VehicleLinkManagerTest.cc

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "VehicleLinkManagerTest.h"
+#include "QGCApplication.h"
+#include "MockLink.h"
+#include "LinkManager.h"
+#include "QGCApplication.h"
+#include "MultiSignalSpyV2.h"
+
+const char* VehicleLinkManagerTest::_primaryLinkChangedSignalName               = "primaryLinkChanged";
+const char* VehicleLinkManagerTest::_allLinksRemovedSignalName                  = "allLinksRemoved";
+const char* VehicleLinkManagerTest::_communicationLostChangedSignalName         = "communicationLostChanged";
+const char* VehicleLinkManagerTest::_communicationLostEnabledChangedSignalName  = "communicationLostEnabledChanged";
+const char* VehicleLinkManagerTest::_linkNamesChangedSignalName                 = "linkNamesChanged";
+const char* VehicleLinkManagerTest::_linkStatusesChangedSignalName              = "linkStatusesChanged";
+
+VehicleLinkManagerTest::VehicleLinkManagerTest(void)
+{
+
+}
+
+void VehicleLinkManagerTest::init(void)
+{
+    UnitTest::init();
+
+    _multiVehicleMgr = qgcApp()->toolbox()->multiVehicleManager();
+
+    QCOMPARE(_linkManager->links().count(),         0);
+    QCOMPARE(_multiVehicleMgr->vehicles()->count(), 0);
+}
+
+void VehicleLinkManagerTest::cleanup(void)
+{
+    // Disconnect all links
+    if (_linkManager->links().count()) {
+        QSignalSpy spyActiveVehicleChanged(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged);
+        _linkManager->disconnectAll();
+        QCOMPARE(spyActiveVehicleChanged.wait(1000),    true);
+        QCOMPARE(_multiVehicleMgr->vehicles()->count(), 0);
+        QCOMPARE(_linkManager->links().count(),         0);
+    }
+
+    _multiVehicleMgr    = nullptr;
+
+    UnitTest::cleanup();
+}
+
+void VehicleLinkManagerTest::_simpleLinkTest(void)
+{
+    SharedLinkConfigurationPtr  mockConfig;
+    SharedLinkInterfacePtr      mockLink;
+
+    QSignalSpy spyVehicleCreate(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged);
+
+    _startMockLink(1, false /*highLatency*/, true /*incrementVehicleId*/, mockConfig, mockLink);
+    QVERIFY(mockConfig);
+    QVERIFY(mockLink);
+
+    QSignalSpy spyConfigDelete  (mockConfig.get(),  &QObject::destroyed);
+    QSignalSpy spyLinkDelete    (mockLink.get(),    &QObject::destroyed);
+    QVERIFY(spyConfigDelete.isValid());
+    QVERIFY(spyLinkDelete.isValid());
+
+    QCOMPARE(spyVehicleCreate.wait(1000), true);
+    QCOMPARE(_multiVehicleMgr->vehicles()->count(), 1);
+    Vehicle* vehicle = _multiVehicleMgr->activeVehicle();
+    QVERIFY(vehicle);
+    QSignalSpy spyVehicleDelete(vehicle, &QObject::destroyed);
+
+    QCOMPARE(mockConfig.use_count(),    2); // Refs: This method, MockLink
+    QCOMPARE(mockLink.use_count(),      3); // Refs: This method, LinkManager, Vehicle
+
+    // We explicitly don't wait for the Vehicle to finish it's startup sequence before we disconnect.
+    // This helps to wring out possible crashing when the link goes down before the startup sequence is complete.
+    mockLink->disconnect();
+
+    // Vehicle should go away due to disconnect
+    QCOMPARE(spyVehicleDelete.wait(500), true);
+
+    // Config/Link should still be alive due to the last refs being held by this method
+    QCOMPARE(spyConfigDelete.count(),   0);
+    QCOMPARE(spyLinkDelete.count(),     0);
+    QCOMPARE(mockConfig.use_count(),    2); // Refs: This method, MockLink
+    QCOMPARE(mockLink.use_count(),      1); // Refs: This method
+
+    // Let go of our refs from this method and config and link should go away
+    mockConfig.reset();
+    mockLink.reset();
+    QCOMPARE(mockConfig.use_count(),    0);
+    QCOMPARE(mockLink.use_count(),      0);
+    QCOMPARE(spyLinkDelete.count(),     1);
+    QCOMPARE(spyConfigDelete.count(),   1);
+}
+
+void VehicleLinkManagerTest::_simpleCommLossTest(void)
+{
+    SharedLinkConfigurationPtr  mockConfig;
+    SharedLinkInterfacePtr      mockLink;
+
+    QSignalSpy spyVehicleCreate(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged);
+
+    _startMockLink(1, false /*highLatency*/, true /*incrementVehicleId*/, mockConfig, mockLink);
+    MockLink* pMockLink = qobject_cast<MockLink*>(mockLink.get());
+
+    QCOMPARE(spyVehicleCreate.wait(1000),           true);
+    QCOMPARE(_multiVehicleMgr->vehicles()->count(), 1);
+    Vehicle* vehicle = _multiVehicleMgr->activeVehicle();
+    QVERIFY(vehicle);
+
+    QSignalSpy spyCommLostChanged(vehicle->vehicleLinkManager(), &VehicleLinkManager::communicationLostChanged);
+    pMockLink->setCommLost(true);
+    QCOMPARE(spyCommLostChanged.wait(VehicleLinkManager::_heartbeatMaxElpasedMSecs * 2),    true);
+    QCOMPARE(spyCommLostChanged.count(),                                                    1);
+    QCOMPARE(spyCommLostChanged[0][0].toBool(),                                             true);
+
+    spyCommLostChanged.clear();
+    pMockLink->setCommLost(false);
+    QCOMPARE(spyCommLostChanged.wait(VehicleLinkManager::_heartbeatMaxElpasedMSecs * 2),    true);
+    QCOMPARE(spyCommLostChanged.count(),                                                    1);
+    QCOMPARE(spyCommLostChanged[0][0].toBool(),                                             false);
+
+    spyCommLostChanged.clear();
+    vehicle->vehicleLinkManager()->setCommunicationLostEnabled(false);
+    pMockLink->setCommLost(true);
+    QCOMPARE(spyCommLostChanged.wait(VehicleLinkManager::_heartbeatMaxElpasedMSecs * 2),    false);
+
+    spyCommLostChanged.clear();
+    vehicle->vehicleLinkManager()->setCommunicationLostEnabled(true);
+    QCOMPARE(spyCommLostChanged.wait(VehicleLinkManager::_heartbeatMaxElpasedMSecs * 2),    true);
+    QCOMPARE(spyCommLostChanged.count(),                                                    1);
+}
+
+void VehicleLinkManagerTest::_multiLinkSingleVehicleTest(void)
+{
+    SharedLinkConfigurationPtr  mockConfig1;
+    SharedLinkInterfacePtr      mockLink1;
+    SharedLinkConfigurationPtr  mockConfig2;
+    SharedLinkInterfacePtr      mockLink2;
+
+    QSignalSpy spyVehicleCreate(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged);
+
+    _startMockLink(1, false /*highLatency*/, false /*incrementVehicleId*/, mockConfig1, mockLink1);
+    _startMockLink(2, false /*highLatency*/, false /*incrementVehicleId*/, mockConfig2, mockLink2);
+    MockLink* pMockLink1 = qobject_cast<MockLink*>(mockLink1.get());
+    MockLink* pMockLink2 = qobject_cast<MockLink*>(mockLink2.get());
+
+    QCOMPARE(spyVehicleCreate.wait(1000),           true);
+    QCOMPARE(_multiVehicleMgr->vehicles()->count(), 1);
+    Vehicle* vehicle = _multiVehicleMgr->activeVehicle();
+    VehicleLinkManager* vehicleLinkManager = vehicle->vehicleLinkManager();
+    QVERIFY(vehicle);
+    QVERIFY(vehicleLinkManager);
+
+    QCOMPARE(mockLink1.get(), vehicleLinkManager->primaryLink());
+
+    QStringList rgNames = vehicleLinkManager->linkNames();
+    QStringList rgStatus = vehicleLinkManager->linkStatuses();
+    QCOMPARE(rgNames.count(), 2);
+    QCOMPARE(rgNames[0], mockConfig1->name());
+    QCOMPARE(rgNames[1], mockConfig2->name());
+    QCOMPARE(rgStatus.count(), 2);
+    QVERIFY(rgStatus[0].isEmpty());
+    QVERIFY(rgStatus[1].isEmpty());
+
+    MultiSignalSpyV2 multiSpy;
+    QVERIFY(multiSpy.init(vehicleLinkManager));
+
+    // Comm lost on 2: 1 is primary, 2 is secondary so comm loss/regain on 2 should only update status text
+
+    pMockLink2->setCommLost(true);
+    QCOMPARE(multiSpy.waitForSignal (_linkStatusesChangedSignalName, VehicleLinkManager::_heartbeatMaxElpasedMSecs * 2), true);
+    QVERIFY(multiSpy.checkOnlySignalByMask(multiSpy.signalNameToMask(_linkStatusesChangedSignalName)));
+
+    rgStatus = vehicleLinkManager->linkStatuses();
+    QCOMPARE(rgStatus.count(), 2);
+    QVERIFY(rgStatus[0].isEmpty());
+    QVERIFY(!rgStatus[1].isEmpty());
+
+    multiSpy.clearAllSignals();
+
+    pMockLink2->setCommLost(false);
+    QCOMPARE(multiSpy.waitForSignal (_linkStatusesChangedSignalName, VehicleLinkManager::_heartbeatMaxElpasedMSecs * 2), true);
+    QVERIFY(multiSpy.checkOnlySignalByMask(multiSpy.signalNameToMask(_linkStatusesChangedSignalName)));
+
+    rgStatus = vehicleLinkManager->linkStatuses();
+    QCOMPARE(rgStatus.count(), 2);
+    QVERIFY(rgStatus[0].isEmpty());
+    QVERIFY(rgStatus[1].isEmpty());
+
+    multiSpy.clearAllSignals();
+
+    // Comm loss on 1: 1 is primary so should trigger switch of primary to 2
+
+    pMockLink1->setCommLost(true);
+    QCOMPARE(multiSpy.waitForSignal (_primaryLinkChangedSignalName, VehicleLinkManager::_heartbeatMaxElpasedMSecs * 2), true);
+    quint32 signalMask = multiSpy.signalNameToMask(_primaryLinkChangedSignalName) | multiSpy.signalNameToMask(_linkStatusesChangedSignalName);
+    QVERIFY(multiSpy.checkOnlySignalByMask(signalMask));
+    QCOMPARE(pMockLink2, vehicleLinkManager->primaryLink());
+
+    rgStatus = vehicleLinkManager->linkStatuses();
+    QCOMPARE(rgStatus.count(), 2);
+    QVERIFY(!rgStatus[0].isEmpty());
+    QVERIFY(rgStatus[1].isEmpty());
+
+    multiSpy.clearAllSignals();
+
+    // Comm regained on 1 should leave 2 as primary and only update status
+
+    pMockLink1->setCommLost(false);
+    QCOMPARE(multiSpy.waitForSignal (_linkStatusesChangedSignalName, VehicleLinkManager::_heartbeatMaxElpasedMSecs * 2),    true);
+    QVERIFY(multiSpy.checkOnlySignalByMask(multiSpy.signalNameToMask(_linkStatusesChangedSignalName)));
+    QCOMPARE(pMockLink2, vehicleLinkManager->primaryLink());
+
+    rgStatus = vehicleLinkManager->linkStatuses();
+    QCOMPARE(rgStatus.count(), 2);
+    QVERIFY(rgStatus[0].isEmpty());
+    QVERIFY(rgStatus[1].isEmpty());
+
+    multiSpy.clearAllSignals();
+}
+
+void VehicleLinkManagerTest::_connectionRemovedTest(void)
+{
+    SharedLinkConfigurationPtr  mockConfig;
+    SharedLinkInterfacePtr      mockLink;
+
+    QSignalSpy spyVehicleCreate(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged);
+
+    _startMockLink(1, false /*highLatency*/, true /*incrementVehicleId*/, mockConfig, mockLink);
+    MockLink* pMockLink = qobject_cast<MockLink*>(mockLink.get());
+
+    QCOMPARE(spyVehicleCreate.wait(1000), true);
+    Vehicle* vehicle = _multiVehicleMgr->activeVehicle();
+    QVERIFY(vehicle);
+
+    QSignalSpy spyCommLostChanged(vehicle->vehicleLinkManager(), &VehicleLinkManager::communicationLostChanged);
+
+    // Connection removed should just signal communication lost
+
+    pMockLink->simulateConnectionRemoved();
+    QCOMPARE(spyCommLostChanged.wait(VehicleLinkManager::_heartbeatMaxElpasedMSecs * 2),   true);
+    QCOMPARE(spyCommLostChanged.count(),                                                   1);
+    QCOMPARE(spyCommLostChanged[0][0].toBool(),                                            true);
+}
+
+void VehicleLinkManagerTest::_highLatencyLinkTest(void)
+{
+    SharedLinkConfigurationPtr  mockConfig1;
+    SharedLinkInterfacePtr      mockLink1;
+    SharedLinkConfigurationPtr  mockConfig2;
+    SharedLinkInterfacePtr      mockLink2;
+
+    QSignalSpy spyVehicleCreate(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged);
+
+    _startMockLink(1, true /*highLatency*/, false /*incrementVehicleId*/, mockConfig1, mockLink1);
+    MockLink* pMockLink1 = qobject_cast<MockLink*>(mockLink1.get());
+
+    QCOMPARE(spyVehicleCreate.wait(1000), true);
+    QCOMPARE(_multiVehicleMgr->vehicles()->count(), 1);
+    Vehicle* vehicle = _multiVehicleMgr->activeVehicle();
+    VehicleLinkManager* vehicleLinkManager = vehicle->vehicleLinkManager();
+    QVERIFY(vehicle);
+    QVERIFY(vehicleLinkManager);
+
+    MultiSignalSpyV2 multiSpyVLM;
+    QVERIFY(multiSpyVLM.init(vehicleLinkManager));
+
+    // Addition of second non high latency link should:
+    //  Change primary link from 1 to 2
+    //  Stop high latency transmission on 1
+
+    QSignalSpy spyTransmissionEnabledChanged(pMockLink1, &MockLink::highLatencyTransmissionEnabledChanged);
+    QVERIFY(spyTransmissionEnabledChanged.isValid());
+    _startMockLink(2, false /*highLatency*/, false /*incrementVehicleId*/, mockConfig2, mockLink2);
+    MockLink* pMockLink2 = qobject_cast<MockLink*>(mockLink2.get());
+
+    QCOMPARE(multiSpyVLM.waitForSignal(_primaryLinkChangedSignalName, 100), true);
+    QCOMPARE(pMockLink2, vehicleLinkManager->primaryLink());
+    QCOMPARE(spyTransmissionEnabledChanged.count(), 1);
+    QCOMPARE(spyTransmissionEnabledChanged.takeFirst()[0].toBool(), false);
+    multiSpyVLM.clearAllSignals();
+    spyTransmissionEnabledChanged.clear();
+
+    // Comm lost on primary:2 should:
+    //  Switch primary to 1
+    //  Re-enable high latency transmission on 1
+
+    pMockLink2->setCommLost(true);
+    QCOMPARE(multiSpyVLM.waitForSignal(_primaryLinkChangedSignalName, VehicleLinkManager::_heartbeatMaxElpasedMSecs * 2), true);
+    QCOMPARE(pMockLink1, vehicleLinkManager->primaryLink());
+    QCOMPARE(spyTransmissionEnabledChanged.count(), 1);
+    QCOMPARE(spyTransmissionEnabledChanged.takeFirst()[0].toBool(), true);
+    spyTransmissionEnabledChanged.clear();
+}
+
+void VehicleLinkManagerTest::_startMockLink(int mockIndex, bool highLatency, bool incrementVehicleId, SharedLinkConfigurationPtr& mockConfig, SharedLinkInterfacePtr& mockLink)
+{
+    MockConfiguration* pMockConfig = new MockConfiguration(QStringLiteral("Mock %1").arg(mockIndex));
+
+    mockConfig = SharedLinkConfigurationPtr(pMockConfig);
+
+    pMockConfig->setDynamic              (true);
+    pMockConfig->setHighLatency          (highLatency);
+    pMockConfig->setIncrementVehicleId   (incrementVehicleId);
+
+    SharedLinkConfigurationPtr sharedConfigmockConfig;
+
+    QVERIFY(_linkManager->createConnectedLink(mockConfig));
+    QVERIFY(mockConfig->link());
+
+    mockLink = _linkManager->sharedLinkInterfacePointerForLink(mockConfig->link());
+    QVERIFY(mockLink);
+}

src/Vehicle/VehicleLinkManagerTest.h

+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "UnitTest.h"
+#include "LinkInterface.h"
+#include "LinkConfiguration.h"
+
+class LinkManager;
+class MultiVehicleManager;
+class VehicleLinkManager;
+
+class VehicleLinkManagerTest : public UnitTest
+{
+    Q_OBJECT
+
+public:
+    VehicleLinkManagerTest(void);
+
+protected:
+    void init   (void) final;
+    void cleanup(void) final;
+
+private slots:
+    void _simpleLinkTest            (void);
+    void _simpleCommLossTest        (void);
+    void _multiLinkSingleVehicleTest(void);
+    void _connectionRemovedTest     (void);
+    void _highLatencyLinkTest       (void);
+
+private:
+    void _startMockLink(int mockIndex, bool highLatency, bool incrementVehicleId, SharedLinkConfigurationPtr& sharedConfig, SharedLinkInterfacePtr& mockLink);
+
+    //void _simpleLinkTest            (void);
+    //void _simpleCommLossTest        (void);
+    //void _multiLinkSingleVehicleTest(void);
+    //void _connectionRemovedTest     (void);
+    //void _highLatencyLinkTest       (void);
+    MultiVehicleManager*    _multiVehicleMgr = nullptr;
+
+    static const char* _primaryLinkChangedSignalName;
+    static const char* _allLinksRemovedSignalName;
+    static const char* _communicationLostChangedSignalName;
+    static const char* _communicationLostEnabledChangedSignalName;
+    static const char* _linkNamesChangedSignalName;
+    static const char* _linkStatusesChangedSignalName;
+};

src/VehicleSetup/FirmwareUpgrade.qml

@@ -128,7 +128,7 @@ SetupPage {
                             statusTextArea.append(qsTr("Detected [%1]: ").arg(availableDevices.length) + availableDevices.join(", "))
                         }
                         if (QGroundControl.multiVehicleManager.activeVehicle) {
-                            QGroundControl.multiVehicleManager.activeVehicle.autoDisconnect = true
+                            QGroundControl.multiVehicleManager.activeVehicle.vehicleLinkManager.autoDisconnect = true
                         }
                     } else {
                         // We end up here when we detect a board plugged in after we've started upgrade

src/VehicleSetup/FirmwareUpgradeController.cc

@@ -17,6 +17,7 @@
 #include "SettingsManager.h"
 #include "QGCZlib.h"
 #include "JsonHelper.h"
+#include "LinkManager.h"
 
 #include <QStandardPaths>
 #include <QRegularExpression>
@@ -180,7 +181,7 @@ QStringList FirmwareUpgradeController::availableBoardsName(void)
     QStringList names;
 
     auto ports = QGCSerialPortInfo::availablePorts();
-    for(const auto info : ports) {
+    for (const auto& info : ports) {
         if(info.canFlash()) {
             info.getBoardInfo(boardType, boardName);
             names.append(boardName);

src/VehicleSetup/FirmwareUpgradeController.h

@@ -10,7 +10,6 @@
 #pragma once
 
 #include "PX4FirmwareUpgradeThread.h"
-#include "LinkManager.h"
 #include "FirmwareImage.h"
 #include "Fact.h"
 

src/VehicleSetup/SetupView.qml

@@ -39,11 +39,14 @@ Rectangle {
     property bool   _fullParameterVehicleAvailable: QGroundControl.multiVehicleManager.parameterReadyVehicleAvailable && !QGroundControl.multiVehicleManager.activeVehicle.parameterManager.missingParameters
     property var    _corePlugin:                    QGroundControl.corePlugin
 
-    function showSummaryPanel()
-    {
+    function showSummaryPanel() {
         if (mainWindow.preventViewSwitch()) {
             return
         }
+        _showSummaryPanel()
+    }
+
+    function _showSummaryPanel() {
         if (_fullParameterVehicleAvailable) {
             if (QGroundControl.multiVehicleManager.activeVehicle.autopilot.vehicleComponents.length === 0) {
                 panelLoader.setSourceComponent(noComponentsVehicleSummaryComponent)
@@ -88,13 +91,13 @@ Rectangle {
         }
     }
 
-    Component.onCompleted: showSummaryPanel()
+    Component.onCompleted: _showSummaryPanel()
 
     Connections {
         target: QGroundControl.corePlugin
         onShowAdvancedUIChanged: {
             if(!QGroundControl.corePlugin.showAdvancedUI) {
-                showSummaryPanel()
+                _showSummaryPanel()
             }
         }
     }
@@ -109,7 +112,7 @@ Rectangle {
                     //      The summary panel is already showing and the active vehicle goes away
                     //      The active vehicle goes away and we are not on the Firmware panel.
                     summaryButton.checked = true
-                    showSummaryPanel()
+                    _showSummaryPanel()
                 }
             }
         }
@@ -246,7 +249,7 @@ Rectangle {
             SubMenuButton {
                 id:                 px4FlowButton
                 exclusiveGroup:     setupButtonGroup
-                visible:            QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle.priorityLink.isPX4Flow : false
+                visible:            QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle.vehicleLinkManager.primaryLink.isPX4Flow : false
                 setupIndicator:     false
                 text:               qsTr("PX4Flow")
                 Layout.fillWidth:   true
@@ -284,7 +287,7 @@ Rectangle {
                 setupIndicator:     false
                 exclusiveGroup:     setupButtonGroup
                 visible:            QGroundControl.multiVehicleManager.parameterReadyVehicleAvailable &&
-                                    !QGroundControl.multiVehicleManager.activeVehicle.highLatencyLink &&
+                                    !QGroundControl.multiVehicleManager.activeVehicle.usingHighLatencyLink &&
                                     _corePlugin.showAdvancedUI
                 text:               qsTr("Parameters")
                 Layout.fillWidth:   true

src/VideoManager/VideoManager.cc

@@ -552,7 +552,7 @@ VideoManager::setfullScreen(bool f)
 {
     if(f) {
         //-- No can do if no vehicle or connection lost
-        if(!_activeVehicle || _activeVehicle->connectionLost()) {
+        if(!_activeVehicle || _activeVehicle->vehicleLinkManager()->communicationLost()) {
             f = false;
         }
     }
@@ -792,7 +792,7 @@ void
 VideoManager::_setActiveVehicle(Vehicle* vehicle)
 {
     if(_activeVehicle) {
-        disconnect(_activeVehicle, &Vehicle::connectionLostChanged, this, &VideoManager::_connectionLostChanged);
+        disconnect(_activeVehicle->vehicleLinkManager(), &VehicleLinkManager::communicationLostChanged, this, &VideoManager::_communicationLostChanged);
         if(_activeVehicle->cameraManager()) {
             QGCCameraControl* pCamera = _activeVehicle->cameraManager()->currentCameraInstance();
             if(pCamera) {
@@ -803,7 +803,7 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle)
     }
     _activeVehicle = vehicle;
     if(_activeVehicle) {
-        connect(_activeVehicle, &Vehicle::connectionLostChanged, this, &VideoManager::_connectionLostChanged);
+        connect(_activeVehicle->vehicleLinkManager(), &VehicleLinkManager::communicationLostChanged, this, &VideoManager::_communicationLostChanged);
         if(_activeVehicle->cameraManager()) {
             connect(_activeVehicle->cameraManager(), &QGCCameraManager::streamChanged, this, &VideoManager::_restartAllVideos);
             QGCCameraControl* pCamera = _activeVehicle->cameraManager()->currentCameraInstance();
@@ -821,7 +821,7 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle)
 
 //----------------------------------------------------------------------------------------
 void
-VideoManager::_connectionLostChanged(bool connectionLost)
+VideoManager::_communicationLostChanged(bool connectionLost)
 {
     if(connectionLost) {
         //-- Disable full screen video if connection is lost

src/VideoManager/VideoManager.h

@@ -136,7 +136,7 @@ protected slots:
     void _updateUVC                 ();
     void _setActiveVehicle          (Vehicle* vehicle);
     void _aspectRatioChanged        ();
-    void _connectionLostChanged     (bool connectionLost);
+    void _communicationLostChanged  (bool communicationLost);
 
 protected:
     friend class FinishVideoInitialization;

src/comm/BluetoothLink.cc

@@ -7,14 +7,6 @@
  *
  ****************************************************************************/
 
-
-/**
- * @file
- *   @brief Definition of Bluetooth connection for unmanned vehicles
- *   @author Gus Grubba <gus@auterion.com>
- *
- */
-
 #include <QtGlobal>
 #include <QTimer>
 #include <QList>
@@ -29,23 +21,17 @@
 #include "QGCApplication.h"
 #include "BluetoothLink.h"
 #include "QGC.h"
+#include "LinkManager.h"
 
-BluetoothLink::BluetoothLink(SharedLinkConfigurationPointer& config)
-    : LinkInterface(config)
-    , _config(qobject_cast<BluetoothConfiguration*>(config.data()))
-    , _connectState(false)
-    , _targetSocket(nullptr)
-#ifdef __ios__
-    , _discoveryAgent(nullptr)
-#endif
-    , _shutDown(false)
+BluetoothLink::BluetoothLink(SharedLinkConfigurationPtr& config)
+    : LinkInterface     (config)
 {
 
 }
 
 BluetoothLink::~BluetoothLink()
 {
-    _disconnect();
+    disconnect();
 #ifdef __ios__
     if(_discoveryAgent) {
         _shutDown = true;
@@ -58,15 +44,7 @@ BluetoothLink::~BluetoothLink()
 
 void BluetoothLink::run()
 {
-}
 
-void BluetoothLink::_restartConnection()
-{
-    if(this->isConnected())
-    {
-        _disconnect();
-        _connect();
-    }
 }
 
 QString BluetoothLink::getName() const
@@ -79,7 +57,6 @@ void BluetoothLink::_writeBytes(const QByteArray bytes)
     if (_targetSocket) {
         if(_targetSocket->write(bytes) > 0) {
             emit bytesSent(this, bytes);
-            _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch());
         } else {
             qWarning() << "Bluetooth write error";
         }
@@ -94,12 +71,11 @@ void BluetoothLink::readBytes()
             datagram.resize(_targetSocket->bytesAvailable());
             _targetSocket->read(datagram.data(), datagram.size());
             emit bytesReceived(this, datagram);
-            _logInputDataRate(datagram.length(), QDateTime::currentMSecsSinceEpoch());
         }
     }
 }
 
-void BluetoothLink::_disconnect(void)
+void BluetoothLink::disconnect(void)
 {
 #ifdef __ios__
     if(_discoveryAgent) {
@@ -109,8 +85,7 @@ void BluetoothLink::_disconnect(void)
         _discoveryAgent = nullptr;
     }
 #endif
-    if(_targetSocket)
-    {
+    if(_targetSocket) {
         _targetSocket->deleteLater();
         _targetSocket = nullptr;
         emit disconnected();
@@ -141,7 +116,7 @@ bool BluetoothLink::_hardwareConnect()
     _discoveryAgent->start();
 #else
     _createSocket();
-    _targetSocket->connectToService(QBluetoothAddress(_config->device().address), QBluetoothUuid(QBluetoothUuid::SerialPort));
+    _targetSocket->connectToService(QBluetoothAddress(qobject_cast<BluetoothConfiguration*>(_config.get())->device().address), QBluetoothUuid(QBluetoothUuid::SerialPort));
 #endif
     return true;
 }
@@ -218,21 +193,6 @@ bool BluetoothLink::isConnected() const
     return _connectState;
 }
 
-qint64 BluetoothLink::getConnectionSpeed() const
-{
-    return 1000000; // 1 Mbit
-}
-
-qint64 BluetoothLink::getCurrentInDataRate() const
-{
-    return 0;
-}
-
-qint64 BluetoothLink::getCurrentOutDataRate() const
-{
-    return 0;
-}
-
 //--------------------------------------------------------------------------
 //-- BluetoothConfiguration
 
@@ -301,16 +261,6 @@ void BluetoothConfiguration::loadSettings(QSettings& settings, const QString& ro
     settings.endGroup();
 }
 
-void BluetoothConfiguration::updateSettings()
-{
-    if(_link) {
-        auto* ulink = qobject_cast<BluetoothLink*>(_link);
-        if(ulink) {
-            ulink->_restartConnection();
-        }
-    }
-}
-
 void BluetoothConfiguration::stopScan()
 {
     if(_deviceDiscover)
@@ -324,15 +274,12 @@ void BluetoothConfiguration::stopScan()
 
 void BluetoothConfiguration::startScan()
 {
-    if(!_deviceDiscover)
-    {
+    if(!_deviceDiscover) {
         _deviceDiscover = new QBluetoothDeviceDiscoveryAgent(this);
         connect(_deviceDiscover, &QBluetoothDeviceDiscoveryAgent::deviceDiscovered,  this, &BluetoothConfiguration::deviceDiscovered);
         connect(_deviceDiscover, &QBluetoothDeviceDiscoveryAgent::finished,          this, &BluetoothConfiguration::doneScanning);
         emit scanningChanged();
-    }
-    else
-    {
+    } else {
         _deviceDiscover->stop();
     }
     _nameList.clear();

src/comm/BluetoothLink.h

@@ -7,14 +7,6 @@
  *
  ****************************************************************************/
 
-
-/*!
- * @file
- *   @brief Bluetooth connection for unmanned vehicles
- *   @author Gus Grubba <gus@auterion.com>
- *
- */
-
 #pragma once
 
 #include <QString>
@@ -29,10 +21,12 @@
 #include <qbluetoothservicediscoveryagent.h>
 
 #include "QGCConfig.h"
-#include "LinkManager.h"
+#include "LinkConfiguration.h"
+#include "LinkInterface.h"
 
 class QBluetoothDeviceDiscoveryAgent;
 class QBluetoothServiceDiscoveryAgent;
+class LinkManager;
 
 class BluetoothData
 {
@@ -85,43 +79,40 @@ public:
     Q_PROPERTY(QStringList  nameList    READ nameList                     NOTIFY nameListChanged)
     Q_PROPERTY(bool         scanning    READ scanning                     NOTIFY scanningChanged)
 
-    Q_INVOKABLE void        startScan   ();
-    Q_INVOKABLE void        stopScan    ();
-
-    QString     devName                 () { return _device.name; }
-    QString     address                 ();
-    QStringList nameList                () { return _nameList; }
-    bool        scanning                () { return _deviceDiscover != nullptr; }
-
-    BluetoothData    device             () { return _device; }
+    Q_INVOKABLE void startScan  (void);
+    Q_INVOKABLE void stopScan   (void);
 
-    void        setDevName              (const QString& name);
+    QString         devName     (void)                  { return _device.name; }
+    QString         address     (void);
+    QStringList     nameList    (void)                  { return _nameList; }
+    bool            scanning    (void)                  { return _deviceDiscover != nullptr; }
+    BluetoothData   device      (void)                  { return _device; }
+    void            setDevName  (const QString& name);
 
-    /// From LinkConfiguration
-    LinkType    type                    () { return LinkConfiguration::TypeBluetooth; }
-    void        copyFrom                (LinkConfiguration* source);
-    void        loadSettings            (QSettings& settings, const QString& root);
-    void        saveSettings            (QSettings& settings, const QString& root);
-    void        updateSettings          ();
-    QString     settingsURL             () { return "BluetoothSettings.qml"; }
-    QString     settingsTitle           ();
+    /// LinkConfiguration overrides
+    LinkType    type            (void) override                                         { return LinkConfiguration::TypeBluetooth; }
+    void        copyFrom        (LinkConfiguration* source) override;
+    void        loadSettings    (QSettings& settings, const QString& root) override;
+    void        saveSettings    (QSettings& settings, const QString& root) override;
+    QString     settingsURL     (void) override                                         { return "BluetoothSettings.qml"; }
+    QString     settingsTitle   (void) override;
 
 public slots:
-    void        deviceDiscovered        (QBluetoothDeviceInfo info);
-    void        doneScanning            ();
+    void deviceDiscovered   (QBluetoothDeviceInfo info);
+    void doneScanning       (void);
 
 signals:
-    void        newDevice               (QBluetoothDeviceInfo info);
-    void        devNameChanged          ();
-    void        addressChanged          ();
-    void        nameListChanged         ();
-    void        scanningChanged         ();
+    void newDevice      (QBluetoothDeviceInfo info);
+    void devNameChanged (void);
+    void addressChanged (void);
+    void nameListChanged(void);
+    void scanningChanged(void);
 
 private:
-    QBluetoothDeviceDiscoveryAgent*     _deviceDiscover;
-    BluetoothData                       _device;
-    QStringList                         _nameList;
-    QList<BluetoothData>                _deviceList;
+    QBluetoothDeviceDiscoveryAgent* _deviceDiscover = nullptr;
+    BluetoothData                   _device;
+    QStringList                     _nameList;
+    QList<BluetoothData>            _deviceList;
 };
 
 class BluetoothLink : public LinkInterface
@@ -132,65 +123,44 @@ class BluetoothLink : public LinkInterface
     friend class LinkManager;
 
 public:
-    void    requestReset            () { }
-    bool    isConnected             () const;
-    QString getName                 () const;
-
-    // Extensive statistics for scientific purposes
-    qint64  getConnectionSpeed      () const;
-    qint64  getCurrentInDataRate    () const;
-    qint64  getCurrentOutDataRate   () const;
-
-    void run();
+    ~BluetoothLink();
 
-    // These are left unimplemented in order to cause linker errors which indicate incorrect usage of
-    // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
-    bool    connect                 (void);
-    bool    disconnect              (void);
+    // Overrides from QThread
+    void run(void) override;
 
-    LinkConfiguration* getLinkConfiguration() { return _config; }
+    // LinkConfiguration overrides
+    bool    isConnected (void) const override;
+    QString getName     (void) const override;
+    void    disconnect  (void) override;
 
 public slots:
-    void    readBytes               ();
-    void    deviceConnected         ();
-    void    deviceDisconnected      ();
-    void    deviceError             (QBluetoothSocket::SocketError error);
+    void    readBytes           (void);
+    void    deviceConnected     (void);
+    void    deviceDisconnected  (void);
+    void    deviceError         (QBluetoothSocket::SocketError error);
 #ifdef __ios__
-    void    serviceDiscovered       (const QBluetoothServiceInfo &info);
-    void    discoveryFinished       ();
+    void    serviceDiscovered   (const QBluetoothServiceInfo &info);
+    void    discoveryFinished   (void);
 #endif
 
-protected:
-
-    BluetoothConfiguration*     _config;
-    bool                        _connectState;
-
-private:
-    // Links are only created/destroyed by LinkManager so constructor/destructor is not public
-    BluetoothLink(SharedLinkConfigurationPointer& config);
-    ~BluetoothLink();
-
-    // From LinkInterface
-    bool _connect               (void);
-    void _disconnect            (void);
-
-    bool _hardwareConnect       ();
-    void _restartConnection     ();
-
 private slots:
-    void _writeBytes            (const QByteArray bytes);
+    // LinkInterface overrides
+    void _writeBytes(const QByteArray bytes) override;
 
 private:
-    void _createSocket          ();
+    BluetoothLink(SharedLinkConfigurationPtr& config);
 
-private:
+    // LinkInterface overrides
+    bool _connect(void) override;
+
+    bool _hardwareConnect   (void);
+    void _createSocket      (void);
 
-    QBluetoothSocket*           _targetSocket;
+    QBluetoothSocket*                   _targetSocket    = nullptr;
 #ifdef __ios__
-    QBluetoothServiceDiscoveryAgent* _discoveryAgent;
+    QBluetoothServiceDiscoveryAgent*    _discoveryAgent = nullptr;
 #endif
-
-    bool                        _shutDown;
-
+    bool                                _shutDown       = false;
+    bool                                _connectState   = false;
 };
 

src/comm/LinkConfiguration.cc

@@ -7,13 +7,6 @@
  *
  ****************************************************************************/
 
-
-/*!
-    @file
-       @brief Link specific configuration base class
-       @author Gus Grubba <gus@auterion.com>
-*/
-
 #include "LinkConfiguration.h"
 #ifndef NO_SERIAL_LINK
 #include "SerialLink.h"
@@ -31,11 +24,10 @@
 #define LINK_SETTING_ROOT "LinkConfigurations"
 
 LinkConfiguration::LinkConfiguration(const QString& name)
-    : _link(nullptr)
-    , _name(name)
-    , _dynamic(false)
-    , _autoConnect(false)
-    , _highLatency(false)
+    : _name         (name)
+    , _dynamic      (false)
+    , _autoConnect  (false)
+    , _highLatency  (false)
 {
     _name = name;
     if (_name.isEmpty()) {
@@ -45,7 +37,7 @@ LinkConfiguration::LinkConfiguration(const QString& name)
 
 LinkConfiguration::LinkConfiguration(LinkConfiguration* copy)
 {
-    _link       = copy->link();
+    _link       = copy->_link;
     _name       = copy->name();
     _dynamic    = copy->isDynamic();
     _autoConnect= copy->isAutoConnect();
@@ -56,7 +48,7 @@ LinkConfiguration::LinkConfiguration(LinkConfiguration* copy)
 void LinkConfiguration::copyFrom(LinkConfiguration* source)
 {
     Q_ASSERT(source != nullptr);
-    _link       = source->link();
+    _link       = source->_link;
     _name       = source->name();
     _dynamic    = source->isDynamic();
     _autoConnect= source->isAutoConnect();
@@ -152,10 +144,8 @@ void LinkConfiguration::setName(const QString name)
     emit nameChanged(name);
 }
 
-void LinkConfiguration::setLink(LinkInterface* link)
+void LinkConfiguration::setLink(SharedLinkInterfacePtr link)
 {
-    if(_link != link) {
-        _link = link;
-        emit linkChanged(link);
-    }
+    _link = link;
+    emit linkChanged();
 }

src/comm/LinkConfiguration.h

@@ -11,6 +11,8 @@
 
 #include <QSettings>
 
+#include <memory>
+
 class LinkInterface;
 
 /// Interface holding link specific settings.
@@ -24,7 +26,7 @@ public:
     virtual ~LinkConfiguration() {}
 
     Q_PROPERTY(QString          name                READ name           WRITE setName           NOTIFY nameChanged)
-    Q_PROPERTY(LinkInterface*   link                READ link           WRITE setLink           NOTIFY linkChanged)
+    Q_PROPERTY(LinkInterface*   link                READ link                                   NOTIFY linkChanged)
     Q_PROPERTY(LinkType         linkType            READ type                                   CONSTANT)
     Q_PROPERTY(bool             dynamic             READ isDynamic      WRITE setDynamic        NOTIFY dynamicChanged)
     Q_PROPERTY(bool             autoConnect         READ isAutoConnect  WRITE setAutoConnect    NOTIFY autoConnectChanged)
@@ -37,10 +39,10 @@ public:
     // Property accessors
 
     QString         name(void) const { return _name; }
-    LinkInterface*  link(void)  { return _link; }
+    LinkInterface*  link(void)  { return _link.lock().get(); }
 
     void            setName(const QString name);
-    void            setLink(LinkInterface* link);
+    void            setLink(std::shared_ptr<LinkInterface> link);
 
     ///  The link types supported by QGC
     ///  Any changes here MUST be reflected in LinkManager::linkTypeStrings()
@@ -61,19 +63,8 @@ public:
     };
     Q_ENUM(LinkType)
 
-    /*!
-     *
-     * Is this a dynamic configuration? (non persistent)
-     * @return True if this is an automatically added configuration.
-     */
-    bool isDynamic() { return _dynamic; }
-
-    /*!
-     *
-     * Is this an Auto Connect configuration?
-     * @return True if this is an Auto Connect configuration (connects automatically at boot time).
-     */
-    bool isAutoConnect() { return _autoConnect; }
+    bool isDynamic      () { return _dynamic; }     ///< Not persisted
+    bool isAutoConnect  () { return _autoConnect; }
 
     /*!
      *
@@ -153,13 +144,6 @@ public:
      */
     virtual QString settingsTitle   () = 0;
 
-    /*!
-     * @brief Update settings
-     *
-     * After editing the settings, use this method to tell the connected link (if any) to reload its configuration.
-     */
-    virtual void updateSettings() {}
-
     /*!
      * @brief Copy instance data
      *
@@ -198,11 +182,12 @@ signals:
     void nameChanged        (const QString& name);
     void dynamicChanged     ();
     void autoConnectChanged ();
-    void linkChanged        (LinkInterface* link);
     void highLatencyChanged ();
+    void linkChanged        ();
 
 protected:
-    LinkInterface* _link; ///< Link currently using this configuration (if any)
+    std::weak_ptr<LinkInterface> _link; ///< Link currently using this configuration (if any)
+
 private:
     QString _name;
     bool    _dynamic;       ///< A connection added automatically and not persistent (unless it's edited).
@@ -210,5 +195,6 @@ private:
     bool    _highLatency;
 };
 
-typedef QSharedPointer<LinkConfiguration> SharedLinkConfigurationPointer;
+typedef std::shared_ptr<LinkConfiguration>  SharedLinkConfigurationPtr;
+typedef std::weak_ptr<LinkConfiguration>    WeakLinkConfigurationPtr;
 

src/comm/LinkInterface.cc

@@ -10,30 +10,24 @@
 #include "LinkInterface.h"
 #include "QGCApplication.h"
 
-bool LinkInterface::active() const
+LinkInterface::LinkInterface(SharedLinkConfigurationPtr& config, bool isPX4Flow)
+    : QThread   (0)
+    , _config   (config)
+    , _isPX4Flow(isPX4Flow)
 {
-    QMapIterator<int /* vehicle id */, MavlinkMessagesTimer*> iter(_mavlinkMessagesTimers);
-    while (iter.hasNext()) {
-        iter.next();
-        if (iter.value()->getActive()) {
-            return true;
-        }
-    }
+    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
 
-    return false;
+    qRegisterMetaType<LinkInterface*>("LinkInterface*");
 }
 
-bool LinkInterface::link_active(int vehicle_id) const
+LinkInterface::~LinkInterface()
 {
-    if (_mavlinkMessagesTimers.contains(vehicle_id)) {
-        return _mavlinkMessagesTimers.value(vehicle_id)->getActive();
-    } else {
-        return false;
+    if (_vehicleReferenceCount != 0) {
+        qWarning() << "~LinkInterface still have vehicle references:" << _vehicleReferenceCount;
     }
+    _config.reset();
 }
 
-/// mavlink channel to use for this link, as used by mavlink_parse_char. The mavlink channel is only
-/// set into the link when it is added to LinkManager
 uint8_t LinkInterface::mavlinkChannel(void) const
 {
     if (!_mavlinkChannelSet) {
@@ -41,139 +35,7 @@ uint8_t LinkInterface::mavlinkChannel(void) const
     }
     return _mavlinkChannel;
 }
-// Links are only created by LinkManager so constructor is not public
-LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config, bool isPX4Flow)
-    : QThread                   (0)
-    , _config                   (config)
-    , _highLatency              (config->isHighLatency())
-    , _mavlinkChannelSet        (false)
-    , _enableRateCollection     (false)
-    , _decodedFirstMavlinkPacket(false)
-    , _isPX4Flow                (isPX4Flow)
-{
-    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
-
-    _config->setLink(this);
-
-    // Initialize everything for the data rate calculation buffers.
-    _inDataIndex  = 0;
-    _outDataIndex = 0;
-
-    // Initialize our data rate buffers.
-    memset(_inDataWriteAmounts, 0, sizeof(_inDataWriteAmounts));
-    memset(_inDataWriteTimes,   0, sizeof(_inDataWriteTimes));
-    memset(_outDataWriteAmounts,0, sizeof(_outDataWriteAmounts));
-    memset(_outDataWriteTimes,  0, sizeof(_outDataWriteTimes));
-
-    qRegisterMetaType<LinkInterface*>("LinkInterface*");
-}
-
-/// This function logs the send times and amounts of datas for input. Data is used for calculating
-/// the transmission rate.
-///     @param byteCount Number of bytes received
-///     @param time Time in ms send occurred
-void LinkInterface::_logInputDataRate(quint64 byteCount, qint64 time) {
-    if(_enableRateCollection)
-        _logDataRateToBuffer(_inDataWriteAmounts, _inDataWriteTimes, &_inDataIndex, byteCount, time);
-}
-
-/// This function logs the send times and amounts of datas for output. Data is used for calculating
-/// the transmission rate.
-///     @param byteCount Number of bytes sent
-///     @param time Time in ms receive occurred
-void LinkInterface::_logOutputDataRate(quint64 byteCount, qint64 time) {
-    if(_enableRateCollection)
-        _logDataRateToBuffer(_outDataWriteAmounts, _outDataWriteTimes, &_outDataIndex, byteCount, time);
-}
-
-/**
-     * @brief logDataRateToBuffer Stores transmission times/amounts for statistics
-     *
-     * This function logs the send times and amounts of datas to the given circular buffers.
-     * This data is used for calculating the transmission rate.
-     *
-     * @param bytesBuffer[out] The buffer to write the bytes value into.
-     * @param timeBuffer[out] The buffer to write the time value into
-     * @param writeIndex[out] The write index used for this buffer.
-     * @param bytes The amount of bytes transmit.
-     * @param time The time (in ms) this transmission occurred.
-     */
-void LinkInterface::_logDataRateToBuffer(quint64 *bytesBuffer, qint64 *timeBuffer, int *writeIndex, quint64 bytes, qint64 time)
-{
-    QMutexLocker dataRateLocker(&_dataRateMutex);
-
-    int i = *writeIndex;
-
-    // Now write into the buffer, if there's no room, we just overwrite the first data point.
-    bytesBuffer[i] = bytes;
-    timeBuffer[i] = time;
-
-    // Increment and wrap the write index
-    ++i;
-    if (i == _dataRateBufferSize)
-    {
-        i = 0;
-    }
-    *writeIndex = i;
-}
-
-/**
-     * @brief getCurrentDataRate Get the current data rate given a data rate buffer.
-     *
-     * This function attempts to use the times and number of bytes transmit into a current data rate
-     * estimation. Since it needs to use timestamps to get the timeperiods over when the data was sent,
-     * this is effectively a global data rate over the last _dataRateBufferSize - 1 data points. Also note
-     * that data points older than NOW - dataRateCurrentTimespan are ignored.
-     *
-     * @param index The first valid sample in the data rate buffer. Refers to the oldest time sample.
-     * @param dataWriteTimes The time, in ms since epoch, that each data sample took place.
-     * @param dataWriteAmounts The amount of data (in bits) that was transferred.
-     * @return The bits per second of data transferrence of the interface over the last [-statsCurrentTimespan, 0] timespan.
-     */
-qint64 LinkInterface::_getCurrentDataRate(int index, const qint64 dataWriteTimes[], const quint64 dataWriteAmounts[]) const
-{
-    const qint64 now = QDateTime::currentMSecsSinceEpoch();
-
-    // Limit the time we calculate to the recent past
-    const qint64 cutoff = now - _dataRateCurrentTimespan;
-
-    // Grab the mutex for working with the stats variables
-    QMutexLocker dataRateLocker(&_dataRateMutex);
 
-    // Now iterate through the buffer of all received data packets adding up all values
-    // within now and our cutof.
-    qint64 totalBytes = 0;
-    qint64 totalTime = 0;
-    qint64 lastTime = 0;
-    int size = _dataRateBufferSize;
-    while (size-- > 0)
-    {
-        // If this data is within our cutoff time, include it in our calculations.
-        // This also accounts for when the buffer is empty and filled with 0-times.
-        if (dataWriteTimes[index] > cutoff && lastTime > 0) {
-            // Track the total time, using the previous time as our timeperiod.
-            totalTime += dataWriteTimes[index] - lastTime;
-            totalBytes += dataWriteAmounts[index];
-        }
-
-        // Track the last time sample for doing timespan calculations
-        lastTime = dataWriteTimes[index];
-
-        // Increment and wrap the index if necessary.
-        if (++index == _dataRateBufferSize)
-        {
-            index = 0;
-        }
-    }
-
-    // Return the final calculated value in bits / s, converted from bytes/ms.
-    qint64 dataRate = (totalTime != 0)?(qint64)((float)totalBytes * 8.0f / ((float)totalTime / 1000.0f)):0;
-
-    // Finally return our calculated data rate.
-    return dataRate;
-}
-
-/// Sets the mavlink channel to use for this link
 void LinkInterface::_setMavlinkChannel(uint8_t channel)
 {
     if (_mavlinkChannelSet) {
@@ -183,37 +45,45 @@ void LinkInterface::_setMavlinkChannel(uint8_t channel)
     _mavlinkChannel = channel;
 }
 
-void LinkInterface::_activeChanged(bool active, int vehicle_id)
+void LinkInterface::writeBytesThreadSafe(const char *bytes, int length)
 {
-    emit activeChanged(this, active, vehicle_id);
+    QByteArray byteArray(bytes, length);
+    _writeBytesMutex.lock();
+    _writeBytes(byteArray);
+    _writeBytesMutex.unlock();
 }
 
-void LinkInterface::startMavlinkMessagesTimer(int vehicle_id) {
-    if (_mavlinkMessagesTimers.contains(vehicle_id)) {
-        _mavlinkMessagesTimers.value(vehicle_id)->restartTimer();
+void LinkInterface::addVehicleReference(void)
+{
+    _vehicleReferenceCount++;
+}
+
+void LinkInterface::removeVehicleReference(void)
+{
+    if (_vehicleReferenceCount != 0) {
+        _vehicleReferenceCount--;
+        if (_vehicleReferenceCount == 0) {
+            disconnect();
+        }
     } else {
-        _mavlinkMessagesTimers.insert(vehicle_id, new MavlinkMessagesTimer(vehicle_id, _highLatency));
-        QObject::connect(_mavlinkMessagesTimers.value(vehicle_id), &MavlinkMessagesTimer::activeChanged, this, &LinkInterface::_activeChanged);
-        _mavlinkMessagesTimers.value(vehicle_id)->init();
+        qWarning() << "LinkInterface::removeVehicleReference called with no vehicle references";
     }
 }
 
-void LinkInterface::stopMavlinkMessagesTimer() {
-    QMapIterator<int /* vehicle id */, MavlinkMessagesTimer*> iter(_mavlinkMessagesTimers);
-    while (iter.hasNext()) {
-        iter.next();
-        QObject::disconnect(iter.value(), &MavlinkMessagesTimer::activeChanged, this, &LinkInterface::_activeChanged);
-        _mavlinkMessagesTimers[iter.key()]->deleteLater();
-        _mavlinkMessagesTimers[iter.key()] = nullptr;
+void LinkInterface::_connectionRemoved(void)
+{
+    if (_vehicleReferenceCount == 0) {
+        // Since there are no vehicles on the link we can disconnect it right now
+        disconnect();
+    } else {
+        // If there are still vehicles on this link we allow communication lost to trigger and don't automatically disconect until all the vehicles go away
     }
-
-    _mavlinkMessagesTimers.clear();
 }
 
-void LinkInterface::writeBytesThreadSafe(const char *bytes, int length)
+#ifdef UNITTEST_BUILD
+#include "MockLink.h"
+bool LinkInterface::isMockLink(void)
 {
-    QByteArray byteArray(bytes, length);
-    _writeBytesMutex.lock();
-    _writeBytes(byteArray);
-    _writeBytesMutex.unlock();
+    return dynamic_cast<MockLink*>(this);
 }
+#endif