From 1e77e295136595aa14596d68f57c088d827f9d86 Mon Sep 17 00:00:00 2001 From: acfloria Date: Fri, 6 Apr 2018 14:34:51 +0200 Subject: [PATCH] Use again the heartbeat instead of bytes received to determine if a link is active --- src/Vehicle/Vehicle.cc | 1 + src/comm/LinkInterface.cc | 27 +++++++++++++-------------- src/comm/LinkInterface.h | 6 +++--- src/comm/LinkManager.cc | 10 +++++++--- src/comm/LinkManager.h | 2 +- 5 files changed, 25 insertions(+), 21 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 71446dfe2..b8e176f60 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2490,6 +2490,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")); + _updatePriorityLink(false, true); } } diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc index f5eafe1af..5b6851a18 100644 --- a/src/comm/LinkInterface.cc +++ b/src/comm/LinkInterface.cc @@ -28,7 +28,7 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config) , _active (false) , _enableRateCollection (false) , _decodedFirstMavlinkPacket(false) - , _bytesReceivedTimer(NULL) + , _heartbeatReceivedTimer(NULL) { _config->setLink(this); @@ -161,8 +161,7 @@ void LinkInterface::_setMavlinkChannel(uint8_t channel) _mavlinkChannel = channel; } - -void LinkInterface::_bytesReceivedTimeout() +void LinkInterface::_heartbeatReceivedTimeout() { if (_active && !_highLatency) { setActive(false); @@ -170,21 +169,21 @@ void LinkInterface::_bytesReceivedTimeout() } void LinkInterface::timerStart() { - if (_bytesReceivedTimer) { - _bytesReceivedTimer->start(); + if (_heartbeatReceivedTimer) { + _heartbeatReceivedTimer->start(); } else { - _bytesReceivedTimer = new QTimer(); - _bytesReceivedTimer->setInterval(_bytesReceivedTimeoutMSecs); - _bytesReceivedTimer->setSingleShot(true); - _bytesReceivedTimer->start(); - QObject::connect(_bytesReceivedTimer, &QTimer::timeout, this, &LinkInterface::_bytesReceivedTimeout); + _heartbeatReceivedTimer = new QTimer(); + _heartbeatReceivedTimer->setInterval(_heartbeatReceivedTimeoutMSecs); + _heartbeatReceivedTimer->setSingleShot(true); + _heartbeatReceivedTimer->start(); + QObject::connect(_heartbeatReceivedTimer, &QTimer::timeout, this, &LinkInterface::_heartbeatReceivedTimeout); } } void LinkInterface::timerStop() { - if (_bytesReceivedTimer) { - _bytesReceivedTimer->stop(); - QObject::disconnect(_bytesReceivedTimer, &QTimer::timeout, this, &LinkInterface::_bytesReceivedTimeout); - delete _bytesReceivedTimer; + if (_heartbeatReceivedTimer) { + _heartbeatReceivedTimer->stop(); + QObject::disconnect(_heartbeatReceivedTimer, &QTimer::timeout, this, &LinkInterface::_heartbeatReceivedTimeout); + delete _heartbeatReceivedTimer; } } diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 9e879ad4d..83a008e4f 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -154,7 +154,7 @@ public slots: private slots: virtual void _writeBytes(const QByteArray) = 0; - void _bytesReceivedTimeout(void); + void _heartbeatReceivedTimeout(void); signals: @@ -298,8 +298,8 @@ private: 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; + static const int _heartbeatReceivedTimeoutMSecs = 3500; // Signal connection lost after 3.5 seconds of no messages + QTimer* _heartbeatReceivedTimer; }; typedef QSharedPointer SharedLinkInterfacePointer; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 87ff78f4b..b5afe21a4 100644 --- a/src/comm/LinkManager.cc +++ b/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 @@ -195,7 +197,6 @@ void LinkManager::_addLink(LinkInterface* link) connect(link, &LinkInterface::communicationError, _app, &QGCApplication::criticalMessageBoxOnMainThread); connect(link, &LinkInterface::bytesReceived, _mavlinkProtocol, &MAVLinkProtocol::receiveBytes); - connect(link, &LinkInterface::bytesReceived, this, &LinkManager::_bytesReceived); _mavlinkProtocol->resetMetadataForLink(link); _mavlinkProtocol->setVersion(_mavlinkProtocol->getCurrentVersion()); @@ -1005,8 +1006,11 @@ void LinkManager::_freeMavlinkChannel(int channel) _mavlinkChannelsUsedBitMask &= ~(1 << channel); } -void LinkManager::_bytesReceived(LinkInterface *link, QByteArray bytes) { - Q_UNUSED(bytes); +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(); diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 46d62b05f..13c0ed852 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -204,7 +204,7 @@ private: SerialConfiguration* _autoconnectConfigurationsContainsPort(const QString& portName); #endif - void _bytesReceived(LinkInterface* link, QByteArray bytes); + 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 -- 2.22.0