From 66fcbc2a2d58e5ea5e26702aacc8231cd8d0d67d Mon Sep 17 00:00:00 2001 From: acfloria Date: Tue, 17 Apr 2018 11:08:14 +0200 Subject: [PATCH] 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 | 2 + src/Vehicle/MultiVehicleManager.cc | 3 - src/Vehicle/Vehicle.cc | 44 +++++++++---- src/Vehicle/Vehicle.h | 2 +- src/comm/HeartbeatTimer.cc | 59 ++++++++++++++++++ src/comm/HeartbeatTimer.h | 99 ++++++++++++++++++++++++++++++ src/comm/LinkInterface.cc | 61 ++++++++++++------ src/comm/LinkInterface.h | 22 +++---- src/comm/LinkManager.cc | 9 +-- 9 files changed, 249 insertions(+), 52 deletions(-) create mode 100644 src/comm/HeartbeatTimer.cc create mode 100644 src/comm/HeartbeatTimer.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 44efb3d22..ed2714441 100644 --- a/qgroundcontrol.pro +++ b/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) diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index cbe4bdea6..478a8b1a6 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/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 diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index b8e176f60..8ab02b4bd 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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(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); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index c0802808a..7807f79be 100644 --- a/src/Vehicle/Vehicle.h +++ b/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); diff --git a/src/comm/HeartbeatTimer.cc b/src/comm/HeartbeatTimer.cc new file mode 100644 index 000000000..7090b7367 --- /dev/null +++ b/src/comm/HeartbeatTimer.cc @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * (c) 2009-2018 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "HeartbeatTimer.h" + +#include + +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); + } +} diff --git a/src/comm/HeartbeatTimer.h b/src/comm/HeartbeatTimer.h new file mode 100644 index 000000000..c62a63f69 --- /dev/null +++ b/src/comm/HeartbeatTimer.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * (c) 2009-2018 QGROUNDCONTROL PROJECT + * + * 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 +#include + +/** + * @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_ diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc index 5b6851a18..996e7cbd0 100644 --- a/src/comm/LinkInterface.cc +++ b/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(); } diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 83a008e4f..f3529b770 100644 --- a/src/comm/LinkInterface.h +++ b/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 _heartbeatTimerList; }; typedef QSharedPointer SharedLinkInterfacePointer; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index b5afe21a4..2d87ed77f 100644 --- a/src/comm/LinkManager.cc +++ b/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); } -- 2.22.0