From 63f1c869986b7d72e083df1c44df5a5a07cc7c5a Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Sun, 21 May 2023 20:24:41 +0200 Subject: [PATCH] APM support forwarding: linkManager and MavlinkProtocol backend implementation --- src/comm/LinkManager.cc | 49 +++++++++++++++++++++++++++++++++++---------- src/comm/LinkManager.h | 44 ++++++++++++++++++++++++---------------- src/comm/MAVLinkProtocol.cc | 12 +++++++++++ 3 files changed, 77 insertions(+), 28 deletions(-) diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 672d0d1..529584d 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -46,8 +46,9 @@ 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 char* LinkManager::_defaultUDPLinkName = "UDP Link (AutoConnect)"; +const char* LinkManager::_mavlinkForwardingLinkName = "MAVLink Forwarding Link"; +const char* LinkManager::_mavlinkForwardingSupportLinkName = "MAVLink Support Forwarding Link"; const int LinkManager::_autoconnectUpdateTimerMSecs = 1000; #ifdef Q_OS_WIN @@ -185,6 +186,19 @@ SharedLinkInterfacePtr LinkManager::mavlinkForwardingLink() return nullptr; } +SharedLinkInterfacePtr LinkManager::mavlinkForwardingSupportLink() +{ + for (int i = 0; i < _rgLinks.count(); i++) { + SharedLinkConfigurationPtr linkConfig = _rgLinks[i]->linkConfiguration(); + if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _mavlinkForwardingSupportLinkName) { + SharedLinkInterfacePtr& link = _rgLinks[i]; + return link; + } + } + + return nullptr; +} + void LinkManager::disconnectAll(void) { QList links = _rgLinks; @@ -410,16 +424,8 @@ void LinkManager::_addMAVLinkForwardingLink(void) } if (!foundMAVLinkForwardingLink) { - 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); - - SharedLinkConfigurationPtr config = addConfiguration(udpConfig); - createConnectedLink(config); + _createDynamicForwardLink(_mavlinkForwardingLinkName, hostName); } } } @@ -812,6 +818,14 @@ void LinkManager::removeConfiguration(LinkConfiguration* config) } } +void LinkManager::createMavlinkForwardingSupportLink(void) +{ + QString hostName = _toolbox->settingsManager()->appSettings()->forwardMavlinkAPMSupportHostName()->rawValue().toString(); + _createDynamicForwardLink(_mavlinkForwardingSupportLinkName, hostName); + _mavlinkSupportForwardingEnabled = true; + emit mavlinkSupportForwardingEnabledChanged(); +} + void LinkManager::_removeConfiguration(LinkConfiguration* config) { _qmlConfigurations.removeOne(config); @@ -908,3 +922,16 @@ bool LinkManager::_isSerialPortConnected(void) #endif return false; } + +void LinkManager::_createDynamicForwardLink(const char* linkName, QString hostName) +{ + UDPConfiguration* udpConfig = new UDPConfiguration(linkName); + udpConfig->setDynamic(true); + + udpConfig->addHost(hostName); + + SharedLinkConfigurationPtr config = addConfiguration(udpConfig); + createConnectedLink(config); + + qCDebug(LinkManagerLog) << "New dynamic MAVLink forwarding port added: " << linkName << " hostname: " << hostName; +} \ No newline at end of file diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 7895a03..1c24584 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -52,20 +52,22 @@ public: LinkManager(QGCApplication* app, QGCToolbox* toolbox); ~LinkManager(); - Q_PROPERTY(bool isBluetoothAvailable READ isBluetoothAvailable CONSTANT) - Q_PROPERTY(QmlObjectListModel* linkConfigurations READ _qmlLinkConfigurations CONSTANT) - Q_PROPERTY(QStringList linkTypeStrings READ linkTypeStrings CONSTANT) - Q_PROPERTY(QStringList serialBaudRates READ serialBaudRates CONSTANT) - Q_PROPERTY(QStringList serialPortStrings READ serialPortStrings NOTIFY commPortStringsChanged) - Q_PROPERTY(QStringList serialPorts READ serialPorts NOTIFY commPortsChanged) + Q_PROPERTY(bool isBluetoothAvailable READ isBluetoothAvailable CONSTANT) + Q_PROPERTY(QmlObjectListModel* linkConfigurations READ _qmlLinkConfigurations CONSTANT) + Q_PROPERTY(QStringList linkTypeStrings READ linkTypeStrings CONSTANT) + Q_PROPERTY(QStringList serialBaudRates READ serialBaudRates CONSTANT) + Q_PROPERTY(QStringList serialPortStrings READ serialPortStrings NOTIFY commPortStringsChanged) + Q_PROPERTY(QStringList serialPorts READ serialPorts NOTIFY commPortsChanged) + Q_PROPERTY(bool mavlinkSupportForwardingEnabled READ mavlinkSupportForwardingEnabled NOTIFY mavlinkSupportForwardingEnabledChanged) /// Create/Edit Link Configuration - Q_INVOKABLE LinkConfiguration* createConfiguration (int type, const QString& name); - Q_INVOKABLE LinkConfiguration* startConfigurationEditing (LinkConfiguration* config); - Q_INVOKABLE void cancelConfigurationEditing (LinkConfiguration* config) { delete config; } - Q_INVOKABLE bool endConfigurationEditing (LinkConfiguration* config, LinkConfiguration* editedConfig); - Q_INVOKABLE bool endCreateConfiguration (LinkConfiguration* config); - Q_INVOKABLE void removeConfiguration (LinkConfiguration* config); + Q_INVOKABLE LinkConfiguration* createConfiguration (int type, const QString& name); + Q_INVOKABLE LinkConfiguration* startConfigurationEditing (LinkConfiguration* config); + Q_INVOKABLE void cancelConfigurationEditing (LinkConfiguration* config) { delete config; } + Q_INVOKABLE bool endConfigurationEditing (LinkConfiguration* config, LinkConfiguration* editedConfig); + Q_INVOKABLE bool endCreateConfiguration (LinkConfiguration* config); + Q_INVOKABLE void removeConfiguration (LinkConfiguration* config); + Q_INVOKABLE void createMavlinkForwardingSupportLink (void); // Called to signal app shutdown. Disconnects all links while turning off auto-connect. Q_INVOKABLE void shutdown(void); @@ -76,11 +78,12 @@ public: bool isBluetoothAvailable (void); - QList links (void) { return _rgLinks; } - QStringList linkTypeStrings (void) const; - QStringList serialBaudRates (void); - QStringList serialPortStrings (void); - QStringList serialPorts (void); + QList links (void) { return _rgLinks; } + QStringList linkTypeStrings (void) const; + QStringList serialBaudRates (void); + QStringList serialPortStrings (void); + QStringList serialPorts (void); + bool mavlinkSupportForwardingEnabled (void) { return _mavlinkSupportForwardingEnabled; } void loadLinkConfigurationList(); void saveLinkConfigurationList(); @@ -104,6 +107,9 @@ public: /// Returns pointer to the mavlink forwarding link, or nullptr if it does not exist SharedLinkInterfacePtr mavlinkForwardingLink(); + /// Returns pointer to the mavlink support forwarding link, or nullptr if it does not exist + SharedLinkInterfacePtr mavlinkForwardingSupportLink(); + void disconnectAll(void); #ifdef QT_DEBUG @@ -136,6 +142,7 @@ public: signals: void commPortStringsChanged(); void commPortsChanged(); + void mavlinkSupportForwardingEnabledChanged(); private slots: void _linkDisconnected (void); @@ -150,6 +157,7 @@ private: void _addZeroConfAutoConnectLink (void); void _addMAVLinkForwardingLink (void); bool _isSerialPortConnected (void); + void _createDynamicForwardLink (const char* linkName, QString hostName); #ifndef NO_SERIAL_LINK bool _portAlreadyConnected (const QString& portName); @@ -190,8 +198,10 @@ private: static const char* _defaultUDPLinkName; static const char* _mavlinkForwardingLinkName; + static const char* _mavlinkForwardingSupportLinkName; static const int _autoconnectUpdateTimerMSecs; static const int _autoconnectConnectDelayMSecs; + bool _mavlinkSupportForwardingEnabled = false; }; diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 6daef39..93f17ba 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -272,6 +272,18 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) } } + // MAVLink forwarding support + bool forwardingSupportEnabled = _linkMgr->mavlinkSupportForwardingEnabled(); + if (forwardingSupportEnabled) { + SharedLinkInterfacePtr forwardingSupportLink = _linkMgr->mavlinkForwardingSupportLink(); + + if (forwardingSupportLink) { + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + int len = mavlink_msg_to_send_buffer(buf, &_message); + forwardingSupportLink->writeBytesThreadSafe((const char*)buf, len); + } + } + //----------------------------------------------------------------- // Log data if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {