Merge pull request #6377 from acfloria/feature/primary_branch

Support for multiple links to a single vehicle

qgroundcontrol.pro

@@ -393,12 +393,14 @@ HEADERS += \
     src/api/QGCOptions.h \
     src/api/QGCSettings.h \
     src/api/QmlComponentInfo.h \
+    src/comm/HeartbeatTimer.h
 
 SOURCES += \
     src/api/QGCCorePlugin.cc \
     src/api/QGCOptions.cc \
     src/api/QGCSettings.cc \
     src/api/QmlComponentInfo.cc \
+    src/comm/HeartbeatTimer.cc
 
 #
 # Unit Test specific configuration goes here (requires full debug build with all plugins)

qgroundcontrol.qrc

@@ -9,10 +9,11 @@
         <file alias="GPSRTKIndicator.qml">src/ui/toolbar/GPSRTKIndicator.qml</file>
         <file alias="MessageIndicator.qml">src/ui/toolbar/MessageIndicator.qml</file>
         <file alias="ModeIndicator.qml">src/ui/toolbar/ModeIndicator.qml</file>
-		<file alias="VTOLModeIndicator.qml">src/ui/toolbar/VTOLModeIndicator.qml</file>
+        <file alias="VTOLModeIndicator.qml">src/ui/toolbar/VTOLModeIndicator.qml</file>
         <file alias="RCRSSIIndicator.qml">src/ui/toolbar/RCRSSIIndicator.qml</file>
         <file alias="TelemetryRSSIIndicator.qml">src/ui/toolbar/TelemetryRSSIIndicator.qml</file>
         <file alias="JoystickIndicator.qml">src/ui/toolbar/JoystickIndicator.qml</file>
+        <file alias="LinkIndicator.qml">src/ui/toolbar/LinkIndicator.qml</file>
     </qresource>
     <qresource prefix="/qml">
         <file alias="CorridorScanEditor.qml">src/PlanView/CorridorScanEditor.qml</file>
@@ -126,7 +127,7 @@
         <file alias="QGroundControl/Controls/SliderSwitch.qml">src/QmlControls/SliderSwitch.qml</file>
         <file alias="QGroundControl/Controls/SubMenuButton.qml">src/QmlControls/SubMenuButton.qml</file>
         <file alias="QGroundControl/Controls/SurveyMapVisual.qml">src/PlanView/SurveyMapVisual.qml</file>
-		<file alias="QGroundControl/Controls/TransectStyleComplexItemStats.qml">src/PlanView/TransectStyleComplexItemStats.qml</file>
+        <file alias="QGroundControl/Controls/TransectStyleComplexItemStats.qml">src/PlanView/TransectStyleComplexItemStats.qml</file>
         <file alias="QGroundControl/Controls/ToolStrip.qml">src/QmlControls/ToolStrip.qml</file>
         <file alias="QGroundControl/Controls/VehicleRotationCal.qml">src/QmlControls/VehicleRotationCal.qml</file>
         <file alias="QGroundControl/Controls/VehicleSummaryRow.qml">src/QmlControls/VehicleSummaryRow.qml</file>
@@ -139,7 +140,7 @@
         <file alias="QGroundControl/FactControls/FactTextField.qml">src/FactSystem/FactControls/FactTextField.qml</file>
         <file alias="QGroundControl/FactControls/FactTextFieldGrid.qml">src/FactSystem/FactControls/FactTextFieldGrid.qml</file>
         <file alias="QGroundControl/FactControls/FactTextFieldRow.qml">src/FactSystem/FactControls/FactTextFieldRow.qml</file>
-		<file alias="QGroundControl/FactControls/FactValueSlider.qml">src/FactSystem/FactControls/FactValueSlider.qml</file>
+        <file alias="QGroundControl/FactControls/FactValueSlider.qml">src/FactSystem/FactControls/FactValueSlider.qml</file>
         <file alias="QGroundControl/FactControls/qmldir">src/FactSystem/FactControls/qmldir</file>
         <file alias="QGroundControl/FlightDisplay/FlightDisplayView.qml">src/FlightDisplay/FlightDisplayView.qml</file>
         <file alias="QGroundControl/FlightDisplay/FlightDisplayViewMap.qml">src/FlightDisplay/FlightDisplayViewMap.qml</file>
