Added mavlink forwarding as an option to UDP connections. Enabling this...

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

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

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
 
@@ -82,6 +85,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)
     */
@@ -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
 
     /*!
@@ -113,6 +129,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
      *
@@ -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<LinkConfiguration> SharedLinkConfigurationPointer;

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)

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;
 

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

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 {