Updated mavlink forwarding implementation to forward all mavlink messages...

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

@@ -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

@@ -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)
 {

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/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();
 }
 
 /*!

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<LinkConfiguration> SharedLinkConfigurationPointer;

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

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;
 

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<LinkInterface*> 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()) {

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 {

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"); }
 

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 {

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("<i> Changing the host name requires restart of application. </i>")
+                        visible:    QGroundControl.settingsManager.appSettings.forwardMavlink.rawValue
+                    }
                 }
             }
             //-----------------------------------------------------------------