diff --git a/src/Settings/App.SettingsGroup.json b/src/Settings/App.SettingsGroup.json index 3fc6fc5789a45c826d1ea7d4befc731c223579c4..9274033ba82cb1a7f89821a1a277294daac47047 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 5d898229af5afada7feeff1ed51218de979b01de..388d73f1749ea4013749662d6fc2d5ae3028d939 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 db5f0f9fc80324296e5156eba9f1820aa1c19607..6b64b0d2442d9e3868bee1d9eadb303615182bc9 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 694f4d673c89bbcd057d598b4e36370aeddf571d..8b7bd45cc9ab174929b900cbbc1793727586bddb 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 0543269638baa178bf02aa89055811f3369aed5b..ee1345edecccf165b44f55dae5b59b6063c1f625 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 7eedc47f6224377d79362dbdd0cf806701330aa8..ab612a7f1436d2f6ab4f84c2998da6917051d8d2 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 6532e2606a5d2136e72f9d52e44ca44029c373dc..bf58dde72f3a99631937822729985f08df7f727c 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 f3ef0c7b903c063d4bf41d0d035490a1ecd3a5d0..7068f75bb179b62976d7f86c0eab181bc646aa73 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 c80a82c195d22c1a23ff8356a509cb9539a15d6f..f336424b687377952ae89771e17dc7de33966fff 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 210eaab69c16480ffcecbc5682e26a5b53debaa0..de42986b59246fd05826c3cdb853d1a986e80a50 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 8d5f63fead96bddb6cb6add802297ed4814bf740..99713fd6df437da21565f2b7758ae06f4960f102 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 f06d4ef4a14b3641c6ca3b153cddf9377d82d41a..869f120af0c5453ce92bf992598a598452db351f 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 + } } } //-----------------------------------------------------------------