From 66590562c7ddf36d2601a4de5ede0d924a47faf7 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 19 May 2020 23:01:38 -0600 Subject: [PATCH] Added mavlink forwarding as an option to UDP connections. Enabling this setting will cause every mavlink packet received by QGC to be forwarded on the link, with the exception of packets originating from said link. --- src/comm/LinkConfiguration.cc | 3 ++ src/comm/LinkConfiguration.h | 55 +++++++++++++++++++++++++++---------- src/comm/LinkManager.cc | 26 ++++++++++++++++-- src/comm/LinkManager.h | 2 +- src/comm/UDPLink.h | 2 ++ src/ui/preferences/LinkSettings.qml | 14 ++++++++++ 6 files changed, 83 insertions(+), 19 deletions(-) diff --git a/src/comm/LinkConfiguration.cc b/src/comm/LinkConfiguration.cc index 8b7bd45..694f4d6 100644 --- a/src/comm/LinkConfiguration.cc +++ b/src/comm/LinkConfiguration.cc @@ -36,6 +36,7 @@ LinkConfiguration::LinkConfiguration(const QString& name) , _dynamic(false) , _autoConnect(false) , _highLatency(false) + , _forwardMavlink(false) { _name = name; if (_name.isEmpty()) { @@ -50,6 +51,7 @@ LinkConfiguration::LinkConfiguration(LinkConfiguration* copy) _dynamic = copy->isDynamic(); _autoConnect= copy->isAutoConnect(); _highLatency= copy->isHighLatency(); + _forwardMavlink= copy->isForwardMavlink(); Q_ASSERT(!_name.isEmpty()); } @@ -61,6 +63,7 @@ void LinkConfiguration::copyFrom(LinkConfiguration* source) _dynamic = source->isDynamic(); _autoConnect= source->isAutoConnect(); _highLatency= source->isHighLatency(); + _forwardMavlink= source->isForwardMavlink(); } /*! diff --git a/src/comm/LinkConfiguration.h b/src/comm/LinkConfiguration.h index ee1345e..0543269 100644 --- a/src/comm/LinkConfiguration.h +++ b/src/comm/LinkConfiguration.h @@ -23,16 +23,19 @@ public: LinkConfiguration(LinkConfiguration* copy); virtual ~LinkConfiguration() {} - Q_PROPERTY(QString name READ name WRITE setName NOTIFY nameChanged) - Q_PROPERTY(LinkInterface* link READ link WRITE setLink NOTIFY linkChanged) - Q_PROPERTY(LinkType linkType READ type CONSTANT) - Q_PROPERTY(bool dynamic READ isDynamic WRITE setDynamic NOTIFY dynamicChanged) - Q_PROPERTY(bool autoConnect READ isAutoConnect WRITE setAutoConnect NOTIFY autoConnectChanged) - Q_PROPERTY(bool autoConnectAllowed READ isAutoConnectAllowed CONSTANT) - Q_PROPERTY(QString settingsURL READ settingsURL CONSTANT) - Q_PROPERTY(QString settingsTitle READ settingsTitle CONSTANT) - Q_PROPERTY(bool highLatency READ isHighLatency WRITE setHighLatency NOTIFY highLatencyChanged) - Q_PROPERTY(bool highLatencyAllowed READ isHighLatencyAllowed CONSTANT) + Q_PROPERTY(QString name READ name WRITE setName NOTIFY nameChanged) + Q_PROPERTY(LinkInterface* link READ link WRITE setLink NOTIFY linkChanged) + Q_PROPERTY(LinkType linkType READ type CONSTANT) + Q_PROPERTY(bool dynamic READ isDynamic WRITE setDynamic NOTIFY dynamicChanged) + Q_PROPERTY(bool autoConnect READ isAutoConnect WRITE setAutoConnect NOTIFY autoConnectChanged) + Q_PROPERTY(bool autoConnectAllowed READ isAutoConnectAllowed CONSTANT) + Q_PROPERTY(QString settingsURL READ settingsURL CONSTANT) + Q_PROPERTY(QString settingsTitle READ settingsTitle CONSTANT) + Q_PROPERTY(bool highLatency READ isHighLatency WRITE setHighLatency NOTIFY highLatencyChanged) + Q_PROPERTY(bool highLatencyAllowed READ isHighLatencyAllowed CONSTANT) + Q_PROPERTY(bool forwardMavlink READ isForwardMavlink WRITE setForwardMavlink NOTIFY forwardMavlinkChanged) + Q_PROPERTY(bool forwardMavlinkAllowed READ isForwardMavlinkAllowed CONSTANT) + // Property accessors @@ -83,6 +86,13 @@ public: bool isHighLatency() { return _highLatency; } /*! + * + * Is this mavlink forwarding configuration? + * @return True if this is a mavlink forwarding configuration (sends all received packets to this end point). + */ + bool isForwardMavlink() { return _forwardMavlink; } + + /*! * Set if this is this a dynamic configuration. (decided at runtime) */ void setDynamic(bool dynamic = true) { _dynamic = dynamic; emit dynamicChanged(); } @@ -97,6 +107,12 @@ public: */ void setHighLatency(bool hl = false) { _highLatency = hl; emit highLatencyChanged(); } + /*! + * Set if this is this mavlink forwarding configuration. + */ + void setForwardMavlink(bool forward = false) { _forwardMavlink = forward; emit forwardMavlinkChanged(); } + + /// Virtual Methods /*! @@ -114,6 +130,13 @@ public: virtual bool isHighLatencyAllowed() { return false; } /*! + * + * Is mavlink forwarding allowed for this type? + * @return True if this type can be set as a mavlink forwarding configuration + */ + virtual bool isForwardMavlinkAllowed() { return false; } + + /*! * @brief Connection type * * Pure virtual method returning one of the -TypeXxx types above. @@ -195,11 +218,12 @@ public: static LinkConfiguration* duplicateSettings(LinkConfiguration *source); signals: - void nameChanged (const QString& name); - void dynamicChanged (); - void autoConnectChanged (); - void linkChanged (LinkInterface* link); - void highLatencyChanged (); + void nameChanged (const QString& name); + void dynamicChanged (); + void autoConnectChanged (); + void linkChanged (LinkInterface* link); + void highLatencyChanged (); + void forwardMavlinkChanged (); protected: LinkInterface* _link; ///< Link currently using this configuration (if any) @@ -208,6 +232,7 @@ private: bool _dynamic; ///< A connection added automatically and not persistent (unless it's edited). bool _autoConnect; ///< This connection is started automatically at boot bool _highLatency; + bool _forwardMavlink; }; typedef QSharedPointer SharedLinkConfigurationPointer; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 41eff10..f6de788 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -34,7 +34,7 @@ QGC_LOGGING_CATEGORY(LinkManagerLog, "LinkManagerLog") QGC_LOGGING_CATEGORY(LinkManagerVerboseLog, "LinkManagerVerboseLog") -const char* LinkManager::_defaultUPDLinkName = "UDP Link (AutoConnect)"; +const char* LinkManager::_defaultUDPLinkName = "UDP Link (AutoConnect)"; const int LinkManager::_autoconnectUpdateTimerMSecs = 1000; #ifdef Q_OS_WIN @@ -344,6 +344,8 @@ void LinkManager::saveLinkConfigurationList() settings.setValue(root + "/type", linkConfig->type()); settings.setValue(root + "/auto", linkConfig->isAutoConnect()); settings.setValue(root + "/high_latency", linkConfig->isHighLatency()); + settings.setValue(root + "/forward_mavlink", linkConfig->isForwardMavlink()); + // Have the instance save its own values linkConfig->saveSettings(settings, root); } @@ -376,6 +378,8 @@ void LinkManager::loadLinkConfigurationList() LinkConfiguration* pLink = nullptr; bool autoConnect = settings.value(root + "/auto").toBool(); bool highLatency = settings.value(root + "/high_latency").toBool(); + bool forwardMavlink = settings.value(root + "/forward_mavlink").toBool(); + switch(type) { #ifndef NO_SERIAL_LINK case LinkConfiguration::TypeSerial: @@ -408,6 +412,7 @@ void LinkManager::loadLinkConfigurationList() //-- Have the instance load its own values pLink->setAutoConnect(autoConnect); pLink->setHighLatency(highLatency); + pLink->setForwardMavlink(forwardMavlink); pLink->loadSettings(settings, root); addConfiguration(pLink); linksChanged = true; @@ -463,7 +468,7 @@ void LinkManager::_updateAutoConnectLinks(void) bool foundUDP = false; for (int i = 0; i < _sharedLinks.count(); i++) { LinkConfiguration* linkConfig = _sharedLinks[i]->getLinkConfiguration(); - if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _defaultUPDLinkName) { + if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _defaultUDPLinkName) { foundUDP = true; break; } @@ -471,7 +476,7 @@ void LinkManager::_updateAutoConnectLinks(void) if (!foundUDP && _autoConnectSettings->autoConnectUDP()->rawValue().toBool()) { qCDebug(LinkManagerLog) << "New auto-connect UDP port added"; //-- Default UDPConfiguration is set up for autoconnect - UDPConfiguration* udpConfig = new UDPConfiguration(_defaultUPDLinkName); + UDPConfiguration* udpConfig = new UDPConfiguration(_defaultUDPLinkName); udpConfig->setDynamic(true); SharedLinkConfigurationPointer config = addConfiguration(udpConfig); createConnectedLink(config); @@ -1011,6 +1016,21 @@ void LinkManager::_freeMavlinkChannel(int channel) void LinkManager::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) { link->startMavlinkMessagesTimer(message.sysid); + + // Walk the list of Links. If mavlink forwarding is enabled for a given link then send the data out. + for (int i = 0; i < _sharedLinks.count(); i++) { + LinkConfiguration* linkConfig = _sharedLinks[i]->getLinkConfiguration(); + + bool isUniqueLink = linkConfig->link() != link; // We do not want to send messages back on the link from which they originated + bool forwardMavlink = isUniqueLink && linkConfig->isForwardMavlink(); + + if (forwardMavlink) { + qCDebug(LinkManagerLog) << "Forwarding mavlink packet"; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + int len = mavlink_msg_to_send_buffer(buffer, &message); + _sharedLinks[i]->writeBytesSafe((const char*)buffer, len); + } + } } LogReplayLink* LinkManager::startLogReplay(const QString& logFile) diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 11d1856..6532e26 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -234,7 +234,7 @@ private: static const int _activeLinkCheckTimeoutMSecs = 15000; ///< Amount of time to wait for a heatbeat. Keep in mind ArduPilot stack heartbeat is slow to come. #endif - static const char* _defaultUPDLinkName; + static const char* _defaultUDPLinkName; static const int _autoconnectUpdateTimerMSecs; static const int _autoconnectConnectDelayMSecs; diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index de42986..210eaab 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -126,6 +126,8 @@ public: void updateSettings (); bool isAutoConnectAllowed () { return true; } bool isHighLatencyAllowed () { return true; } + bool isForwardMavlinkAllowed() { return true; } + QString settingsURL () { return "UdpSettings.qml"; } QString settingsTitle () { return tr("UDP Link Settings"); } diff --git a/src/ui/preferences/LinkSettings.qml b/src/ui/preferences/LinkSettings.qml index 99713fd..8d5f63f 100644 --- a/src/ui/preferences/LinkSettings.qml +++ b/src/ui/preferences/LinkSettings.qml @@ -326,6 +326,20 @@ Rectangle { checked = editConfig.highLatency } } + QGCCheckBox { + text: qsTr("Forward all mavlink packets to this end point") + checked: false + enabled: editConfig ? editConfig.forwardMavlinkAllowed : false + onCheckedChanged: { + if(editConfig) { + editConfig.forwardMavlink = checked + } + } + Component.onCompleted: { + if(editConfig) + checked = editConfig.forwardMavlink + } + } } } Item {