Update active property for each link separately

Each link updates the active property based on incoming bytes.
The vehicle tracks the active property of each link and in case of an inactive priority link it updates the priority link.

src/Vehicle/Vehicle.cc

@@ -222,12 +222,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 +600,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;
@@ -1622,11 +1609,12 @@ void Vehicle::_addLink(LinkInterface* link)
     if (!_containsLink(link)) {
         qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16);
         _links += link;
-        _updatePriorityLink();
+        _updatePriorityLink(true);
         _updateHighLatencyLink();
         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 +1623,7 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
     qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count();
 
     _links.removeOne(link);
-    _updatePriorityLink();
+    _updatePriorityLink(true);
 
     if (_links.count() == 0 && !_allLinksInactiveSent) {
         qCDebug(VehicleLog) << "All links inactive";
@@ -1681,7 +1669,7 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
     emit messagesSentChanged();
 }
 
-void Vehicle::_updatePriorityLink(void)
+void Vehicle::_updatePriorityLink(bool updateActive)
 {
     emit linkNamesChanged();
     LinkInterface* newPriorityLink = NULL;
@@ -1696,7 +1684,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->active()) {
                 // 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;
@@ -1705,12 +1693,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);
@@ -1719,7 +1708,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->active()) {
                         newPriorityLink = link;
                         break;
                     }
@@ -1731,10 +1720,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() && link->active()) {
+                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()) {
+            if (link->highLatency() && link->active()) {
                 newPriorityLink = link;
                 break;
             }
@@ -1746,9 +1746,14 @@ void Vehicle::_updatePriorityLink(void)
         newPriorityLink = _links[0];
     }
 
+    if (_priorityLink.data() != newPriorityLink) {
         _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
         _updateHighLatencyLink();
         emit priorityLinkNameChanged(_priorityLink->getName());
+        if (updateActive) {
+            _linkActiveChanged(_priorityLink.data(), _priorityLink->active());
+        }
+    }
 }
 
 void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
@@ -2125,6 +2130,7 @@ void Vehicle::setPriorityLinkByName(const QString& priorityLinkName)
         _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
         _updateHighLatencyLink();
         emit priorityLinkNameChanged(_priorityLink->getName());
+        _linkActiveChanged(_priorityLink.data(), _priorityLink->active());
     }
 }
 
@@ -2428,21 +2434,36 @@ void Vehicle::setConnectionLostEnabled(bool connectionLostEnabled)
     }
 }
 
-void Vehicle::_connectionLostTimeout(void)
+void Vehicle::_linkActiveChanged(LinkInterface *link, bool active)
 {
-    if (highLatencyLink()) {
-        // No connection timeout on high latency links
-        return;
-    }
+    qWarning() << "Vehicle active changed called";
+    if (link == _priorityLink) {
+        if (active && _connectionLost) {
+            // communication to priority link regained
+            _connectionLost = false;
+            _say(QString(tr("%1 communication to priority link %2 regained")).arg(_vehicleIdSpeech()).arg(link->getName()));
 
-    if (_connectionLostEnabled && !_connectionLost) {
+            // 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
+
+        } else if (!active && !_connectionLost) {
+            // communication to priority link lost
+            _say(QString(tr("%1 communication to priority link %2 lost")).arg(_vehicleIdSpeech()).arg(link->getName()));
+            _updatePriorityLink(false);
+
+            if (link == _priorityLink) {
+                 _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech()));
+
+                if (_connectionLostEnabled) {
                     _connectionLost = true;
                     _heardFrom = false;
                     _maxProtoVersion = 0;
                     emit connectionLostChanged(true);
-        _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech()));
-        if (_autoDisconnect) {
 
+                    if (_autoDisconnect) {
                         // Reset link state
                         for (int i = 0; i < _links.length(); i++) {
                             _mavlink->resetMetadataForLink(_links.at(i));
@@ -2450,21 +2471,14 @@ void Vehicle::_connectionLostTimeout(void)
                         disconnectInactiveVehicle();
                     }
                 }
-}
 
-void Vehicle::_connectionActive(void)
-{
-    _connectionLostTimer.start();
-    if (_connectionLost) {
-        _connectionLost = false;
-        emit connectionLostChanged(false);
-        _say(QString(tr("%1 communication regained")).arg(_vehicleIdSpeech()));
-
-        // 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
+            } else {
+                 _say(QString(tr("%1 use %2 as the priority link")).arg(_vehicleIdSpeech()).arg(link->getName()));
+                // nothing more to do
+            }
+        }
+    } else {
+        _say(QString(tr("%1 communication to auxiliary link %2 %3")).arg(_vehicleIdSpeech()).arg(link->getName()).arg(active ? "regained" : "lost"));
     }
 }
 