@@ -225,10 +226,10 @@
         <file alias="USBBoardInfo.json">src/comm/USBBoardInfo.json</file>
         <file alias="Vehicle/BatteryFact.json">src/Vehicle/BatteryFact.json</file>
         <file alias="Vehicle/ClockFact.json">src/Vehicle/ClockFact.json</file>
-		<file alias="Vehicle/DistanceSensorFact.json">src/Vehicle/DistanceSensorFact.json</file>
+        <file alias="Vehicle/DistanceSensorFact.json">src/Vehicle/DistanceSensorFact.json</file>
         <file alias="Vehicle/GPSFact.json">src/Vehicle/GPSFact.json</file>
         <file alias="Vehicle/GPSRTKFact.json">src/Vehicle/GPSRTKFact.json</file>
-		<file alias="Vehicle/SetpointFact.json">src/Vehicle/SetpointFact.json</file>
+        <file alias="Vehicle/SetpointFact.json">src/Vehicle/SetpointFact.json</file>
         <file alias="Vehicle/SubmarineFact.json">src/Vehicle/SubmarineFact.json</file>
         <file alias="Vehicle/TemperatureFact.json">src/Vehicle/TemperatureFact.json</file>
         <file alias="Vehicle/VehicleFact.json">src/Vehicle/VehicleFact.json</file>

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -344,6 +344,7 @@ const QVariantList &FirmwarePlugin::toolBarIndicators(const Vehicle* vehicle)
         _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/VTOLModeIndicator.qml")));
         _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ArmedIndicator.qml")));
         _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPSRTKIndicator.qml")));
+        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/LinkIndicator.qml")));
     }
     return _toolBarIndicatorList;
 }

src/QGCApplication.cc

@@ -361,6 +361,7 @@ void QGCApplication::_initCommon(void)
     qmlRegisterUncreatableType<ParameterManager>    ("QGroundControl.Vehicle",              1, 0, "ParameterManager",       "Reference only");
     qmlRegisterUncreatableType<QGCCameraManager>    ("QGroundControl.Vehicle",              1, 0, "QGCCameraManager",       "Reference only");
     qmlRegisterUncreatableType<QGCCameraControl>    ("QGroundControl.Vehicle",              1, 0, "QGCCameraControl",       "Reference only");
+    qmlRegisterUncreatableType<LinkInterface>       ("QGroundControl.Vehicle",              1, 0, "LinkInterface",          "Reference only");
     qmlRegisterUncreatableType<JoystickManager>     ("QGroundControl.JoystickManager",      1, 0, "JoystickManager",        "Reference only");
     qmlRegisterUncreatableType<Joystick>            ("QGroundControl.JoystickManager",      1, 0, "Joystick",               "Reference only");
     qmlRegisterUncreatableType<QGCPositionManager>  ("QGroundControl.QGCPositionManager",   1, 0, "QGCPositionManager",     "Reference only");

src/Vehicle/MultiVehicleManager.cc

@@ -138,9 +138,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle
         setActiveVehicle(vehicle);
     }
 
-    // Mark link as active
-    link->setActive(true);
-
 #if defined (__ios__) || defined(__android__)
     if(_vehicles.count() == 1) {
         //-- Once a vehicle is connected, keep screen from going off

src/Vehicle/Vehicle.h

@@ -10,6 +10,7 @@
 #pragma once
 
 #include <QObject>
+#include <QVariantList>
 #include <QGeoCoordinate>
 
 #include "FactGroup.h"
@@ -493,6 +494,8 @@ public:
     Q_PROPERTY(bool                 vtolInFwdFlight         READ vtolInFwdFlight        WRITE setVtolInFwdFlight        NOTIFY vtolInFwdFlightChanged)
     Q_PROPERTY(bool                 highLatencyLink         READ highLatencyLink                                        NOTIFY highLatencyLinkChanged)
     Q_PROPERTY(bool                 supportsTerrainFrame    READ supportsTerrainFrame                                   NOTIFY firmwareTypeChanged)
+    Q_PROPERTY(QString              priorityLinkName        READ priorityLinkName       WRITE setPriorityLinkByName     NOTIFY priorityLinkNameChanged)
+    Q_PROPERTY(QVariantList         links                   READ links                                                  NOTIFY linksChanged)
 
     // Vehicle state used for guided control
     Q_PROPERTY(bool flying                  READ flying NOTIFY flyingChanged)                               ///< Vehicle is flying
@@ -501,7 +504,7 @@ public:
     Q_PROPERTY(bool guidedModeSupported     READ guidedModeSupported CONSTANT)                              ///< Guided mode commands are supported by this vehicle
     Q_PROPERTY(bool pauseVehicleSupported   READ pauseVehicleSupported CONSTANT)                            ///< Pause vehicle command is supported
     Q_PROPERTY(bool orbitModeSupported      READ orbitModeSupported CONSTANT)                               ///< Orbit mode is supported by this vehicle
-    Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT)                          ///< Guided takeoff supported    
+    Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT)                          ///< Guided takeoff supported
 
     Q_PROPERTY(ParameterManager* parameterManager READ parameterManager CONSTANT)
 
