Allow tracking multiple heartbeats for a single link

Add the HeartbeatTimer class to track a single heartbeat for a single link.
For every new received heartbeat an instance of the class is created.
Every instance emits a signal if it timed out or the timer is restarted.
Each vehicle then decides based on all different signals which link is the
priority link.

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)

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

@@ -1627,6 +1627,11 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
     qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count();
 
     _links.removeOne(link);
+
+    if (_priorityLink.data() == link) {
+        _priorityLink.clear();
+    }
+
     _updatePriorityLink(true, true);
 
     if (_links.count() == 0 && !_allLinksInactiveSent) {
@@ -1688,7 +1693,7 @@ void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
     // 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() && _priorityLink->active()) {
+            if (!_priorityLink.data()->highLatency() && _priorityLink->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;
@@ -1712,7 +1717,7 @@ void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
             if (config) {
                 SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(config);
                 if (pSerialConfig && pSerialConfig->usbDirect()) {
-                    if (_priorityLink.data() != link && link->active()) {
+                    if (_priorityLink.data() != link && link->active(_id)) {
                         newPriorityLink = link;
                         break;
                     }
@@ -1727,7 +1732,7 @@ void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
         // 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()) {
+            if (!link->highLatency() && link->active(_id)) {
                 newPriorityLink = link;
                 break;
             }
@@ -1738,7 +1743,7 @@ void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
         // Search for an active high latency link
         for (int i=0; i<_links.count(); i++) {
             LinkInterface* link = _links[i];
-            if (link->highLatency() && link->active()) {
+            if (link->highLatency() && link->active(_id)) {
                 newPriorityLink = link;
                 break;
             }
@@ -1760,7 +1765,7 @@ void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
 
         emit priorityLinkNameChanged(_priorityLink->getName());
         if (updateActive) {
-            _linkActiveChanged(_priorityLink.data(), _priorityLink->active());
+            _linkActiveChanged(_priorityLink.data(), _priorityLink->active(_id), _id);
         }
     }
 }
@@ -2116,11 +2121,19 @@ QStringList Vehicle::linkNames(void) const
 
 QString Vehicle::priorityLinkName(void) const
 {
-    return _priorityLink->getName();
+    if (_priorityLink) {
+      return _priorityLink->getName();
+    }
+
+    return "none";
 }
 
 void Vehicle::setPriorityLinkByName(const QString& priorityLinkName)
 {
+    if (!_priorityLink) {
+        return;
+    }
+
     if (priorityLinkName == _priorityLink->getName()) {
         // The link did not change
         return;
@@ -2139,7 +2152,7 @@ void Vehicle::setPriorityLinkByName(const QString& priorityLinkName)
         _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
         _updateHighLatencyLink(true);
         emit priorityLinkNameChanged(_priorityLink->getName());
-        _linkActiveChanged(_priorityLink.data(), _priorityLink->active());
+        _linkActiveChanged(_priorityLink.data(), _priorityLink->active(_id), _id);
     }
 }
 
@@ -2443,14 +2456,19 @@ void Vehicle::setConnectionLostEnabled(bool connectionLostEnabled)
     }
 }
 
-void Vehicle::_linkActiveChanged(LinkInterface *link, bool active)
+void Vehicle::_linkActiveChanged(LinkInterface *link, bool active, int vehicleID)
 {
+    // only continue if the vehicle id is correct
+    if (vehicleID != _id) {
+        return;
+    }
+
     if (link == _priorityLink) {
         if (active && _connectionLost) {
             // communication to priority link regained
             _connectionLost = false;
             emit connectionLostChanged(false);
-            qgcApp()->showMessage((tr("communication to priority link %2 regained")).arg(link->getName()));
+            qgcApp()->showMessage((tr("%1 communication to priority link %2 regained")).arg(_vehicleIdSpeech()).arg(link->getName()));
 
             if (_priorityLink->highLatency()) {
                 _setMaxProtoVersion(100);
@@ -2464,7 +2482,7 @@ void Vehicle::_linkActiveChanged(LinkInterface *link, bool active)
 
         } else if (!active && !_connectionLost) {
             // communication to priority link lost
-            qgcApp()->showMessage((tr("communication to priority link %2 lost")).arg(link->getName()));
+            qgcApp()->showMessage((tr("%1 communication to priority link %2 lost")).arg(_vehicleIdSpeech()).arg(link->getName()));
 
             _updatePriorityLink(false, true);
 
@@ -2489,7 +2507,7 @@ void Vehicle::_linkActiveChanged(LinkInterface *link, bool active)
             }
         }
     } else {
-        qgcApp()->showMessage((tr("communication to auxiliary link %2 %3")).arg(link->getName()).arg(active ? "regained" : "lost"));
+        qgcApp()->showMessage((tr("%1 communication to auxiliary link %2 %3")).arg(_vehicleIdSpeech()).arg(link->getName()).arg(active ? "regained" : "lost"));
         _updatePriorityLink(false, true);
     }
 }
@@ -3368,6 +3386,10 @@ void Vehicle::_vehicleParamLoaded(bool ready)
 
 void Vehicle::_updateHighLatencyLink(bool sendCommand)
 {
+    if (!_priorityLink) {
+        return;
+    }
+
     if (_priorityLink->highLatency() != _highLatencyLink) {
         _highLatencyLink = _priorityLink->highLatency();
         _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);

src/Vehicle/Vehicle.h

@@ -1099,7 +1099,7 @@ private:
     void _rallyPointManagerError(int errorCode, const QString& errorMsg);
     void _mapTrajectoryStart(void);
     void _mapTrajectoryStop(void);
-    void _linkActiveChanged(LinkInterface* link, bool active);
+    void _linkActiveChanged(LinkInterface* link, bool active, int vehicleID);
     void _say(const QString& text);
     QString _vehicleIdSpeech(void);
     void _handleMavlinkLoggingData(mavlink_message_t& message);

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
+{
+    for( int i=0; i<_heartbeatTimerList.count(); ++i ) {
+        if (_heartbeatTimerList[i]->getActive()) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+bool LinkInterface::active(int vehicle_id) const
+{
+    for( int i=0; i<_heartbeatTimerList.count(); ++i ) {
+        if (_heartbeatTimerList[i]->getVehicleID() == vehicle_id) {
+            return _heartbeatTimerList[i]->getActive();
+        }
+    }
+
+    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,8 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config)
     , _config                   (config)
     , _highLatency              (config->isHighLatency())
     , _mavlinkChannelSet        (false)
-    , _active                   (false)
     , _enableRateCollection     (false)
     , _decodedFirstMavlinkPacket(false)
-    , _heartbeatReceivedTimer(NULL)
 {
     _config->setLink(this);
 
@@ -161,29 +181,34 @@ void LinkInterface::_setMavlinkChannel(uint8_t channel)
     _mavlinkChannel = channel;
 }
 
-void LinkInterface::_heartbeatReceivedTimeout()
+void LinkInterface::_activeChanged(bool active, int vehicle_id)
 {
-    if (_active && !_highLatency) {
-        setActive(false);
-    }
+    emit activeChanged(this, active, vehicle_id);
 }
 
-void LinkInterface::timerStart() {
-    if (_heartbeatReceivedTimer) {
-        _heartbeatReceivedTimer->start();
+void LinkInterface::timerStart(int vehicle_id) {
+    int timer_index{-1};
+    for( int i=0; i<_heartbeatTimerList.count(); ++i ) {
+        if (_heartbeatTimerList[i]->getVehicleID() == vehicle_id) {
+            timer_index = i;
+            break;
+        }
+    }
+
+    if (timer_index != -1) {
+        _heartbeatTimerList[timer_index]->restartTimer();
     } else {
-        _heartbeatReceivedTimer = new QTimer();
-        _heartbeatReceivedTimer->setInterval(_heartbeatReceivedTimeoutMSecs);
-        _heartbeatReceivedTimer->setSingleShot(true);
-        _heartbeatReceivedTimer->start();
-        QObject::connect(_heartbeatReceivedTimer, &QTimer::timeout, this, &LinkInterface::_heartbeatReceivedTimeout);
+        _heartbeatTimerList.append(new HeartbeatTimer(vehicle_id, _highLatency));
+        QObject::connect(_heartbeatTimerList.last(), &HeartbeatTimer::activeChanged, this, &LinkInterface::_activeChanged);
     }
 }
 
 void LinkInterface::timerStop() {
-    if (_heartbeatReceivedTimer) {
-        _heartbeatReceivedTimer->stop();
-        QObject::disconnect(_heartbeatReceivedTimer, &QTimer::timeout, this, &LinkInterface::_heartbeatReceivedTimeout);
-        delete _heartbeatReceivedTimer;
+    for(int i=0; i<_heartbeatTimerList.count(); ++i ) {
+        QObject::disconnect(_heartbeatTimerList[i], &HeartbeatTimer::activeChanged, this, &LinkInterface::_activeChanged);
+        delete _heartbeatTimerList[i];
+        _heartbeatTimerList[i] = nullptr;
     }
+
+    _heartbeatTimerList.clear();
 }

src/comm/LinkInterface.h

@@ -21,6 +21,7 @@
 
 #include "QGCMAVLink.h"
 #include "LinkConfiguration.h"
+#include "HeartbeatTimer.h"
 
 class LinkManager;
 
@@ -37,16 +38,16 @@ class LinkInterface : public QThread
     friend class LinkManager;
 
 public:    
-    ~LinkInterface() {
-        _config->setLink(NULL);
+    virtual ~LinkInterface() {
         timerStop();
+        _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(this, active); }
+    bool active() const;
+    bool active(int vehicle_id) const;
 
     LinkConfiguration* getLinkConfiguration(void) { return _config.data(); }
 
@@ -154,12 +155,11 @@ public slots:
 private slots:
     virtual void _writeBytes(const QByteArray) = 0;
 
-    void _heartbeatReceivedTimeout(void);
-
+    void _activeChanged(bool active, int vehicle_id);
     
 signals:
     void autoconnectChanged(bool autoconnect);
-    void activeChanged(LinkInterface* link, bool active);
+    void activeChanged(LinkInterface* link, bool active, int vehicle_id);
     void _invokeWriteBytes(QByteArray);
     void highLatencyChanged(bool highLatency);
 
@@ -264,7 +264,7 @@ private:
      *
      * Allocate the timer if it does not exist yet and start it.
      */
-    void timerStart();
+    void timerStart(int vehicle_id);
 
     /**
      * @brief timerStop
@@ -294,12 +294,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
 
-    static const int    _heartbeatReceivedTimeoutMSecs = 3500;  // Signal connection lost after 3.5 seconds of no messages
-    QTimer*             _heartbeatReceivedTimer;
+    QList<HeartbeatTimer*> _heartbeatTimerList;
 };
 
 typedef QSharedPointer<LinkInterface> SharedLinkInterfacePointer;

src/comm/LinkManager.cc

@@ -174,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;
     }
 
@@ -1007,14 +1007,9 @@ void LinkManager::_freeMavlinkChannel(int channel)
 }
 
 void LinkManager::_heartbeatReceived(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType) {
-    Q_UNUSED(vehicleId);
     Q_UNUSED(componentId);
     Q_UNUSED(vehicleFirmwareType);
     Q_UNUSED(vehicleType);
 
-    link->timerStart();
-
-    if (!link->active()) {
-        link->setActive(true);
-    }
+    link->timerStart(vehicleId);
 }