src/Vehicle/Vehicle.h

@@ -1042,7 +1042,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);
@@ -1100,14 +1099,14 @@ private:
     void _rallyPointManagerError(int errorCode, const QString& errorMsg);
     void _mapTrajectoryStart(void);
     void _mapTrajectoryStop(void);
-    void _connectionActive(void);
+    void _linkActiveChanged(LinkInterface* link, bool active);
     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);
     void _commonInit(void);
     void _startPlanRequest(void);
     void _setupAutoDisarmSignalling(void);
@@ -1199,8 +1198,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;
 

src/comm/LinkInterface.cc

@@ -43,6 +43,14 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config)
 
     QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::_writeBytes);
     qRegisterMetaType<LinkInterface*>("LinkInterface*");
+
+    // active Lost timer
+    _bytesReceivedTimer.setInterval(_bytesReceivedTimeoutMSecs);
+    _bytesReceivedTimer.setSingleShot(false);
+    _bytesReceivedTimer.start();
+    QObject::connect(&_bytesReceivedTimer, &QTimer::timeout, this, &LinkInterface::_bytesReceivedTimeout);
+
+    QObject::connect(this, &LinkInterface::bytesReceived, this, &LinkInterface::_bytesReceived);
 }
 
 /// This function logs the send times and amounts of datas for input. Data is used for calculating
@@ -159,3 +167,23 @@ void LinkInterface::_setMavlinkChannel(uint8_t channel)
     _mavlinkChannelSet = true;
     _mavlinkChannel = channel;
 }
+
+void LinkInterface::_bytesReceived(LinkInterface* link, QByteArray bytes)
+{
+    Q_UNUSED(bytes);
+
+    if (this == link) {
+        _bytesReceivedTimer.start();
+
+        if (!link->active()) {
+            link->setActive(true);
+        }
+    }
+}
+
+void LinkInterface::_bytesReceivedTimeout()
+{
+    if (_active && !_highLatency) {
+        setActive(false);
+    }
+}

src/comm/LinkInterface.h

@@ -17,6 +17,7 @@
 #include <QMetaType>
 #include <QSharedPointer>
 #include <QDebug>
+#include <QTimer>
 
 #include "QGCMAVLink.h"
 #include "LinkConfiguration.h"
@@ -42,7 +43,7 @@ public:
 
     // Property accessors
     bool active(void)           { return _active; }
-    void setActive(bool active) { _active = active; emit activeChanged(active); }
+    void setActive(bool active) { _active = active; emit activeChanged(this, active); }
 
     LinkConfiguration* getLinkConfiguration(void) { return _config.data(); }
 
@@ -152,7 +153,7 @@ private slots:
     
 signals:
     void autoconnectChanged(bool autoconnect);
-    void activeChanged(bool active);
+    void activeChanged(LinkInterface* link, bool active);
     void _invokeWriteBytes(QByteArray);
     void highLatencyChanged(bool highLatency);
 
@@ -249,6 +250,10 @@ private:
 
     virtual void _disconnect(void) = 0;
 
+    void _bytesReceived(LinkInterface* link, QByteArray bytes);
+    
+    void _bytesReceivedTimeout(void);
+
     /// Sets the mavlink channel to use for this link
     void _setMavlinkChannel(uint8_t channel);
     
@@ -276,6 +281,9 @@ private:
     bool _active;                       ///< true: link is actively receiving mavlink messages
     bool _enableRateCollection;
     bool _decodedFirstMavlinkPacket;    ///< true: link has correctly decoded it's first mavlink packet
+
+    static const int    _bytesReceivedTimeoutMSecs = 3500;  // Signal connection lost after 3.5 seconds of no messages
+    QTimer              _bytesReceivedTimer;
 };
 
 typedef QSharedPointer<LinkInterface> SharedLinkInterfacePointer;