Use any mavlink message instead of only the heartbeat to determine if the link is active

qgroundcontrol.pro

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

src/comm/LinkInterface.cc

@@ -12,7 +12,7 @@
 
 bool LinkInterface::active() const
 {
-    QMapIterator<int /* vehicle id */, HeartbeatTimer*> iter(_heartbeatTimers);
+    QMapIterator<int /* vehicle id */, MavlinkMessagesTimer*> iter(_mavlinkMessagesTimers);
     while (iter.hasNext()) {
         iter.next();
         if (iter.value()->getActive()) {
@@ -25,8 +25,8 @@ bool LinkInterface::active() const
 
 bool LinkInterface::link_active(int vehicle_id) const
 {
-    if (_heartbeatTimers.contains(vehicle_id)) {
-        return _heartbeatTimers.value(vehicle_id)->getActive();
+    if (_mavlinkMessagesTimers.contains(vehicle_id)) {
+        return _mavlinkMessagesTimers.value(vehicle_id)->getActive();
     } else {
         return false;
     }
@@ -189,24 +189,24 @@ 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();
+void LinkInterface::startMavlinkMessagesTimer(int vehicle_id) {
+    if (_mavlinkMessagesTimers.contains(vehicle_id)) {
+        _mavlinkMessagesTimers.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);
-        _heartbeatTimers.value(vehicle_id)->init();
+        _mavlinkMessagesTimers.insert(vehicle_id, new MavlinkMessagesTimer(vehicle_id, _highLatency));
+        QObject::connect(_mavlinkMessagesTimers.value(vehicle_id), &MavlinkMessagesTimer::activeChanged, this, &LinkInterface::_activeChanged);
+        _mavlinkMessagesTimers.value(vehicle_id)->init();
     }
 }
 
-void LinkInterface::stopHeartbeatTimer() {
-    QMapIterator<int /* vehicle id */, HeartbeatTimer*> iter(_heartbeatTimers);
+void LinkInterface::stopMavlinkMessagesTimer() {
+    QMapIterator<int /* vehicle id */, MavlinkMessagesTimer*> iter(_mavlinkMessagesTimers);
     while (iter.hasNext()) {
         iter.next();
-        QObject::disconnect(iter.value(), &HeartbeatTimer::activeChanged, this, &LinkInterface::_activeChanged);
-        _heartbeatTimers[iter.key()]->deleteLater();
-        _heartbeatTimers[iter.key()] = nullptr;
+        QObject::disconnect(iter.value(), &MavlinkMessagesTimer::activeChanged, this, &LinkInterface::_activeChanged);
+        _mavlinkMessagesTimers[iter.key()]->deleteLater();
+        _mavlinkMessagesTimers[iter.key()] = nullptr;
     }
 
-    _heartbeatTimers.clear();
+    _mavlinkMessagesTimers.clear();
 }

src/comm/LinkInterface.h

@@ -21,7 +21,7 @@
 
 #include "QGCMAVLink.h"
 #include "LinkConfiguration.h"
-#include "HeartbeatTimer.h"
+#include "MavlinkMessagesTimer.h"
 
 class LinkManager;
 
@@ -39,7 +39,7 @@ class LinkInterface : public QThread
 
 public:    
     virtual ~LinkInterface() {
-        stopHeartbeatTimer();
+        stopMavlinkMessagesTimer();
         _config->setLink(NULL);
     }
 
@@ -264,19 +264,19 @@ private:
     void _setMavlinkChannel(uint8_t channel);
     
     /**
-     * @brief startHeartbeatTimer
+     * @brief startMavlinkMessagesTimer
      *
-     * Start/restart the heartbeat timer for the specific vehicle.
+     * Start/restart the mavlink messages timer for the specific vehicle.
      * If no timer exists an instance is allocated.
      */
-    void startHeartbeatTimer(int vehicle_id);
+    void startMavlinkMessagesTimer(int vehicle_id);
 
     /**
-     * @brief stopHeartbeatTimer
+     * @brief stopMavlinkMessagesTimer
      *
-     * Stop and deallocate the heartbeat timers for all vehicles if any exists.
+     * Stop and deallocate the mavlink messages timers for all vehicles if any exists.
      */
-    void stopHeartbeatTimer();
+    void stopMavlinkMessagesTimer();
 
     bool _mavlinkChannelSet;    ///< true: _mavlinkChannel has been set
     uint8_t _mavlinkChannel;    ///< mavlink channel to use for this link, as used by mavlink_parse_char
@@ -303,7 +303,7 @@ private:
     bool _decodedFirstMavlinkPacket;    ///< true: link has correctly decoded it's first mavlink packet
     bool _isPX4Flow;
 
-    QMap<int /* vehicle id */, HeartbeatTimer*> _heartbeatTimers;
+    QMap<int /* vehicle id */, MavlinkMessagesTimer*> _mavlinkMessagesTimers;
 };
 
 typedef QSharedPointer<LinkInterface> SharedLinkInterfacePointer;

src/comm/LinkManager.cc

@@ -80,7 +80,7 @@ void LinkManager::setToolbox(QGCToolbox *toolbox)
     _autoConnectSettings = toolbox->settingsManager()->autoConnectSettings();
     _mavlinkProtocol = _toolbox->mavlinkProtocol();
 
-    connect(_mavlinkProtocol, &MAVLinkProtocol::vehicleHeartbeatInfo, this, &LinkManager::_heartbeatReceived);
+    connect(_mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &LinkManager::_mavlinkMessageReceived);
 
     connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateAutoConnectLinks);
     _portListTimer.start(_autoconnectUpdateTimerMSecs); // timeout must be long enough to get past bootloader on second pass
@@ -1008,10 +1008,6 @@ 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);
+void LinkManager::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) {
+    link->startMavlinkMessagesTimer(message.sysid);
 }

src/comm/LinkManager.h

@@ -204,7 +204,7 @@ private:
     SerialConfiguration* _autoconnectConfigurationsContainsPort(const QString& portName);
 #endif
 
-    void _heartbeatReceived(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType);
+    void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
 
     bool    _configUpdateSuspended;                     ///< true: stop updating configuration list
     bool    _configurationsLoaded;                      ///< true: Link configurations have been loaded

src/comm/HeartbeatTimer.cc → src/comm/MavlinkMessagesTimer.cc

@@ -7,11 +7,11 @@
  *
  ****************************************************************************/
 
-#include "HeartbeatTimer.h"
+#include "MavlinkMessagesTimer.h"
 
 #include <QDebug>
 
-HeartbeatTimer::HeartbeatTimer(int vehicle_id, bool high_latency) :
+MavlinkMessagesTimer::MavlinkMessagesTimer(int vehicle_id, bool high_latency) :
     _active(true),
     _timer(new QTimer),
     _vehicleID(vehicle_id),
@@ -19,22 +19,22 @@ HeartbeatTimer::HeartbeatTimer(int vehicle_id, bool high_latency) :
 {
 }
 
-void HeartbeatTimer::init()
+void MavlinkMessagesTimer::init()
 {
     if (!_high_latency) {
-        _timer->setInterval(_heartbeatReceivedTimeoutMSecs);
-        _timer->setSingleShot(true);
+        _timer->setInterval(_messageReceivedTimeoutMSecs);
+        _timer->setSingleShot(false);
         _timer->start();
     }
     emit activeChanged(true, _vehicleID);
-    QObject::connect(_timer, &QTimer::timeout, this, &HeartbeatTimer::timerTimeout);
+    QObject::connect(_timer, &QTimer::timeout, this, &MavlinkMessagesTimer::timerTimeout);
 }
 
 
-HeartbeatTimer::~HeartbeatTimer()
+MavlinkMessagesTimer::~MavlinkMessagesTimer()
 {
     if (_timer) {
-        QObject::disconnect(_timer, &QTimer::timeout, this, &HeartbeatTimer::timerTimeout);
+        QObject::disconnect(_timer, &QTimer::timeout, this, &MavlinkMessagesTimer::timerTimeout);
         _timer->stop();
         delete _timer;
         _timer = nullptr;
@@ -43,7 +43,7 @@ HeartbeatTimer::~HeartbeatTimer()
     emit activeChanged(false, _vehicleID);
 }
 
-void HeartbeatTimer::restartTimer()
+void MavlinkMessagesTimer::restartTimer()
 {
     if (!_active) {
         _active = true;
@@ -53,7 +53,7 @@ void HeartbeatTimer::restartTimer()
     _timer->start();
 }
 
-void HeartbeatTimer::timerTimeout()
+void MavlinkMessagesTimer::timerTimeout()
 {
     if (!_high_latency) {
         if (_active) {

src/comm/HeartbeatTimer.h → src/comm/MavlinkMessagesTimer.h

@@ -7,34 +7,34 @@
  *
  ****************************************************************************/
 
-#ifndef _HEARTBEATTIMER_H_
-#define _HEARTBEATTIMER_H_
+#ifndef _MAVLINKMESSAGESTIMER_H_
+#define _MAVLINKMESSAGESTIMER_H_
 
 #include <QTimer>
 #include <QObject>
 
 /**
- * @brief The HeartbeatTimer class
+ * @brief The MavlinkMessagesTimer 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
+ * Track the mavlink messages for a single vehicle on one link.
+ * As long as regular messages 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
+class MavlinkMessagesTimer : public QObject
 {
     Q_OBJECT
 
 public:
     /**
-     * @brief HeartbeatTimer
+     * @brief MavlinkMessagesTimer
      *
      * 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);
+    MavlinkMessagesTimer(int vehicle_id, bool high_latency);
 
     /**
      * @brief init
@@ -43,7 +43,7 @@ public:
      */
     void init();
 
-    ~HeartbeatTimer();
+    ~MavlinkMessagesTimer();
 
     /**
      * @brief getActive
@@ -68,7 +68,7 @@ signals:
     /**
      * @brief heartbeatTimeout
      *
-     * Emitted if no heartbeat is received over the specified time.
+     * Emitted if no mavlink message is received over the specified time.
      *
      * @param vehicle_id: The vehicle ID for which the heartbeat timed out.
      */
@@ -100,7 +100,7 @@ private:
     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
+    static const int    _messageReceivedTimeoutMSecs = 3500;  // Signal connection lost after 3.5 seconds of no messages
 };
 
-#endif // _HEARTBEATTIMER_H_
+#endif // _MAVLINKMESSAGESTIMER_H_