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.cc

@@ -168,6 +168,7 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _gitHash(versionNotSetValue)
     , _uid(0)
     , _lastAnnouncedLowBatteryPercent(100)
+    , _priorityLinkCommanded(false)
     , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
@@ -191,8 +192,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _clockFactGroup(this)
     , _distanceSensorFactGroup(this)
 {
-    _addLink(link);
-
     connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadSettings);
     connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleAvailableChanged, this, &Vehicle::_loadSettings);
 
@@ -200,6 +199,8 @@ Vehicle::Vehicle(LinkInterface*             link,
 
     connect(_mavlink, &MAVLinkProtocol::messageReceived,     this, &Vehicle::_mavlinkMessageReceived);
 
+    _addLink(link);
+
     connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
     connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);
     connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
@@ -222,12 +223,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
     _prearmErrorTimer.setSingleShot(true);
 
-    // Connection Lost timer
-    _connectionLostTimer.setInterval(_connectionLostTimeoutMSecs);
-    _connectionLostTimer.setSingleShot(false);
-    _connectionLostTimer.start();
-    connect(&_connectionLostTimer, &QTimer::timeout, this, &Vehicle::_connectionLostTimeout);
-
     // Send MAV_CMD ack timer
     _mavCommandAckTimer.setSingleShot(true);
     _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
@@ -606,13 +601,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
         }
     }
 
-
-    // Mark this vehicle as active - but only if the traffic is coming from
-    // the actual vehicle
-    if (message.sysid == _id) {
-        _connectionActive();
-    }
-
     // Give the plugin a change to adjust the message contents
     if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
         return;
@@ -1160,6 +1148,7 @@ void Vehicle::_handleCommandLong(mavlink_message_t& message)
                 if (sl && sl->getSerialConfig()->usbDirect()) {
                     qDebug() << "Disconnecting:" << _links.at(i)->getName();
                     qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i));
+                    emit linksChanged();
                 }
             }
         }
@@ -1622,11 +1611,16 @@ void Vehicle::_addLink(LinkInterface* link)
     if (!_containsLink(link)) {
         qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16);
         _links += link;
-        _updatePriorityLink();
-        _updateHighLatencyLink();
+        if (_links.count() <= 1) {
+            _updatePriorityLink(true /* updateActive */, false /* sendCommand */);
+        } else {
+            _updatePriorityLink(true /* updateActive */, true /* sendCommand */);
+        }
+
         connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted);
         connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted);
         connect(link, &LinkInterface::highLatencyChanged, this, &Vehicle::_updateHighLatencyLink);
+        connect(link, &LinkInterface::activeChanged, this, &Vehicle::_linkActiveChanged);
     }
 }
 
@@ -1635,7 +1629,12 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
     qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count();
 
     _links.removeOne(link);
-    _updatePriorityLink();
+
+    if (_priorityLink.data() == link) {
+        _priorityLink.clear();
+    }
+
+    _updatePriorityLink(true /* updateActive */, true /* sendCommand */);
 
     if (_links.count() == 0 && !_allLinksInactiveSent) {
         qCDebug(VehicleLog) << "All links inactive";
@@ -1681,8 +1680,19 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
     emit messagesSentChanged();
 }
 
