From af2309a990eede9ea3ed390952937b62bdb54cdc Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 26 May 2020 00:01:29 -0600 Subject: [PATCH] Updated mavlink forwarding implementation to forward all mavlink messages received on all channels onto a single UDP port. A new mavlink forwarding UDP link is created if forwarding is enabled. The mavlink settings page now has a Fact checkbox for enabling forwarding and a fact text field for specifying the host name. --- src/Settings/App.SettingsGroup.json | 14 +++++++ src/Settings/AppSettings.cc | 2 + src/Settings/AppSettings.h | 2 + src/comm/LinkConfiguration.cc | 3 -- src/comm/LinkConfiguration.h | 55 +++++++------------------- src/comm/LinkManager.cc | 47 ++++++++++++++++++++-- src/comm/LinkManager.h | 4 ++ src/comm/MAVLinkProtocol.cc | 27 ++++++------- src/comm/UDPLink.cc | 2 +- src/comm/UDPLink.h | 2 - src/ui/preferences/LinkSettings.qml | 14 ------- src/ui/preferences/MavlinkSettings.qml | 28 +++++++++++++ 12 files changed, 122 insertions(+), 78 deletions(-) diff --git a/src/Settings/App.SettingsGroup.json b/src/Settings/App.SettingsGroup.json index 3fc6fc578..9274033ba 100644 --- a/src/Settings/App.SettingsGroup.json +++ b/src/Settings/App.SettingsGroup.json @@ -286,6 +286,20 @@ "shortDescription": "Comma separated list of first run prompt ids which have already been shown.", "type": "string", "defaultValue": "" +}, +{ + "name": "forwardMavlink", + "shortDescription": "Enable mavlink forwarding", + "longDescription": "Enable mavlink forwarding", + "type": "bool", + "defaultValue": false +}, +{ + "name": "forwardMavlinkHostName", + "shortDescription": "Host name", + "longDescription": "Host name to forward mavlink to. i.e: localhost:14445", + "type": "string", + "defaultValue": "localhost:14445" } ] } diff --git a/src/Settings/AppSettings.cc b/src/Settings/AppSettings.cc index 5d898229a..388d73f17 100644 --- a/src/Settings/AppSettings.cc +++ b/src/Settings/AppSettings.cc @@ -106,6 +106,8 @@ DECLARE_SETTINGSFACT(AppSettings, disableAllPersistence) DECLARE_SETTINGSFACT(AppSettings, usePairing) DECLARE_SETTINGSFACT(AppSettings, saveCsvTelemetry) DECLARE_SETTINGSFACT(AppSettings, firstRunPromptIdsShown) +DECLARE_SETTINGSFACT(AppSettings, forwardMavlink) +DECLARE_SETTINGSFACT(AppSettings, forwardMavlinkHostName) DECLARE_SETTINGSFACT_NO_FUNC(AppSettings, indoorPalette) { diff --git a/src/Settings/AppSettings.h b/src/Settings/AppSettings.h index db5f0f9fc..6b64b0d24 100644 --- a/src/Settings/AppSettings.h +++ b/src/Settings/AppSettings.h @@ -60,6 +60,8 @@ public: DEFINE_SETTINGFACT(usePairing) DEFINE_SETTINGFACT(saveCsvTelemetry) DEFINE_SETTINGFACT(firstRunPromptIdsShown) + DEFINE_SETTINGFACT(forwardMavlink) + DEFINE_SETTINGFACT(forwardMavlinkHostName) // Although this is a global setting it only affects ArduPilot vehicle since PX4 automatically starts the stream from the vehicle side diff --git a/src/comm/LinkConfiguration.cc b/src/comm/LinkConfiguration.cc index 694f4d673..8b7bd45cc 100644 --- a/src/comm/LinkConfiguration.cc +++ b/src/comm/LinkConfiguration.cc @@ -36,7 +36,6 @@ LinkConfiguration::LinkConfiguration(const QString& name) , _dynamic(false) , _autoConnect(false) , _highLatency(false) - , _forwardMavlink(false) { _name = name; if (_name.isEmpty()) { @@ -51,7 +50,6 @@ LinkConfiguration::LinkConfiguration(LinkConfiguration* copy) _dynamic = copy->isDynamic(); _autoConnect= copy->isAutoConnect(); _highLatency= copy->isHighLatency(); - _forwardMavlink= copy->isForwardMavlink(); Q_ASSERT(!_name.isEmpty()); } @@ -63,7 +61,6 @@ 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 054326963..ee1345ede 100644 --- a/src/comm/LinkConfiguration.h +++ b/src/comm/LinkConfiguration.h @@ -23,19 +23,16 @@ 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(bool forwardMavlink READ isForwardMavlink WRITE setForwardMavlink NOTIFY forwardMavlinkChanged) - Q_PROPERTY(bool forwardMavlinkAllowed READ isForwardMavlinkAllowed 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) // Property accessors @@ -85,13 +82,6 @@ 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) */ @@ -107,12 +97,6 @@ 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 /*! @@ -129,13 +113,6 @@ 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 * @@ -218,12 +195,11 @@ public: static LinkConfiguration* duplicateSettings(LinkConfiguration *source); signals: - void nameChanged (const QString& name); - void dynamicChanged (); - void autoConnectChanged (); - void linkChanged (LinkInterface* link); - void highLatencyChanged (); - void forwardMavlinkChanged (); + void nameChanged (const QString& name); + void dynamicChanged (); + void autoConnectChanged (); + void linkChanged (LinkInterface* link); + void highLatencyChanged (); protected: LinkInterface* _link; ///< Link currently using this configuration (if any) @@ -232,7 +208,6 @@ 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 7eedc47f6..ab612a7f1 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -35,6 +35,7 @@ QGC_LOGGING_CATEGORY(LinkManagerLog, "LinkManagerLog") QGC_LOGGING_CATEGORY(LinkManagerVerboseLog, "LinkManagerVerboseLog") const char* LinkManager::_defaultUDPLinkName = "UDP Link (AutoConnect)"; +const char* LinkManager::_mavlinkForwardingLinkName = "MAVLink Forwarding Link"; const int LinkManager::_autoconnectUpdateTimerMSecs = 1000; #ifdef Q_OS_WIN @@ -173,6 +174,19 @@ LinkInterface* LinkManager::createConnectedLink(const QString& name) return nullptr; } +SharedLinkInterfacePointer LinkManager::mavlinkForwardingLink() +{ + for (int i = 0; i < _sharedLinks.count(); i++) { + LinkConfiguration* linkConfig = _sharedLinks[i]->getLinkConfiguration(); + if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _mavlinkForwardingLinkName) { + SharedLinkInterfacePointer& link = _sharedLinks[i]; + return link; + } + } + + return nullptr; +} + void LinkManager::_addLink(LinkInterface* link) { if (thread() != QThread::currentThread()) { @@ -344,8 +358,6 @@ 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); } @@ -378,7 +390,6 @@ 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 @@ -412,7 +423,6 @@ 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; @@ -482,6 +492,35 @@ void LinkManager::_updateAutoConnectLinks(void) createConnectedLink(config); emit linkConfigurationsChanged(); } + + // Connect MAVLink forwarding if it is enabled + bool foundMAVLinkForwardingLink = false; + for (int i = 0; i < _sharedLinks.count(); i++) { + LinkConfiguration* linkConfig = _sharedLinks[i]->getLinkConfiguration(); + if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _mavlinkForwardingLinkName) { + foundMAVLinkForwardingLink = true; + // TODO: should we check if the host/port matches the mavlinkForwardHostName setting and update if it does not match? + break; + } + } + + // Create the link if necessary + bool forwardingEnabled = _toolbox->settingsManager()->appSettings()->forwardMavlink()->rawValue().toBool(); + if (!foundMAVLinkForwardingLink && forwardingEnabled) { + + qCDebug(LinkManagerLog) << "New MAVLink forwarding port added"; + + UDPConfiguration* udpConfig = new UDPConfiguration(_mavlinkForwardingLinkName); + udpConfig->setDynamic(true); + + QString hostName = _toolbox->settingsManager()->appSettings()->forwardMavlinkHostName()->rawValue().toString(); + udpConfig->addHost(hostName); + + SharedLinkConfigurationPointer config = addConfiguration(udpConfig); + createConnectedLink(config); + emit linkConfigurationsChanged(); + } + #ifndef __mobile__ #ifndef NO_SERIAL_LINK // check to see if nmea gps is configured for UDP input, if so, set it up to connect diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 6532e2606..bf58dde72 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -112,6 +112,9 @@ public: /// Creates, connects (and adds) a link based on the given configuration name. LinkInterface* createConnectedLink(const QString& name); + /// Returns pointer to the mavlink forwarding link, or nullptr if it does not exist + SharedLinkInterfacePointer mavlinkForwardingLink(); + /// Disconnects all existing links void disconnectAll(void); @@ -235,6 +238,7 @@ private: #endif static const char* _defaultUDPLinkName; + static const char* _mavlinkForwardingLinkName; static const int _autoconnectUpdateTimerMSecs; static const int _autoconnectConnectDelayMSecs; diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index f3ef0c7b9..7068f75bb 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -208,20 +208,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) return; } - // Walk the list of Links. If mavlink forwarding is enabled for a given link then send the data out. - QList links = _linkMgr->links(); - for (int i = 0; i < links.count(); i++) { - LinkConfiguration* linkConfig = links[i]->getLinkConfiguration(); - - bool isUniqueLink = links[i] != link; // We do not want to send messages back on the link from which they originated - bool forwardMavlink = isUniqueLink && linkConfig->isForwardMavlink(); - - if (forwardMavlink) { - qDebug() << "Forwarding mavlink packet on: " << linkConfig->name(); - links[i]->writeBytesSafe(b.data(), b.length()); - } - } - uint8_t mavlinkChannel = link->mavlinkChannel(); static int nonmavlinkCount = 0; @@ -282,6 +268,19 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) //qDebug() << foo << _message.seq << expectedSeq << lastSeq << totalLossCounter[mavlinkChannel] << totalReceiveCounter[mavlinkChannel] << totalSentCounter[mavlinkChannel] << "(" << _message.sysid << _message.compid << ")"; + //----------------------------------------------------------------- + // MAVLink forwarding + bool forwardingEnabled = _app->toolbox()->settingsManager()->appSettings()->forwardMavlink()->rawValue().toBool(); + if (forwardingEnabled) { + SharedLinkInterfacePointer forwardingLink = _linkMgr->mavlinkForwardingLink(); + + if (forwardingLink) { + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + int len = mavlink_msg_to_send_buffer(buf, &_message); + forwardingLink->writeBytesSafe((const char*)buf, len); + } + } + //----------------------------------------------------------------- // Log data if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) { diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index c80a82c19..f336424b6 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -185,7 +185,7 @@ void UDPLink::_writeBytes(const QByteArray data) void UDPLink::_writeDataGram(const QByteArray data, const UDPCLient* target) { - // qDebug() << "UDP Out" << target->address << target->port; + //qDebug() << "UDP Out" << target->address << target->port; if(_socket->writeDatagram(data, target->address, target->port) < 0) { qWarning() << "Error writing to" << target->address << target->port; } else { diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index 210eaab69..de42986b5 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -126,8 +126,6 @@ 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 8d5f63fea..99713fd6d 100644 --- a/src/ui/preferences/LinkSettings.qml +++ b/src/ui/preferences/LinkSettings.qml @@ -326,20 +326,6 @@ 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 { diff --git a/src/ui/preferences/MavlinkSettings.qml b/src/ui/preferences/MavlinkSettings.qml index f06d4ef4a..869f120af 100644 --- a/src/ui/preferences/MavlinkSettings.qml +++ b/src/ui/preferences/MavlinkSettings.qml @@ -153,6 +153,34 @@ Rectangle { QGroundControl.isVersionCheckEnabled = checked } } + + FactCheckBox { + id: mavlinkForwardingChecked + text: qsTr("Enable MAVLink forwarding") + fact: QGroundControl.settingsManager.appSettings.forwardMavlink + } + + Row { + spacing: ScreenTools.defaultFontPixelWidth + QGCLabel { + width: _labelWidth + anchors.baseline: mavlinkForwardingHostNameField.baseline + visible: QGroundControl.settingsManager.appSettings.forwardMavlink.rawValue + text: qsTr("Host name:") + } + FactTextField { + id: mavlinkForwardingHostNameField + fact: QGroundControl.settingsManager.appSettings.forwardMavlinkHostName + width: _valueWidth + visible: QGroundControl.settingsManager.appSettings.forwardMavlink.rawValue + anchors.verticalCenter: parent.verticalCenter + } + + } + QGCLabel { + text: qsTr(" Changing the host name requires restart of application. ") + visible: QGroundControl.settingsManager.appSettings.forwardMavlink.rawValue + } } } //----------------------------------------------------------------- -- 2.22.0