diff --git a/src/comm/LinkConfiguration.cc b/src/comm/LinkConfiguration.cc index 8b7bd45cc9ab174929b900cbbc1793727586bddb..694f4d673c89bbcd057d598b4e36370aeddf571d 100644 --- a/src/comm/LinkConfiguration.cc +++ b/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(); } /*! diff --git a/src/comm/LinkConfiguration.h b/src/comm/LinkConfiguration.h index ee1345edecccf165b44f55dae5b59b6063c1f625..0543269638baa178bf02aa89055811f3369aed5b 100644 --- a/src/comm/LinkConfiguration.h +++ b/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 SharedLinkConfigurationPointer; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 41eff10cff6b0a7488eb05b301bd30213275a8b1..f6de788ba93a1da6456f7ba0f08cdbc881fc0b63 100644 --- a/src/comm/LinkManager.cc +++ b/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) diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 11d1856a299229611290112206add496ed79391c..6532e2606a5d2136e72f9d52e44ca44029c373dc 100644 --- a/src/comm/LinkManager.h +++ b/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; diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index de42986b59246fd05826c3cdb853d1a986e80a50..210eaab69c16480ffcecbc5682e26a5b53debaa0 100644 --- a/src/comm/UDPLink.h +++ b/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"); } diff --git a/src/ui/preferences/LinkSettings.qml b/src/ui/preferences/LinkSettings.qml index 99713fd6df437da21565f2b7758ae06f4960f102..8d5f63fead96bddb6cb6add802297ed4814bf740 100644 --- a/src/ui/preferences/LinkSettings.qml +++ b/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 {