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
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 45
      src/comm/LinkManager.cc
  2. 10
      src/comm/LinkManager.h
  3. 12
      src/comm/MAVLinkProtocol.cc

45
src/comm/LinkManager.cc
Unescape View File

@ -48,6 +48,7 @@ QGC_LOGGING_CATEGORY(LinkManagerVerboseLog, "LinkManagerVerboseLog") @@ -48,6 +48,7 @@ QGC_LOGGING_CATEGORY(LinkManagerVerboseLog, "LinkManagerVerboseLog")
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() @@ -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<SharedLinkInterfacePtr> links = _rgLinks;
@ -410,16 +424,8 @@ void LinkManager::_addMAVLinkForwardingLink(void) @@ -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) @@ -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) @@ -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;
}

10
src/comm/LinkManager.h
Unescape View File

@ -58,6 +58,7 @@ public: @@ -58,6 +58,7 @@ public:
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);
@ -66,6 +67,7 @@ public: @@ -66,6 +67,7 @@ public:
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);
@ -81,6 +83,7 @@ public: @@ -81,6 +83,7 @@ public:
QStringList serialBaudRates (void);
QStringList serialPortStrings (void);
QStringList serialPorts (void);
bool mavlinkSupportForwardingEnabled (void) { return _mavlinkSupportForwardingEnabled; }
void loadLinkConfigurationList();
void saveLinkConfigurationList();
@ -104,6 +107,9 @@ public: @@ -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: @@ -136,6 +142,7 @@ public:
signals:
void commPortStringsChanged();
void commPortsChanged();
void mavlinkSupportForwardingEnabledChanged();
private slots:
void _linkDisconnected (void);
@ -150,6 +157,7 @@ private: @@ -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: @@ -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;
};

12
src/comm/MAVLinkProtocol.cc
Unescape View File

@ -272,6 +272,18 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) @@ -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()) {

Write Preview
Loading…
Cancel
Save