diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index b5af342c41da675ccc5e13eeaed1d8cff30b6a91..c09b079efe3f21c2de2243a994bee259d89d5fb7 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/LinkInterface.cc b/src/comm/LinkInterface.cc index d23201c5ef4481effaa225e102b3f21b9901f069..824b143c88fc0dc4101ec633fddfb2d7bac1203a 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 0597d3ec4f3746c44f86864b72218646e195dd89..58b8a27d5544477fac71b383012732df9a2a2a64 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 20612542d8d993a1811295a730849f7dbe186c21..ada53b2cdfde80d65e8b2a8683249b42332f32fe 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 8c21ed5dac0542418aaa0c5e9affa93cd74461a7..83474b6ba90d39dcf1c79dab6a5bb79571223aec 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/HeartbeatTimer.cc b/src/comm/MavlinkMessagesTimer.cc similarity index 65% rename from src/comm/HeartbeatTimer.cc rename to src/comm/MavlinkMessagesTimer.cc index ec1843a651e163eb8b74c9ad6c3ecc28bc54a60a..d270acf7bd516a0a23695452e7b9b0fda6bc9bf4 100644 --- a/src/comm/HeartbeatTimer.cc +++ b/src/comm/MavlinkMessagesTimer.cc @@ -7,11 +7,11 @@ * ****************************************************************************/ -#include "HeartbeatTimer.h" +#include "MavlinkMessagesTimer.h" #include -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) { diff --git a/src/comm/HeartbeatTimer.h b/src/comm/MavlinkMessagesTimer.h similarity index 78% rename from src/comm/HeartbeatTimer.h rename to src/comm/MavlinkMessagesTimer.h index ea166873c8b47a18202731eaeabe4b3f3c408ec2..0e4010d3b4dd2d4d24b497e1e2b91086aa9cd336 100644 --- a/src/comm/HeartbeatTimer.h +++ b/src/comm/MavlinkMessagesTimer.h @@ -7,34 +7,34 @@ * ****************************************************************************/ -#ifndef _HEARTBEATTIMER_H_ -#define _HEARTBEATTIMER_H_ +#ifndef _MAVLINKMESSAGESTIMER_H_ +#define _MAVLINKMESSAGESTIMER_H_ #include #include /** - * @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_