-void Vehicle::_updatePriorityLink(void)
+void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
 {
+    emit linksPropertiesChanged();
+
+    // if the priority link is commanded and still active don't change anything
+    if (_priorityLinkCommanded) {
+        if (_priorityLink.data()->link_active(_id)) {
+            return;
+        } else {
+            _priorityLinkCommanded = false;
+        }
+    }
+
     LinkInterface* newPriorityLink = NULL;
 
     // This routine specifically does not clear _priorityLink when there are no links remaining.
@@ -1695,7 +1705,7 @@ void Vehicle::_updatePriorityLink(void)
     // Check for the existing priority link to still be valid
     for (int i=0; i<_links.count(); i++) {
         if (_priorityLink.data() == _links[i]) {
-            if (!_priorityLink.data()->highLatency()) {
+            if (!_priorityLink.data()->highLatency() && _priorityLink->link_active(_id)) {
                 // Link is still valid. Continue to use it unless it is high latency. In that case we still look for a better
                 // link to use as priority link.
                 return;
@@ -1704,12 +1714,13 @@ void Vehicle::_updatePriorityLink(void)
     }
 
     // The previous priority link is no longer valid. We must no find the best link available in this priority order:
-    //      Direct USB connection
-    //      Not a high latency link
+    //      First active direct USB connection
+    //      Any active non high latency link
+    //      An active high latency link
     //      Any link
 
 #ifndef NO_SERIAL_LINK
-    // Search for direct usb connection
+    // Search for an active direct usb connection
     for (int i=0; i<_links.count(); i++) {
         LinkInterface* link = _links[i];
         SerialLink* pSerialLink = qobject_cast<SerialLink*>(link);
@@ -1718,7 +1729,7 @@ void Vehicle::_updatePriorityLink(void)
             if (config) {
                 SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(config);
                 if (pSerialConfig && pSerialConfig->usbDirect()) {
-                    if (_priorityLink.data() != link) {
+                    if (_priorityLink.data() != link && link->link_active(_id)) {
                         newPriorityLink = link;
                         break;
                     }
@@ -1730,10 +1741,21 @@ void Vehicle::_updatePriorityLink(void)
 #endif
 
     if (!newPriorityLink) {
-        // Search for non-high latency link
+        // Search for an active non-high latency link
         for (int i=0; i<_links.count(); i++) {
             LinkInterface* link = _links[i];
-            if (!link->highLatency()) {
+            if (!link->highLatency() && link->link_active(_id)) {
+                newPriorityLink = link;
+                break;
+            }
+        }
+    }
+
+    if (!newPriorityLink) {
+        // Search for an active high latency link
+        for (int i=0; i<_links.count(); i++) {
+            LinkInterface* link = _links[i];
+            if (link->highLatency() && link->link_active(_id)) {
                 newPriorityLink = link;
                 break;
             }
@@ -1745,8 +1767,19 @@ void Vehicle::_updatePriorityLink(void)
         newPriorityLink = _links[0];
     }
 
-    _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
-    _updateHighLatencyLink();
+    if (_priorityLink.data() != newPriorityLink) {
+        if (_priorityLink) {
+            qgcApp()->showMessage((tr("switch to %2 as priority link")).arg(newPriorityLink->getName()));
+        }
+        _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
+
+        _updateHighLatencyLink(sendCommand);
+
+        emit priorityLinkNameChanged(_priorityLink->getName());
+        if (updateActive) {
+            _linkActiveChanged(_priorityLink.data(), _priorityLink->link_active(_id), _id);
+        }
+    }
 }
 
 void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
@@ -2088,6 +2121,53 @@ void Vehicle::setFlightMode(const QString& flightMode)
     }
 }
 
+QString Vehicle::priorityLinkName(void) const
+{
+    if (_priorityLink) {
+      return _priorityLink->getName();
+    }
+
+    return "none";
+}
+
+QVariantList Vehicle::links(void) const {
+    QVariantList ret;
+
+    foreach( const auto &item, _links )
+        ret << QVariant::fromValue(item);
+
+    return ret;
+}
+
+void Vehicle::setPriorityLinkByName(const QString& priorityLinkName)
+{
+    if (!_priorityLink) {
+        return;
+    }
+
+    if (priorityLinkName == _priorityLink->getName()) {
+        // The link did not change
+        return;
+    }
+
+    LinkInterface* newPriorityLink = NULL;
+
+
+    for (int i=0; i<_links.count(); i++) {
+        if (_links[i]->getName() == priorityLinkName) {
+            newPriorityLink = _links[i];
+        }
+    }
+
+    if (newPriorityLink) {
+        _priorityLinkCommanded = true;
+        _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
+        _updateHighLatencyLink(true);
+        emit priorityLinkNameChanged(_priorityLink->getName());
+        _linkActiveChanged(_priorityLink.data(), _priorityLink->link_active(_id), _id);
+    }
+}
+
 bool Vehicle::hilMode(void)
 {
     return _base_mode & MAV_MODE_FLAG_HIL_ENABLED;
@@ -2332,7 +2412,6 @@ void Vehicle::disconnectInactiveVehicle(void)
 {
     // Vehicle is no longer communicating with us, disconnect all links
 
-
     LinkManager* linkMgr = _toolbox->linkManager();
     for (int i=0; i<_links.count(); i++) {
         // FIXME: This linkInUse check is a hack fix for multiple vehicles on the same link.
@@ -2341,6 +2420,7 @@ void Vehicle::disconnectInactiveVehicle(void)
             linkMgr->disconnectLink(_links[i]);
         }
     }
+    emit linksChanged();
 }
 
 void Vehicle::_imageReady(UASInterface*)
@@ -2388,43 +2468,62 @@ void Vehicle::setConnectionLostEnabled(bool connectionLostEnabled)
     }
 }
 
-void Vehicle::_connectionLostTimeout(void)
+void Vehicle::_linkActiveChanged(LinkInterface *link, bool active, int vehicleID)
 {
-    if (highLatencyLink()) {
-        // No connection timeout on high latency links
+    // only continue if the vehicle id is correct
+    if (vehicleID != _id) {
         return;
     }
 
-    if (_connectionLostEnabled && !_connectionLost) {
-        _connectionLost = true;
-        _heardFrom = false;
-        _maxProtoVersion = 0;
-        emit connectionLostChanged(true);
-        _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech()));
-        if (_autoDisconnect) {
+    emit linksPropertiesChanged();
 
-            // Reset link state
-            for (int i = 0; i < _links.length(); i++) {
-                _mavlink->resetMetadataForLink(_links.at(i));
+    if (link == _priorityLink) {
+        if (active && _connectionLost) {
+            // communication to priority link regained
+            _connectionLost = false;
+            emit connectionLostChanged(false);
+            qgcApp()->showMessage((tr("%1 communication to %2 link %3 regained")).arg(_vehicleIdSpeech()).arg((_links.count() > 1) ? "priority" : "").arg(link->getName()));
+
+            if (_priorityLink->highLatency()) {
+                _setMaxProtoVersion(100);
+            } else {
+                // Re-negotiate protocol version for the link
+                sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
+                               MAV_CMD_REQUEST_PROTOCOL_VERSION,
+                               false,                                   // No error shown if fails
+                               1);                                     // Request protocol version
             }
-            disconnectInactiveVehicle();
-        }
-    }
-}
 
-void Vehicle::_connectionActive(void)
-{
-    _connectionLostTimer.start();
-    if (_connectionLost) {
-        _connectionLost = false;
-        emit connectionLostChanged(false);
-        _say(QString(tr("%1 communication regained")).arg(_vehicleIdSpeech()));
+        } else if (!active && !_connectionLost) {
+            // communication to priority link lost
+            qgcApp()->showMessage((tr("%1 communication to %2 link %3 lost")).arg(_vehicleIdSpeech()).arg((_links.count() > 1) ? "priority" : "").arg(link->getName()));
 
-        // Re-negotiate protocol version for the link
-        sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
-                       MAV_CMD_REQUEST_PROTOCOL_VERSION,
-                       false,                                   // No error shown if fails
-                       1);                                     // Request protocol version
+            _updatePriorityLink(false /* updateActive */, true /* sendCommand */);
+
+            if (link == _priorityLink) {
+                _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech()));
+                qgcApp()->showMessage((tr("%1 communication lost")).arg(_vehicleIdSpeech()));
+
+                if (_connectionLostEnabled) {
+                    _connectionLost = true;
+                    _heardFrom = false;
+                    _maxProtoVersion = 0;
+                    emit connectionLostChanged(true);
+
+                    if (_autoDisconnect) {
+                        // Reset link state
+                        for (int i = 0; i < _links.length(); i++) {
+                            _mavlink->resetMetadataForLink(_links.at(i));
+                        }
+
+                        disconnectInactiveVehicle();
+                    }
+                }
+            }
+        }
+    } else {
+        qgcApp()->showMessage((tr("%1 communication to auxiliary link %2 %3")).arg(_vehicleIdSpeech()).arg(link->getName()).arg(active ? "regained" : "lost"));
+        _updatePriorityLink(false /* updateActive */, true /* sendCommand */);
     }
 }
 
@@ -3300,12 +3399,23 @@ void Vehicle::_vehicleParamLoaded(bool ready)
     }
 }
 
-void Vehicle::_updateHighLatencyLink(void)
+void Vehicle::_updateHighLatencyLink(bool sendCommand)
 {
+    if (!_priorityLink) {
+        return;
+    }
+
     if (_priorityLink->highLatency() != _highLatencyLink) {
         _highLatencyLink = _priorityLink->highLatency();
         _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
         emit highLatencyLinkChanged(_highLatencyLink);
+
+        if (sendCommand) {
+            sendMavCommand(defaultComponentId(),
+                           MAV_CMD_CONTROL_HIGH_LATENCY,
+                           true,
+                           _highLatencyLink ? 1.0f : 0.0f); // request start/stop transmitting over high latency telemetry
+        }
     }
 }
 

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()
+        }
+    }
+}