Merge pull request #8763 from dakejahl/pr-mavlink_forwarding

MAVLink forwarding via UDP

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"
 }
 ]
 }

src/Settings/AppSettings.cc

@@ -119,6 +119,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)
 {

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

src/comm/LinkManager.cc

@@ -34,7 +34,8 @@
 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 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()) {
@@ -376,6 +390,7 @@ void LinkManager::loadLinkConfigurationList()
                             LinkConfiguration* pLink = nullptr;
                             bool autoConnect = settings.value(root + "/auto").toBool();
                             bool highLatency = settings.value(root + "/high_latency").toBool();
+
                             switch(type) {
 #ifndef NO_SERIAL_LINK
                             case LinkConfiguration::TypeSerial:
@@ -463,7 +478,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,12 +486,42 @@ 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);
         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);
+        udpConfig->setTransmitOnly(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

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);
 
@@ -234,7 +237,8 @@ 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 char*  _mavlinkForwardingLinkName;
     static const int    _autoconnectUpdateTimerMSecs;
     static const int    _autoconnectConnectDelayMSecs;
 

src/comm/MAVLinkProtocol.cc

@@ -268,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()) {

src/comm/UDPLink.cc

@@ -291,13 +291,16 @@ bool UDPLink::_hardwareConnect()
     if (_connectState) {
         _socket->joinMulticastGroup(QHostAddress("224.0.0.1"));
         //-- Make sure we have a large enough IO buffers
+
 #ifdef __mobile__
-        _socket->setSocketOption(QAbstractSocket::SendBufferSizeSocketOption,     64 * 1024);
-        _socket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, 128 * 1024);
+        int bufferSizeMultiplier = 1;
 #else
-        _socket->setSocketOption(QAbstractSocket::SendBufferSizeSocketOption,    256 * 1024);
-        _socket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, 512 * 1024);
+        int bufferSizeMultiplier = 4;
 #endif
+        int receiveBufferSize = _udpConfig->isTransmitOnly() ? 0 : 512 * 1024;
+        _socket->setSocketOption(QAbstractSocket::SendBufferSizeSocketOption,     bufferSizeMultiplier * 64 * 1024);
+        _socket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, bufferSizeMultiplier * receiveBufferSize);
+
         _registerZeroconf(_udpConfig->localPort(), kZeroconfRegistration);
         QObject::connect(_socket, &QUdpSocket::readyRead, this, &UDPLink::readBytes);
         emit connected();
@@ -369,7 +372,9 @@ void UDPLink::_deregisterZeroconf()
 //--------------------------------------------------------------------------
 //-- UDPConfiguration
 
-UDPConfiguration::UDPConfiguration(const QString& name) : LinkConfiguration(name)
+UDPConfiguration::UDPConfiguration(const QString& name)
+    : LinkConfiguration(name)
+    , _transmitOnly(false)
 {
     AutoConnectSettings* settings = qgcApp()->toolbox()->settingsManager()->autoConnectSettings();
     _localPort = settings->udpListenPort()->rawValue().toInt();
@@ -379,7 +384,9 @@ UDPConfiguration::UDPConfiguration(const QString& name) : LinkConfiguration(name
     }
 }
 
-UDPConfiguration::UDPConfiguration(UDPConfiguration* source) : LinkConfiguration(source)
+UDPConfiguration::UDPConfiguration(UDPConfiguration* source)
+    : LinkConfiguration(source)
+    , _transmitOnly(false)
 {
     _copyFrom(source);
 }

src/comm/UDPLink.h

@@ -111,6 +111,14 @@ public:
      */
     void setLocalPort   (quint16 port);
 
+    /*!
+     * @brief Set the UDP port to be transmit only. Receive buffer is set to zero.
+     *
+     */
+    void setTransmitOnly (bool state) { _transmitOnly = state; }
+
+    bool isTransmitOnly () { return _transmitOnly; }
+
     /*!
      * @brief QML Interface
      */
@@ -142,6 +150,7 @@ private:
     QList<UDPCLient*>   _targetHosts;
     QStringList         _hostList;      ///< Exposed to QML
     quint16             _localPort;
+    bool                _transmitOnly;
 };
 
 class UDPLink : public LinkInterface

src/ui/preferences/MavlinkSettings.qml

@@ -153,6 +153,36 @@ Rectangle {
                             QGroundControl.isVersionCheckEnabled = checked
                         }
                     }
+
+                    FactCheckBox {
+                        id:         mavlinkForwardingChecked
+                        text:       qsTr("Enable MAVLink forwarding")
+                        fact:       QGroundControl.settingsManager.appSettings.forwardMavlink
+                        visible:    QGroundControl.settingsManager.appSettings.forwardMavlink.visible
+                    }
+
+                    Row {
+                        spacing:    ScreenTools.defaultFontPixelWidth
+                        QGCLabel {
+                            width:              _labelWidth
+                            anchors.baseline:   mavlinkForwardingHostNameField.baseline
+                            visible:            QGroundControl.settingsManager.appSettings.forwardMavlinkHostName.visible
+                            text:               qsTr("Host name:")
+                        }
+                        FactTextField {
+                            id:                     mavlinkForwardingHostNameField
+                            fact:                   QGroundControl.settingsManager.appSettings.forwardMavlinkHostName
+                            width:                  _valueWidth
+                            visible:                QGroundControl.settingsManager.appSettings.forwardMavlinkHostName.visible
+                            enabled:                QGroundControl.settingsManager.appSettings.forwardMavlink.rawValue
+                            anchors.verticalCenter: parent.verticalCenter
+                        }
+
+                    }
+                   QGCLabel {
+                        text:       qsTr("<i> Changing the host name requires restart of application. </i>")
+                        visible:    QGroundControl.settingsManager.appSettings.forwardMavlinkHostName.visible
+                    }
                 }
             }
             //-----------------------------------------------------------------