@@ -692,6 +695,10 @@ public:
     QString flightMode(void) const;
     void setFlightMode(const QString& flightMode);
 
+    QString priorityLinkName(void) const;
+    QVariantList links(void) const;
+    void setPriorityLinkByName(const QString& priorityLinkName);
+
     bool hilMode(void);
     void setHilMode(bool hilMode);
 
@@ -943,6 +950,9 @@ signals:
     void capabilityBitsChanged(uint64_t capabilityBits);
     void toolBarIndicatorsChanged(void);
     void highLatencyLinkChanged(bool highLatencyLink);
+    void priorityLinkNameChanged(const QString& priorityLinkName);
+    void linksChanged(void);
+    void linksPropertiesChanged(void);
 
     void messagesReceivedChanged    ();
     void messagesSentChanged        ();
@@ -1024,7 +1034,7 @@ private slots:
     void _offlineVehicleTypeSettingChanged(QVariant value);
     void _offlineCruiseSpeedSettingChanged(QVariant value);
     void _offlineHoverSpeedSettingChanged(QVariant value);
-    void _updateHighLatencyLink(void);
+    void _updateHighLatencyLink(bool sendCommand = true);
 
     void _handleTextMessage                 (int newCount);
     void _handletextMessageReceived         (UASMessage* message);
