From d4124c20bc49ad7f57f154200bdbdd8876d9ca95 Mon Sep 17 00:00:00 2001 From: acfloria Date: Tue, 26 Jun 2018 12:33:48 +0200 Subject: [PATCH 1/2] Use any mavlink message instead of only the heartbeat to determine if the link is active --- qgroundcontrol.pro | 4 +- src/comm/HeartbeatTimer.cc | 65 ------------------------ src/comm/HeartbeatTimer.h | 106 --------------------------------------- src/comm/LinkInterface.cc | 30 +++++------ src/comm/LinkInterface.h | 18 +++---- src/comm/LinkManager.cc | 10 ++-- src/comm/LinkManager.h | 2 +- src/comm/MavlinkMessagesTimer.cc | 65 ++++++++++++++++++++++++ src/comm/MavlinkMessagesTimer.h | 106 +++++++++++++++++++++++++++++++++++++++ 9 files changed, 201 insertions(+), 205 deletions(-) delete mode 100644 src/comm/HeartbeatTimer.cc delete mode 100644 src/comm/HeartbeatTimer.h create mode 100644 src/comm/MavlinkMessagesTimer.cc create mode 100644 src/comm/MavlinkMessagesTimer.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index b5af342..c09b079 100644 --- a/qgroundcontrol.pro +++ b/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) diff --git a/src/comm/HeartbeatTimer.cc b/src/comm/HeartbeatTimer.cc deleted file mode 100644 index ec1843a..0000000 --- a/src/comm/HeartbeatTimer.cc +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * (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) -{ -} - -void HeartbeatTimer::init() -{ - 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 deleted file mode 100644 index ea16687..0000000 --- a/src/comm/HeartbeatTimer.h +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * (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); - - /** - * @brief init - * - * Starts the timer and emits the signal that the link is active for this vehicle ID - */ - void init(); - - ~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 d23201c..824b143 100644 --- a/src/comm/LinkInterface.cc +++ b/src/comm/LinkInterface.cc @@ -12,7 +12,7 @@ bool LinkInterface::active() const { - QMapIterator iter(_heartbeatTimers); + QMapIterator 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 iter(_heartbeatTimers); +void LinkInterface::stopMavlinkMessagesTimer() { + QMapIterator 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(); } diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 0597d3e..58b8a27 100644 --- a/src/comm/LinkInterface.h +++ b/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 _heartbeatTimers; + QMap _mavlinkMessagesTimers; }; typedef QSharedPointer SharedLinkInterfacePointer; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 2061254..ada53b2 100644 --- a/src/comm/LinkManager.cc +++ b/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); } diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 8c21ed5..83474b6 100644 --- a/src/comm/LinkManager.h +++ b/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 diff --git a/src/comm/MavlinkMessagesTimer.cc b/src/comm/MavlinkMessagesTimer.cc new file mode 100644 index 0000000..d270acf --- /dev/null +++ b/src/comm/MavlinkMessagesTimer.cc @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * (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 "MavlinkMessagesTimer.h" + +#include + +MavlinkMessagesTimer::MavlinkMessagesTimer(int vehicle_id, bool high_latency) : + _active(true), + _timer(new QTimer), + _vehicleID(vehicle_id), + _high_latency(high_latency) +{ +} + +void MavlinkMessagesTimer::init() +{ + if (!_high_latency) { + _timer->setInterval(_messageReceivedTimeoutMSecs); + _timer->setSingleShot(false); + _timer->start(); + } + emit activeChanged(true, _vehicleID); + QObject::connect(_timer, &QTimer::timeout, this, &MavlinkMessagesTimer::timerTimeout); +} + + +MavlinkMessagesTimer::~MavlinkMessagesTimer() +{ + if (_timer) { + QObject::disconnect(_timer, &QTimer::timeout, this, &MavlinkMessagesTimer::timerTimeout); + _timer->stop(); + delete _timer; + _timer = nullptr; + } + + emit activeChanged(false, _vehicleID); +} + +void MavlinkMessagesTimer::restartTimer() +{ + if (!_active) { + _active = true; + emit activeChanged(true, _vehicleID); + } + + _timer->start(); +} + +void MavlinkMessagesTimer::timerTimeout() +{ + if (!_high_latency) { + if (_active) { + _active = false; + emit activeChanged(false, _vehicleID); + } + emit heartbeatTimeout(_vehicleID); + } +} diff --git a/src/comm/MavlinkMessagesTimer.h b/src/comm/MavlinkMessagesTimer.h new file mode 100644 index 0000000..0e4010d --- /dev/null +++ b/src/comm/MavlinkMessagesTimer.h @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * (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 _MAVLINKMESSAGESTIMER_H_ +#define _MAVLINKMESSAGESTIMER_H_ + +#include +#include + +/** + * @brief The MavlinkMessagesTimer class + * + * 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 MavlinkMessagesTimer : public QObject +{ + Q_OBJECT + +public: + /** + * @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. + */ + MavlinkMessagesTimer(int vehicle_id, bool high_latency); + + /** + * @brief init + * + * Starts the timer and emits the signal that the link is active for this vehicle ID + */ + void init(); + + ~MavlinkMessagesTimer(); + + /** + * @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 mavlink message 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 _messageReceivedTimeoutMSecs = 3500; // Signal connection lost after 3.5 seconds of no messages +}; + +#endif // _MAVLINKMESSAGESTIMER_H_ From b0df0096c216e5f0ce6f8b6441c481c660f8f149 Mon Sep 17 00:00:00 2001 From: acfloria Date: Tue, 26 Jun 2018 18:12:39 +0200 Subject: [PATCH 2/2] Fix link fallback for multiple links --- src/Vehicle/Vehicle.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index cd12a23..9dce524 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2508,7 +2508,10 @@ void Vehicle::_linkActiveChanged(LinkInterface *link, bool active, int vehicleID 1); // Request protocol version } } else if (!active && !_connectionLost) { - if (_connectionLostEnabled) { + _updatePriorityLink(false /* updateActive */, false /* sendCommand */); + + // check if another active link has been found + if (link == _priorityLink) { _connectionLost = true; communicationLost = true; _heardFrom = false;