zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

APM support forwarding: linkManager and MavlinkProtocol backend implementation

QGC4.4
davidsastresas 2 years ago committed by David Sastre
parent
e74147f4c7
commit
63f1c86998
3 changed files with 77 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 49
      src/comm/LinkManager.cc
  2. 44
      src/comm/LinkManager.h
  3. 12
      src/comm/MAVLinkProtocol.cc

49
src/comm/LinkManager.cc
Unescape View File

@ -46,8 +46,9 @@
QGC_LOGGING_CATEGORY(LinkManagerLog, "LinkManagerLog") QGC_LOGGING_CATEGORY(LinkManagerLog, "LinkManagerLog")
QGC_LOGGING_CATEGORY(LinkManagerVerboseLog, "LinkManagerVerboseLog") QGC_LOGGING_CATEGORY(LinkManagerVerboseLog, "LinkManagerVerboseLog")
const char* LinkManager::_defaultUDPLinkName = "UDP Link (AutoConnect)"; const char* LinkManager::_defaultUDPLinkName = "UDP Link (AutoConnect)";
const char* LinkManager::_mavlinkForwardingLinkName = "MAVLink Forwarding Link"; const char* LinkManager::_mavlinkForwardingLinkName = "MAVLink Forwarding Link";
const char* LinkManager::_mavlinkForwardingSupportLinkName = "MAVLink Support Forwarding Link";
const int LinkManager::_autoconnectUpdateTimerMSecs = 1000; const int LinkManager::_autoconnectUpdateTimerMSecs = 1000;
#ifdef Q_OS_WIN #ifdef Q_OS_WIN
@ -185,6 +186,19 @@ SharedLinkInterfacePtr LinkManager::mavlinkForwardingLink()
return nullptr; 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) void LinkManager::disconnectAll(void)
{ {
QList<SharedLinkInterfacePtr> links = _rgLinks; QList<SharedLinkInterfacePtr> links = _rgLinks;
@ -410,16 +424,8 @@ void LinkManager::_addMAVLinkForwardingLink(void)
} }
if (!foundMAVLinkForwardingLink) { 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(); QString hostName = _toolbox->settingsManager()->appSettings()->forwardMavlinkHostName()->rawValue().toString();
udpConfig->addHost(hostName); _createDynamicForwardLink(_mavlinkForwardingLinkName, hostName);
SharedLinkConfigurationPtr config = addConfiguration(udpConfig);
createConnectedLink(config);
} }
} }
} }
@ -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) void LinkManager::_removeConfiguration(LinkConfiguration* config)
{ {
_qmlConfigurations.removeOne(config); _qmlConfigurations.removeOne(config);
@ -908,3 +922,16 @@ bool LinkManager::_isSerialPortConnected(void)
#endif #endif
return false; 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;
}

44
src/comm/LinkManager.h
Unescape View File

@ -52,20 +52,22 @@ public:
LinkManager(QGCApplication* app, QGCToolbox* toolbox); LinkManager(QGCApplication* app, QGCToolbox* toolbox);
~LinkManager(); ~LinkManager();
Q_PROPERTY(bool isBluetoothAvailable READ isBluetoothAvailable CONSTANT) Q_PROPERTY(bool isBluetoothAvailable READ isBluetoothAvailable CONSTANT)
Q_PROPERTY(QmlObjectListModel* linkConfigurations READ _qmlLinkConfigurations CONSTANT) Q_PROPERTY(QmlObjectListModel* linkConfigurations READ _qmlLinkConfigurations CONSTANT)
Q_PROPERTY(QStringList linkTypeStrings READ linkTypeStrings CONSTANT) Q_PROPERTY(QStringList linkTypeStrings READ linkTypeStrings CONSTANT)
Q_PROPERTY(QStringList serialBaudRates READ serialBaudRates CONSTANT) Q_PROPERTY(QStringList serialBaudRates READ serialBaudRates CONSTANT)
Q_PROPERTY(QStringList serialPortStrings READ serialPortStrings NOTIFY commPortStringsChanged) Q_PROPERTY(QStringList serialPortStrings READ serialPortStrings NOTIFY commPortStringsChanged)
Q_PROPERTY(QStringList serialPorts READ serialPorts NOTIFY commPortsChanged) Q_PROPERTY(QStringList serialPorts READ serialPorts NOTIFY commPortsChanged)
Q_PROPERTY(bool mavlinkSupportForwardingEnabled READ mavlinkSupportForwardingEnabled NOTIFY mavlinkSupportForwardingEnabledChanged)
/// Create/Edit Link Configuration /// Create/Edit Link Configuration
Q_INVOKABLE LinkConfiguration* createConfiguration (int type, const QString& name); Q_INVOKABLE LinkConfiguration* createConfiguration (int type, const QString& name);
Q_INVOKABLE LinkConfiguration* startConfigurationEditing (LinkConfiguration* config); Q_INVOKABLE LinkConfiguration* startConfigurationEditing (LinkConfiguration* config);
Q_INVOKABLE void cancelConfigurationEditing (LinkConfiguration* config) { delete config; } Q_INVOKABLE void cancelConfigurationEditing (LinkConfiguration* config) { delete config; }
Q_INVOKABLE bool endConfigurationEditing (LinkConfiguration* config, LinkConfiguration* editedConfig); Q_INVOKABLE bool endConfigurationEditing (LinkConfiguration* config, LinkConfiguration* editedConfig);
Q_INVOKABLE bool endCreateConfiguration (LinkConfiguration* config); Q_INVOKABLE bool endCreateConfiguration (LinkConfiguration* config);
Q_INVOKABLE void removeConfiguration (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. // Called to signal app shutdown. Disconnects all links while turning off auto-connect.
Q_INVOKABLE void shutdown(void); Q_INVOKABLE void shutdown(void);
@ -76,11 +78,12 @@ public:
bool isBluetoothAvailable (void); bool isBluetoothAvailable (void);
QList<SharedLinkInterfacePtr> links (void) { return _rgLinks; } QList<SharedLinkInterfacePtr> links (void) { return _rgLinks; }
QStringList linkTypeStrings (void) const; QStringList linkTypeStrings (void) const;
QStringList serialBaudRates (void); QStringList serialBaudRates (void);
QStringList serialPortStrings (void); QStringList serialPortStrings (void);
QStringList serialPorts (void); QStringList serialPorts (void);
bool mavlinkSupportForwardingEnabled (void) { return _mavlinkSupportForwardingEnabled; }
void loadLinkConfigurationList(); void loadLinkConfigurationList();
void saveLinkConfigurationList(); void saveLinkConfigurationList();
@ -104,6 +107,9 @@ public:
/// Returns pointer to the mavlink forwarding link, or nullptr if it does not exist /// Returns pointer to the mavlink forwarding link, or nullptr if it does not exist
SharedLinkInterfacePtr mavlinkForwardingLink(); SharedLinkInterfacePtr mavlinkForwardingLink();
/// Returns pointer to the mavlink support forwarding link, or nullptr if it does not exist
SharedLinkInterfacePtr mavlinkForwardingSupportLink();
void disconnectAll(void); void disconnectAll(void);
#ifdef QT_DEBUG #ifdef QT_DEBUG
@ -136,6 +142,7 @@ public:
signals: signals:
void commPortStringsChanged(); void commPortStringsChanged();
void commPortsChanged(); void commPortsChanged();
void mavlinkSupportForwardingEnabledChanged();
private slots: private slots:
void _linkDisconnected (void); void _linkDisconnected (void);
@ -150,6 +157,7 @@ private:
void _addZeroConfAutoConnectLink (void); void _addZeroConfAutoConnectLink (void);
void _addMAVLinkForwardingLink (void); void _addMAVLinkForwardingLink (void);
bool _isSerialPortConnected (void); bool _isSerialPortConnected (void);
void _createDynamicForwardLink (const char* linkName, QString hostName);
#ifndef NO_SERIAL_LINK #ifndef NO_SERIAL_LINK
bool _portAlreadyConnected (const QString& portName); bool _portAlreadyConnected (const QString& portName);
@ -190,8 +198,10 @@ private:
static const char* _defaultUDPLinkName; static const char* _defaultUDPLinkName;
static const char* _mavlinkForwardingLinkName; static const char* _mavlinkForwardingLinkName;
static const char* _mavlinkForwardingSupportLinkName;
static const int _autoconnectUpdateTimerMSecs; static const int _autoconnectUpdateTimerMSecs;
static const int _autoconnectConnectDelayMSecs; static const int _autoconnectConnectDelayMSecs;
bool _mavlinkSupportForwardingEnabled = false;
}; };

12
src/comm/MAVLinkProtocol.cc
Unescape View File

@ -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 // Log data
if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) { if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {

Write Preview
Loading…
Cancel
Save