@@ -1034,7 +1044,6 @@ private slots:
     void _updateAttitude                    (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
     /** @brief A new camera image has arrived */
     void _imageReady                        (UASInterface* uas);
-    void _connectionLostTimeout(void);
     void _prearmErrorTimeout(void);
     void _missionLoadComplete(void);
     void _geoFenceLoadComplete(void);
@@ -1092,14 +1101,14 @@ private:
     void _rallyPointManagerError(int errorCode, const QString& errorMsg);
     void _mapTrajectoryStart(void);
     void _mapTrajectoryStop(void);
-    void _connectionActive(void);
+    void _linkActiveChanged(LinkInterface* link, bool active, int vehicleID);
     void _say(const QString& text);
     QString _vehicleIdSpeech(void);
     void _handleMavlinkLoggingData(mavlink_message_t& message);
     void _handleMavlinkLoggingDataAcked(mavlink_message_t& message);
     void _ackMavlinkLogData(uint16_t sequence);
     void _sendNextQueuedMavCommand(void);
-    void _updatePriorityLink(void);
+    void _updatePriorityLink(bool updateActive, bool sendCommand);
     void _commonInit(void);
     void _startPlanRequest(void);
     void _setupAutoDisarmSignalling(void);
@@ -1191,8 +1200,6 @@ private:
     // Lost connection handling
     bool                _connectionLost;
     bool                _connectionLostEnabled;
-    static const int    _connectionLostTimeoutMSecs = 3500;  // Signal connection lost after 3.5 seconds of missed heartbeat
-    QTimer              _connectionLostTimer;
 
     bool                _initialPlanRequestComplete;
 
@@ -1266,6 +1273,7 @@ private:
     int _lastAnnouncedLowBatteryPercent;
 
     SharedLinkInterfacePointer _priorityLink;  // We always keep a reference to the priority link to manage shutdown ordering
+    bool _priorityLinkCommanded;
 
     // FactGroup facts
 

src/comm/HeartbeatTimer.cc

+/****************************************************************************
+ *
+ *   (c) 2009-2018 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 "HeartbeatTimer.h"
+
+#include <QDebug>
+
+HeartbeatTimer::HeartbeatTimer(int vehicle_id, bool high_latency) :
+    _active(true),
+    _timer(new QTimer),
+    _vehicleID(vehicle_id),
+    _high_latency(high_latency)
+{
+    if (!high_latency) {
+        _timer->setInterval(_heartbeatReceivedTimeoutMSecs);
+        _timer->setSingleShot(true);
+        _timer->start();
+    }
+    emit activeChanged(true, _vehicleID);
+    QObject::connect(_timer, &QTimer::timeout, this, &HeartbeatTimer::timerTimeout);
+}
+
+HeartbeatTimer::~HeartbeatTimer() {
+    if (_timer) {
+        QObject::disconnect(_timer, &QTimer::timeout, this, &HeartbeatTimer::timerTimeout);
+        _timer->stop();
+        delete _timer;
+        _timer = nullptr;
+    }
+
+    emit activeChanged(false, _vehicleID);
+}
+
+void HeartbeatTimer::restartTimer()
+{
+    if (!_active) {
+        _active = true;
+        emit activeChanged(true, _vehicleID);
+    }
+
+    _timer->start();
+}
+
+void HeartbeatTimer::timerTimeout()
+{
+    if (!_high_latency) {
+        if (_active) {
+            _active = false;
+            emit activeChanged(false, _vehicleID);
+        }
+        emit heartbeatTimeout(_vehicleID);
+    }
+}

src/comm/HeartbeatTimer.h

+/****************************************************************************
+ *
+ *   (c) 2009-2018 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.
+ *
+ ****************************************************************************/
+
+#ifndef _HEARTBEATTIMER_H_
+#define _HEARTBEATTIMER_H_
+
+#include <QTimer>
+#include <QObject>
+
+/**
+ * @brief The HeartbeatTimer class
+ *
+ * Track the heartbeat for a single vehicle on one link.
+ * As long as regular heartbeats are received the status is active. On the timer timeout
+ * status is set to inactive. On any status change the activeChanged signal is emitted.
+ * If high_latency is true then active is always true.
+ */
+class HeartbeatTimer : public QObject
+{
+    Q_OBJECT
+
+public:
+    /**
+     * @brief HeartbeatTimer
+     *
+     * Constructor
+     *
+     * @param vehicle_id: The vehicle ID for which the heartbeat will be tracked.
+     * @param high_latency: Indicates if the link is a high latency one.
+     */
+    HeartbeatTimer(int vehicle_id, bool high_latency);
+
+    ~HeartbeatTimer();
+
+    /**
+     * @brief getActive
+     * @return The current value of active
+     */
+    bool getActive() const { return _active; }
+
+    /**
+     * @brief getVehicleID
+     * @return The vehicle ID
+     */
+    int getVehicleID() const { return _vehicleID; }
+
+    /**
+     * @brief restartTimer
+     *
+     * Restarts the timer and emits the signal if the timer was previously inactive
+     */
+    void restartTimer();
+
+signals:
+    /**
+     * @brief heartbeatTimeout
+     *
+     * Emitted if no heartbeat is received over the specified time.
+     *
+     * @param vehicle_id: The vehicle ID for which the heartbeat timed out.
+     */
+    void heartbeatTimeout(int vehicle_id);
+
+    /**
+     * @brief activeChanged
+     *
+     * Emitted if the active status changes.
+     *
+     * @param active: The new value of the active state.
+     * @param vehicle_id: The vehicle ID for which the active state changed.
+     */
+    void activeChanged(bool active, int vehicle_id);
+private slots:
+    /**
+     * @brief timerTimeout
+     *
+     * Handle the timer timeout.
+     *
+     * Emit the signals according to the current state for regular links.
+     * Do nothing for a high latency link.
+     */
+    void timerTimeout();
+
+private:
+    bool _active = false; // The state of active. Is true if the timer has not timed out.
+    QTimer* _timer = nullptr; // Single shot timer
+    int _vehicleID = -1; // Vehicle ID for which the heartbeat is tracked.
+    bool _high_latency = false; // Indicates if the link is a high latency link or not.
+
+    static const int    _heartbeatReceivedTimeoutMSecs = 3500;  // Signal connection lost after 3.5 seconds of no messages
+};
+
+#endif // _HEARTBEATTIMER_H_

src/comm/LinkInterface.cc

@@ -10,6 +10,28 @@
 #include "LinkInterface.h"
 #include "QGCApplication.h"
 
+bool LinkInterface::active() const
+{
+    QMapIterator<int /* vehicle id */, HeartbeatTimer*> iter(_heartbeatTimers);
+    while (iter.hasNext()) {
+        iter.next();
+        if (iter.value()->getActive()) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+bool LinkInterface::link_active(int vehicle_id) const
+{
+    if (_heartbeatTimers.contains(vehicle_id)) {
+        return _heartbeatTimers.value(vehicle_id)->getActive();
+    } else {
+        return false;
+    }
+}
+
 /// 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
@@ -25,10 +47,11 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config)
     , _config                   (config)
     , _highLatency              (config->isHighLatency())
     , _mavlinkChannelSet        (false)
-    , _active                   (false)
     , _enableRateCollection     (false)
     , _decodedFirstMavlinkPacket(false)
 {
+    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
+
     _config->setLink(this);
 
     // Initialize everything for the data rate calculation buffers.
@@ -159,3 +182,29 @@ void LinkInterface::_setMavlinkChannel(uint8_t channel)
     _mavlinkChannelSet = true;
     _mavlinkChannel = channel;
 }
+
+void LinkInterface::_activeChanged(bool active, int vehicle_id)
+{
+    emit activeChanged(this, active, vehicle_id);
+}
+
+void LinkInterface::startHeartbeatTimer(int vehicle_id) {
+    if (_heartbeatTimers.contains(vehicle_id)) {
+        _heartbeatTimers.value(vehicle_id)->restartTimer();
+    } else {
+        _heartbeatTimers.insert(vehicle_id, new HeartbeatTimer(vehicle_id, _highLatency));
+        QObject::connect(_heartbeatTimers.value(vehicle_id), &HeartbeatTimer::activeChanged, this, &LinkInterface::_activeChanged);
+    }
+}
+
+void LinkInterface::stopHeartbeatTimer() {
+    QMapIterator<int /* vehicle id */, HeartbeatTimer*> iter(_heartbeatTimers);
+    while (iter.hasNext()) {
+        iter.next();
+        QObject::disconnect(iter.value(), &HeartbeatTimer::activeChanged, this, &LinkInterface::_activeChanged);
+        delete _heartbeatTimers[iter.key()];
+        _heartbeatTimers[iter.key()] = nullptr;
+    }
+
+    _heartbeatTimers.clear();
+}

src/comm/LinkInterface.h

@@ -17,9 +17,11 @@
 #include <QMetaType>
 #include <QSharedPointer>
 #include <QDebug>
+#include <QTimer>
 
 #include "QGCMAVLink.h"
 #include "LinkConfiguration.h"
+#include "HeartbeatTimer.h"
 
 class LinkManager;
 
@@ -36,13 +38,17 @@ class LinkInterface : public QThread
     friend class LinkManager;
 
 public:    
-    ~LinkInterface() { _config->setLink(NULL); }
+    virtual ~LinkInterface() {
+        stopHeartbeatTimer();
+        _config->setLink(NULL);
+    }
 
-    Q_PROPERTY(bool active      READ active         WRITE setActive         NOTIFY activeChanged)
+    Q_PROPERTY(bool active      READ active                                 NOTIFY activeChanged)
 
     // Property accessors
-    bool active(void)           { return _active; }
-    void setActive(bool active) { _active = active; emit activeChanged(active); }
+    bool active() const;
+    Q_INVOKABLE bool link_active(int vehicle_id) const;
+    Q_INVOKABLE bool getHighLatency(void) const { return _highLatency; }
 
     LinkConfiguration* getLinkConfiguration(void) { return _config.data(); }
 
@@ -51,7 +57,7 @@ public:
     /**
      * @brief Get the human readable name of this link
      */
-    virtual QString getName() const = 0;
+    Q_INVOKABLE virtual QString getName() const = 0;
 
     virtual void requestReset() = 0;
 
@@ -149,10 +155,12 @@ public slots:
 
 private slots:
     virtual void _writeBytes(const QByteArray) = 0;
+
+    void _activeChanged(bool active, int vehicle_id);
     
 signals:
     void autoconnectChanged(bool autoconnect);
-    void activeChanged(bool active);
+    void activeChanged(LinkInterface* link, bool active, int vehicle_id);
     void _invokeWriteBytes(QByteArray);
     void highLatencyChanged(bool highLatency);
 
@@ -248,10 +256,25 @@ private:
     virtual bool _connect(void) = 0;
 
     virtual void _disconnect(void) = 0;
-    
+
     /// Sets the mavlink channel to use for this link
     void _setMavlinkChannel(uint8_t channel);
     
+    /**
+     * @brief startHeartbeatTimer
+     *
+     * Start/restart the heartbeat timer for the specific vehicle.
+     * If no timer exists an instance is allocated.
+     */
+    void startHeartbeatTimer(int vehicle_id);
+
+    /**
+     * @brief stopHeartbeatTimer
+     *
+     * Stop and deallocate the heartbeat timers for all vehicles if any exists.
+     */
+    void stopHeartbeatTimer();
+
     bool _mavlinkChannelSet;    ///< true: _mavlinkChannel has been set
     uint8_t _mavlinkChannel;    ///< mavlink channel to use for this link, as used by mavlink_parse_char
     
@@ -273,9 +296,10 @@ private:
     
     mutable QMutex _dataRateMutex; // Mutex for accessing the data rate member variables
 
-    bool _active;                       ///< true: link is actively receiving mavlink messages
     bool _enableRateCollection;
     bool _decodedFirstMavlinkPacket;    ///< true: link has correctly decoded it's first mavlink packet
+
+    QMap<int /* vehicle id */, HeartbeatTimer*> _heartbeatTimers;
 };
 
 typedef QSharedPointer<LinkInterface> SharedLinkInterfacePointer;

src/comm/LinkManager.cc

@@ -80,6 +80,8 @@ void LinkManager::setToolbox(QGCToolbox *toolbox)
     _autoConnectSettings = toolbox->settingsManager()->autoConnectSettings();
     _mavlinkProtocol = _toolbox->mavlinkProtocol();
 
+    connect(_mavlinkProtocol, &MAVLinkProtocol::vehicleHeartbeatInfo, this, &LinkManager::_heartbeatReceived);
+
     connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateAutoConnectLinks);
     _portListTimer.start(_autoconnectUpdateTimerMSecs); // timeout must be long enough to get past bootloader on second pass
 
@@ -172,7 +174,7 @@ LinkInterface* LinkManager::createConnectedLink(const QString& name)
 void LinkManager::_addLink(LinkInterface* link)
 {
     if (thread() != QThread::currentThread()) {
-        qWarning() << "_deleteLink called from incorrect thread";
+        qWarning() << "_addLink called from incorrect thread";
         return;
     }
 
@@ -338,6 +340,7 @@ void LinkManager::saveLinkConfigurationList()
                 settings.setValue(root + "/name", linkConfig->name());
                 settings.setValue(root + "/type", linkConfig->type());
                 settings.setValue(root + "/auto", linkConfig->isAutoConnect());
+                settings.setValue(root + "/high_latency", linkConfig->isHighLatency());
                 // Have the instance save its own values
                 linkConfig->saveSettings(settings, root);
             }
@@ -369,6 +372,7 @@ void LinkManager::loadLinkConfigurationList()
                         if(!name.isEmpty()) {
                             LinkConfiguration* pLink = NULL;
                             bool autoConnect = settings.value(root + "/auto").toBool();
+                            bool highLatency = settings.value(root + "/high_latency").toBool();
                             switch((LinkConfiguration::LinkType)type) {
 #ifndef NO_SERIAL_LINK
                             case LinkConfiguration::TypeSerial:
@@ -403,6 +407,7 @@ void LinkManager::loadLinkConfigurationList()
                             if(pLink) {
                                 //-- Have the instance load its own values
                                 pLink->setAutoConnect(autoConnect);
+                                pLink->setHighLatency(highLatency);
                                 pLink->loadSettings(settings, root);
                                 addConfiguration(pLink);
                                 linksChanged = true;
@@ -1000,3 +1005,11 @@ void LinkManager::_freeMavlinkChannel(int channel)
 {
     _mavlinkChannelsUsedBitMask &= ~(1 << channel);
 }
+
+void LinkManager::_heartbeatReceived(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType) {
+    Q_UNUSED(componentId);
+    Q_UNUSED(vehicleFirmwareType);
+    Q_UNUSED(vehicleType);
+
+    link->startHeartbeatTimer(vehicleId);
+}

src/comm/LinkManager.h

@@ -204,6 +204,8 @@ private:
     SerialConfiguration* _autoconnectConfigurationsContainsPort(const QString& portName);
 #endif
 
+    void _heartbeatReceived(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType);
+
     bool    _configUpdateSuspended;                     ///< true: stop updating configuration list
     bool    _configurationsLoaded;                      ///< true: Link configurations have been loaded
     bool    _connectionsSuspended;                      ///< true: all new connections should not be allowed

src/comm/SerialLink.cc

@@ -309,7 +309,7 @@ bool SerialLink::isConnected() const
 
 QString SerialLink::getName() const
 {
-    return _serialConfig->portName();
+    return _serialConfig->name();
 }
 
 /**

src/ui/toolbar/LinkIndicator.qml

+/****************************************************************************
+ *
+ *   (c) 2009-2016 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.
+ *
+ ****************************************************************************/
+
+
+import QtQuick          2.3
+import QtQuick.Controls 1.2
+
+import QGroundControl                       1.0
+import QGroundControl.Controls              1.0
+import QGroundControl.MultiVehicleManager   1.0
+import QGroundControl.ScreenTools           1.0
+import QGroundControl.Palette               1.0
+import QGroundControl.Vehicle               1.0
+
+//-------------------------------------------------------------------------
+//-- Link Indicator
+Item {
+    anchors.top:    parent.top
+    anchors.bottom: parent.bottom
+    width:          priorityLinkSelector.width
+
+    property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
+    property bool _visible: false
+
+    QGCLabel {
+        id:                     priorityLinkSelector
+        text:                   _activeVehicle ? _activeVehicle.priorityLinkName : qsTr("N/A", "No data to display")
+        font.pointSize:         ScreenTools.mediumFontPointSize
+        color:                  qgcPal.buttonText
+        anchors.verticalCenter: parent.verticalCenter
+        visible:                _visible
+        Menu {
+            id: linkSelectionMenu
+        }
+        Component {
+            id: linkSelectionMenuItemComponent
+            MenuItem {
+                onTriggered: _activeVehicle.priorityLinkName = text
+            }
+        }
+        property var linkSelectionMenuItems: []
+        function updatelinkSelectionMenu() {
+            if (_activeVehicle) {
+                // Remove old menu items
+                for (var i = 0; i < linkSelectionMenuItems.length; i++) {
+                    linkSelectionMenu.removeItem(linkSelectionMenuItems[i])
+                }
+                linkSelectionMenuItems.length = 0
+
+                // Add new items
+                var has_hl = false;
+                var links = _activeVehicle.links
+                for (var i = 0; i < links.length; i++) {
+                    var menuItem = linkSelectionMenuItemComponent.createObject(null, { "text": links[i].getName(), "enabled": links[i].link_active(_activeVehicle.id)})
+                    linkSelectionMenuItems.push(menuItem)
+                    linkSelectionMenu.insertItem(i, menuItem)
+
+                    if (links[i].getHighLatency()) {
+                        has_hl = true
+                    }
+                }
+
+                _visible = links.length > 1 && has_hl
+            }
+        }
+
+        Component.onCompleted: priorityLinkSelector.updatelinkSelectionMenu()
+
+        Connections {
+            target:                 QGroundControl.multiVehicleManager
+            onActiveVehicleChanged: priorityLinkSelector.updatelinkSelectionMenu()
+        }
+
+        Connections {
+            target:                 _activeVehicle
+            onLinksChanged:         priorityLinkSelector.updatelinkSelectionMenu()
+        }
+
+        Connections {
+            target:                     _activeVehicle
+            onLinksPropertiesChanged:   priorityLinkSelector.updatelinkSelectionMenu()
+        }
+
+        MouseArea {
+            visible:        _activeVehicle
+            anchors.fill:   parent
+            onClicked:      linkSelectionMenu.popup()
+        }
+    }
+}