diff --git a/src/Comms/BluetoothLink.cc b/src/Comms/BluetoothLink.cc index 0526fe997d7..00a715d5205 100644 --- a/src/Comms/BluetoothLink.cc +++ b/src/Comms/BluetoothLink.cc @@ -197,7 +197,7 @@ BluetoothConfiguration::BluetoothConfiguration(const QString& name) } -BluetoothConfiguration::BluetoothConfiguration(BluetoothConfiguration* source) +BluetoothConfiguration::BluetoothConfiguration(const BluetoothConfiguration* source) : LinkConfiguration(source) , _deviceDiscover(nullptr) , _device(source->device()) @@ -222,10 +222,10 @@ QString BluetoothConfiguration::settingsTitle() } } -void BluetoothConfiguration::copyFrom(LinkConfiguration *source) +void BluetoothConfiguration::copyFrom(const LinkConfiguration *source) { LinkConfiguration::copyFrom(source); - auto* usource = qobject_cast(source); + const BluetoothConfiguration* const usource = qobject_cast(source); Q_ASSERT(usource != nullptr); _device = usource->device(); } @@ -338,7 +338,7 @@ void BluetoothConfiguration::setDevName(const QString &name) } } -QString BluetoothConfiguration::address() +QString BluetoothConfiguration::address() const { #ifdef Q_OS_IOS return {}; diff --git a/src/Comms/BluetoothLink.h b/src/Comms/BluetoothLink.h index 8445fe8ba64..f0e28315ecb 100644 --- a/src/Comms/BluetoothLink.h +++ b/src/Comms/BluetoothLink.h @@ -66,7 +66,7 @@ class BluetoothConfiguration : public LinkConfiguration public: BluetoothConfiguration(const QString& name); - BluetoothConfiguration(BluetoothConfiguration* source); + BluetoothConfiguration(const BluetoothConfiguration* source); ~BluetoothConfiguration(); Q_PROPERTY(QString devName READ devName WRITE setDevName NOTIFY devNameChanged) @@ -77,16 +77,16 @@ class BluetoothConfiguration : public LinkConfiguration Q_INVOKABLE void startScan (void); Q_INVOKABLE void stopScan (void); - QString devName (void) { return _device.name; } - QString address (void); - QStringList nameList (void) { return _nameList; } - bool scanning (void) { return _deviceDiscover != nullptr; } - BluetoothData device (void) { return _device; } + QString devName (void) const { return _device.name; } + QString address (void) const; + QStringList nameList (void) const { return _nameList; } + bool scanning (void) const { return _deviceDiscover != nullptr; } + BluetoothData device (void) const { return _device; } void setDevName (const QString& name); /// LinkConfiguration overrides - LinkType type (void) override { return LinkConfiguration::TypeBluetooth; } - void copyFrom (LinkConfiguration* source) override; + LinkType type (void) const override { return LinkConfiguration::TypeBluetooth; } + void copyFrom (const LinkConfiguration* source) override; void loadSettings (QSettings& settings, const QString& root) override; void saveSettings (QSettings& settings, const QString& root) override; QString settingsURL (void) override { return "BluetoothSettings.qml"; } diff --git a/src/Comms/LinkConfiguration.cc b/src/Comms/LinkConfiguration.cc index 28ee009b13a..cfb4d46c62b 100644 --- a/src/Comms/LinkConfiguration.cc +++ b/src/Comms/LinkConfiguration.cc @@ -31,7 +31,7 @@ LinkConfiguration::LinkConfiguration(const QString &name, QObject *parent) // qCDebug(AudioOutputLog) << Q_FUNC_INFO << this; } -LinkConfiguration::LinkConfiguration(LinkConfiguration *copy, QObject *parent) +LinkConfiguration::LinkConfiguration(const LinkConfiguration *copy, QObject *parent) : QObject(parent) , _link(copy->_link) , _name(copy->name()) @@ -49,7 +49,7 @@ LinkConfiguration::~LinkConfiguration() // qCDebug(AudioOutputLog) << Q_FUNC_INFO << this; } -void LinkConfiguration::copyFrom(LinkConfiguration *source) +void LinkConfiguration::copyFrom(const LinkConfiguration *source) { Q_CHECK_PTR(source); @@ -102,38 +102,38 @@ LinkConfiguration *LinkConfiguration::createSettings(int type, const QString &na return config; } -LinkConfiguration *LinkConfiguration::duplicateSettings(LinkConfiguration *source) +LinkConfiguration *LinkConfiguration::duplicateSettings(const LinkConfiguration *source) { LinkConfiguration *dupe = nullptr; switch(source->type()) { #ifndef NO_SERIAL_LINK case TypeSerial: - dupe = new SerialConfiguration(qobject_cast(source)); + dupe = new SerialConfiguration(qobject_cast(source)); break; #endif case TypeUdp: - dupe = new UDPConfiguration(qobject_cast(source)); + dupe = new UDPConfiguration(qobject_cast(source)); break; case TypeTcp: - dupe = new TCPConfiguration(qobject_cast(source)); + dupe = new TCPConfiguration(qobject_cast(source)); break; #ifdef QGC_ENABLE_BLUETOOTH case TypeBluetooth: - dupe = new BluetoothConfiguration(qobject_cast(source)); + dupe = new BluetoothConfiguration(qobject_cast(source)); break; #endif case TypeLogReplay: - dupe = new LogReplayLinkConfiguration(qobject_cast(source)); + dupe = new LogReplayLinkConfiguration(qobject_cast(source)); break; #ifdef QT_DEBUG case TypeMock: - dupe = new MockConfiguration(qobject_cast(source)); + dupe = new MockConfiguration(qobject_cast(source)); break; #endif #ifndef QGC_AIRLINK_DISABLED case Airlink: - dupe = new AirlinkConfiguration(qobject_cast(source)); + dupe = new AirlinkConfiguration(qobject_cast(source)); break; #endif case TypeLast: @@ -152,7 +152,7 @@ void LinkConfiguration::setName(const QString &name) } } -void LinkConfiguration::setLink(SharedLinkInterfacePtr link) +void LinkConfiguration::setLink(const SharedLinkInterfacePtr link) { if (link.get() != this->link()) { _link = link; diff --git a/src/Comms/LinkConfiguration.h b/src/Comms/LinkConfiguration.h index ab4e887e238..0d644b6b65e 100644 --- a/src/Comms/LinkConfiguration.h +++ b/src/Comms/LinkConfiguration.h @@ -31,14 +31,14 @@ class LinkConfiguration : public QObject public: LinkConfiguration(const QString &name, QObject *parent = nullptr); - LinkConfiguration(LinkConfiguration *copy, QObject *parent = nullptr); + LinkConfiguration(const LinkConfiguration *copy, QObject *parent = nullptr); virtual ~LinkConfiguration(); QString name() const { return _name; } void setName(const QString &name); LinkInterface *link() { return _link.lock().get(); } - void setLink(std::shared_ptr link); + void setLink(const std::shared_ptr link); /// Is this a dynamic configuration? /// @return True if not persisted @@ -62,7 +62,7 @@ class LinkConfiguration : public QObject /// Copy instance data, When manipulating data, you create a copy of the configuration using the copy constructor, /// edit it and then transfer its content to the original using this method. /// @param[in] source The source instance (the edited copy) - virtual void copyFrom(LinkConfiguration *source); + virtual void copyFrom(const LinkConfiguration *source); /// The link types supported by QGC /// Any changes here MUST be reflected in LinkManager::linkTypeStrings() @@ -88,7 +88,7 @@ class LinkConfiguration : public QObject /// Connection type, pure virtual method returning one of the -TypeXxx types above. /// @return The type of links these settings belong to. - virtual LinkType type() = 0; + virtual LinkType type() const = 0; /// Load settings, Pure virtual method telling the instance to load its configuration. /// @param[in] settings The QSettings instance to use @@ -112,7 +112,7 @@ class LinkConfiguration : public QObject /// Duplicate configuration instance. Helper method to create a new instance copy for editing. /// @return A new copy of the given settings instance - static LinkConfiguration *duplicateSettings(LinkConfiguration *source); + static LinkConfiguration *duplicateSettings(const LinkConfiguration *source); /// Root path for QSettings /// @return The root path of the settings. diff --git a/src/Comms/LinkInterface.h b/src/Comms/LinkInterface.h index 4a20da23d18..41fce26a01b 100644 --- a/src/Comms/LinkInterface.h +++ b/src/Comms/LinkInterface.h @@ -37,6 +37,7 @@ class LinkInterface : public QThread virtual bool isSecureConnection() { return false; } ///< Returns true if the connection is secure (e.g. USB, wired ethernet) SharedLinkConfigurationPtr linkConfiguration() { return _config; } + const SharedLinkConfigurationPtr linkConfiguration() const { return _config; } uint8_t mavlinkChannel() const; bool mavlinkChannelIsSet() const; bool isPX4Flow() const { return _isPX4Flow; } diff --git a/src/Comms/LinkManager.cc b/src/Comms/LinkManager.cc index 585a01366cf..33cde95db46 100644 --- a/src/Comms/LinkManager.cc +++ b/src/Comms/LinkManager.cc @@ -17,6 +17,8 @@ #include "MultiVehicleManager.h" #include "DeviceInfo.h" #include "QGCLoggingCategory.h" +#include "QmlObjectListModel.h" +#include "UdpIODevice.h" #ifdef QGC_ENABLE_BLUETOOTH #include "BluetoothLink.h" @@ -44,41 +46,43 @@ #include #endif -#include #include +#include +#include #include -QGC_LOGGING_CATEGORY(LinkManagerLog, "LinkManagerLog") -QGC_LOGGING_CATEGORY(LinkManagerVerboseLog, "LinkManagerVerboseLog") +QGC_LOGGING_CATEGORY(LinkManagerLog, "qgc.comms.linkmanager") +QGC_LOGGING_CATEGORY(LinkManagerVerboseLog, "qgc.comms.linkmanager:verbose") LinkManager::LinkManager(QGCApplication* app, QGCToolbox* toolbox) : QGCTool(app, toolbox) - , _configUpdateSuspended(false) - , _configurationsLoaded(false) - , _connectionsSuspended(false) - , _mavlinkChannelsUsedBitMask(1) // We never use channel 0 to avoid sequence numbering problems - , _autoConnectSettings(nullptr) - , _mavlinkProtocol(nullptr) -{ - qmlRegisterUncreatableType ("QGroundControl", 1, 0, "LinkManager", "Reference only"); - qmlRegisterUncreatableType("QGroundControl", 1, 0, "LinkConfiguration", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl", 1, 0, "LinkInterface", "Reference only"); - - qmlRegisterUncreatableType("QGroundControl.Vehicle", 1, 0, "LinkInterface", "Reference only"); - qmlRegisterUncreatableType("QGroundControl", 1, 0, "LogReplayLink", "Reference only"); - qmlRegisterType ("QGroundControl", 1, 0, "LogReplayLinkController"); - - qRegisterMetaType(); - qRegisterMetaType("LinkInterface*"); + , _portListTimer(new QTimer(this)) + , _qmlConfigurations(new QmlObjectListModel(this)) #ifndef NO_SERIAL_LINK - qRegisterMetaType("QGCSerialPortInfo"); + , _nmeaSocket(new UdpIODevice(this)) +#endif +{ + // qCDebug(LinkManagerLog) << Q_FUNC_INFO << this; + + (void) qmlRegisterUncreatableType("QGroundControl", 1, 0, "LinkManager", "Reference only"); + (void) qmlRegisterUncreatableType("QGroundControl", 1, 0, "LinkConfiguration", "Reference only"); + (void) qmlRegisterUncreatableType("QGroundControl", 1, 0, "LinkInterface", "Reference only"); + + (void) qmlRegisterUncreatableType("QGroundControl.Vehicle", 1, 0, "LinkInterface", "Reference only"); + (void) qmlRegisterUncreatableType("QGroundControl", 1, 0, "LogReplayLink", "Reference only"); + (void) qmlRegisterType ("QGroundControl", 1, 0, "LogReplayLinkController"); + + (void) qRegisterMetaType("QAbstractSocket::SocketError"); + (void) qRegisterMetaType("LinkInterface*"); +#ifndef NO_SERIAL_LINK + (void) qRegisterMetaType("QGCSerialPortInfo"); #endif } LinkManager::~LinkManager() { - + // qCDebug(LinkManagerLog) << Q_FUNC_INFO << this; } void LinkManager::setToolbox(QGCToolbox *toolbox) @@ -88,21 +92,25 @@ void LinkManager::setToolbox(QGCToolbox *toolbox) _autoConnectSettings = toolbox->settingsManager()->autoConnectSettings(); _mavlinkProtocol = _toolbox->mavlinkProtocol(); - connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateAutoConnectLinks); - _portListTimer.start(_autoconnectUpdateTimerMSecs); // timeout must be long enough to get past bootloader on second pass + (void) connect(_portListTimer, &QTimer::timeout, this, &LinkManager::_updateAutoConnectLinks); + _portListTimer->start(_autoconnectUpdateTimerMSecs); // timeout must be long enough to get past bootloader on second pass +} + +QmlObjectListModel *LinkManager::_qmlLinkConfigurations() +{ + return _qmlConfigurations; } -// This should only be used by Qml code -void LinkManager::createConnectedLink(LinkConfiguration* config) +void LinkManager::createConnectedLink(const LinkConfiguration *config) { - for(int i = 0; i < _rgLinkConfigs.count(); i++) { - SharedLinkConfigurationPtr& sharedConfig = _rgLinkConfigs[i]; - if (sharedConfig.get() == config) + for (SharedLinkConfigurationPtr &sharedConfig : _rgLinkConfigs) { + if (sharedConfig.get() == config) { createConnectedLink(sharedConfig); + } } } -bool LinkManager::createConnectedLink(SharedLinkConfigurationPtr& config, bool isPX4Flow) +bool LinkManager::createConnectedLink(SharedLinkConfigurationPtr &config, bool isPX4Flow) { SharedLinkInterfacePtr link = nullptr; @@ -139,44 +147,45 @@ bool LinkManager::createConnectedLink(SharedLinkConfigurationPtr& config, bool i break; #endif case LinkConfiguration::TypeLast: + default: break; } - if (link) { - if (false == link->_allocateMavlinkChannel() ) { - qCWarning(LinkManagerLog) << "Link failed to setup mavlink channels"; - return false; - } + if (!link) { + return false; + } - _rgLinks.append(link); - config->setLink(link); + if (!link->_allocateMavlinkChannel()) { + qCWarning(LinkManagerLog) << "Link failed to setup mavlink channels"; + return false; + } - connect(link.get(), &LinkInterface::communicationError, _app, &QGCApplication::criticalMessageBoxOnMainThread); - connect(link.get(), &LinkInterface::bytesReceived, _mavlinkProtocol, &MAVLinkProtocol::receiveBytes); - connect(link.get(), &LinkInterface::bytesSent, _mavlinkProtocol, &MAVLinkProtocol::logSentBytes); - connect(link.get(), &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected); + (void) _rgLinks.append(link); + config->setLink(link); - _mavlinkProtocol->resetMetadataForLink(link.get()); - _mavlinkProtocol->setVersion(_mavlinkProtocol->getCurrentVersion()); + (void) connect(link.get(), &LinkInterface::communicationError, _app, &QGCApplication::criticalMessageBoxOnMainThread); + (void) connect(link.get(), &LinkInterface::bytesReceived, _mavlinkProtocol, &MAVLinkProtocol::receiveBytes); + (void) connect(link.get(), &LinkInterface::bytesSent, _mavlinkProtocol, &MAVLinkProtocol::logSentBytes); + (void) connect(link.get(), &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected); - if (!link->_connect()) { - link->_freeMavlinkChannel(); - _rgLinks.removeAt(_rgLinks.indexOf(link)); - config->setLink(nullptr); - return false; - } + _mavlinkProtocol->resetMetadataForLink(link.get()); + _mavlinkProtocol->setVersion(_mavlinkProtocol->getCurrentVersion()); - return true; + if (!link->_connect()) { + link->_freeMavlinkChannel(); + _rgLinks.removeAt(_rgLinks.indexOf(link)); + config->setLink(nullptr); + return false; } - return false; + return true; } SharedLinkInterfacePtr LinkManager::mavlinkForwardingLink() { - for (auto& link : _rgLinks) { - SharedLinkConfigurationPtr linkConfig = link->linkConfiguration(); - if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _mavlinkForwardingLinkName) { + for (SharedLinkInterfacePtr &link : _rgLinks) { + const SharedLinkConfigurationPtr linkConfig = link->linkConfiguration(); + if ((linkConfig->type() == LinkConfiguration::TypeUdp) && (linkConfig->name() == _mavlinkForwardingLinkName)) { return link; } } @@ -186,9 +195,9 @@ SharedLinkInterfacePtr LinkManager::mavlinkForwardingLink() SharedLinkInterfacePtr LinkManager::mavlinkForwardingSupportLink() { - for (auto& link : _rgLinks) { - SharedLinkConfigurationPtr linkConfig = link->linkConfiguration(); - if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _mavlinkForwardingSupportLinkName) { + for (SharedLinkInterfacePtr &link : _rgLinks) { + const SharedLinkConfigurationPtr linkConfig = link->linkConfiguration(); + if ((linkConfig->type() == LinkConfiguration::TypeUdp) && (linkConfig->name() == _mavlinkForwardingSupportLinkName)) { return link; } } @@ -196,30 +205,29 @@ SharedLinkInterfacePtr LinkManager::mavlinkForwardingSupportLink() return nullptr; } -void LinkManager::disconnectAll(void) +void LinkManager::disconnectAll() { - QList links = _rgLinks; - - for (const SharedLinkInterfacePtr& sharedLink: links) { + const QList links = _rgLinks; + for (const SharedLinkInterfacePtr &sharedLink: links) { sharedLink->disconnect(); } } -void LinkManager::_linkDisconnected(void) +void LinkManager::_linkDisconnected() { - LinkInterface* link = qobject_cast(sender()); + LinkInterface* const link = qobject_cast(sender()); if (!link || !containsLink(link)) { return; } - disconnect(link, &LinkInterface::communicationError, _app, &QGCApplication::criticalMessageBoxOnMainThread); - disconnect(link, &LinkInterface::bytesReceived, _mavlinkProtocol, &MAVLinkProtocol::receiveBytes); - disconnect(link, &LinkInterface::bytesSent, _mavlinkProtocol, &MAVLinkProtocol::logSentBytes); - disconnect(link, &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected); + (void) disconnect(link, &LinkInterface::communicationError, _app, &QGCApplication::criticalMessageBoxOnMainThread); + (void) disconnect(link, &LinkInterface::bytesReceived, _mavlinkProtocol, &MAVLinkProtocol::receiveBytes); + (void) disconnect(link, &LinkInterface::bytesSent, _mavlinkProtocol, &MAVLinkProtocol::logSentBytes); + (void) disconnect(link, &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected); link->_freeMavlinkChannel(); - for (int i=0; i<_rgLinks.count(); i++) { + for (int i = 0; i < _rgLinks.count(); i++) { if (_rgLinks[i].get() == link) { qCDebug(LinkManagerLog) << "LinkManager::_linkDisconnected" << _rgLinks[i]->linkConfiguration()->name() << _rgLinks[i].use_count(); _rgLinks.removeAt(i); @@ -228,64 +236,55 @@ void LinkManager::_linkDisconnected(void) } } -SharedLinkInterfacePtr LinkManager::sharedLinkInterfacePointerForLink(LinkInterface* link, bool ignoreNull) +SharedLinkInterfacePtr LinkManager::sharedLinkInterfacePointerForLink(LinkInterface *link) { - for (int i=0; i<_rgLinks.count(); i++) { - if (_rgLinks[i].get() == link) { - return _rgLinks[i]; + for (SharedLinkInterfacePtr &sharedLink: _rgLinks) { + if (sharedLink.get() == link) { + return sharedLink; } } - if (!ignoreNull) - qWarning() << "LinkManager::sharedLinkInterfaceForLink returning nullptr"; + qCWarning(LinkManagerLog) << "returning nullptr"; return SharedLinkInterfacePtr(nullptr); } /// @brief If all new connections should be suspended a message is displayed to the user and true -/// is returned; -bool LinkManager::_connectionsSuspendedMsg(void) +/// is returned; +bool LinkManager::_connectionsSuspendedMsg() { if (_connectionsSuspended) { qgcApp()->showAppMessage(tr("Connect not allowed: %1").arg(_connectionsSuspendedReason)); return true; - } else { - return false; } -} - -void LinkManager::setConnectionsSuspended(QString reason) -{ - _connectionsSuspended = true; - _connectionsSuspendedReason = reason; -} -void LinkManager::suspendConfigurationUpdates(bool suspend) -{ - _configUpdateSuspended = suspend; + return false; } void LinkManager::saveLinkConfigurationList() { QSettings settings; settings.remove(LinkConfiguration::settingsRoot()); + int trueCount = 0; for (int i = 0; i < _rgLinkConfigs.count(); i++) { SharedLinkConfigurationPtr linkConfig = _rgLinkConfigs[i]; - if (linkConfig) { - if (!linkConfig->isDynamic()) { - QString root = LinkConfiguration::settingsRoot(); - root += QString("/Link%1").arg(trueCount++); - settings.setValue(root + "/name", linkConfig->name()); - settings.setValue(root + "/type", linkConfig->type()); - settings.setValue(root + "/auto", linkConfig->isAutoConnect()); - settings.setValue(root + "/high_latency", linkConfig->isHighLatency()); - // Have the instance save its own values - linkConfig->saveSettings(settings, root); - } - } else { - qWarning() << "Internal error for link configuration in LinkManager"; + if (!linkConfig) { + qCWarning(LinkManagerLog) << "Internal error for link configuration in LinkManager"; + continue; + } + + if (linkConfig->isDynamic()) { + continue; } + + const QString root = LinkConfiguration::settingsRoot() + QStringLiteral("/Link%1").arg(trueCount++); + settings.setValue(root + "/name", linkConfig->name()); + settings.setValue(root + "/type", linkConfig->type()); + settings.setValue(root + "/auto", linkConfig->isAutoConnect()); + settings.setValue(root + "/high_latency", linkConfig->isHighLatency()); + linkConfig->saveSettings(settings, root); } + QString root(LinkConfiguration::settingsRoot()); settings.setValue(root + "/count", trueCount); } @@ -351,16 +350,16 @@ void LinkManager::loadLinkConfigurationList() addConfiguration(link); } } else { - qWarning() << "Link Configuration" << root << "has an empty name." ; + qCWarning(LinkManagerLog) << "Link Configuration" << root << "has an empty name." ; } } else { - qWarning() << "Link Configuration" << root << "has no name." ; + qCWarning(LinkManagerLog) << "Link Configuration" << root << "has no name." ; } } else { - qWarning() << "Link Configuration" << root << "an invalid type: " << type; + qCWarning(LinkManagerLog) << "Link Configuration" << root << "an invalid type: " << type; } } else { - qWarning() << "Link Configuration" << root << "has no type." ; + qCWarning(LinkManagerLog) << "Link Configuration" << root << "has no type." ; } } } @@ -369,53 +368,46 @@ void LinkManager::loadLinkConfigurationList() _configurationsLoaded = true; } -void LinkManager::_addUDPAutoConnectLink(void) +void LinkManager::_addUDPAutoConnectLink() { - if (_autoConnectSettings->autoConnectUDP()->rawValue().toBool()) { - bool foundUDP = false; - - for (int i = 0; i < _rgLinks.count(); i++) { - SharedLinkConfigurationPtr linkConfig = _rgLinks[i]->linkConfiguration(); - if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _defaultUDPLinkName) { - foundUDP = true; - break; - } - } + if (!_autoConnectSettings->autoConnectUDP()->rawValue().toBool()) { + return; + } - if (!foundUDP) { - qCDebug(LinkManagerLog) << "New auto-connect UDP port added"; - //-- Default UDPConfiguration is set up for autoconnect - UDPConfiguration* udpConfig = new UDPConfiguration(_defaultUDPLinkName); - udpConfig->setDynamic(true); - SharedLinkConfigurationPtr config = addConfiguration(udpConfig); - createConnectedLink(config); + for (SharedLinkInterfacePtr &link : _rgLinks) { + const SharedLinkConfigurationPtr linkConfig = link->linkConfiguration(); + if ((linkConfig->type() == LinkConfiguration::TypeUdp) && (linkConfig->name() == _defaultUDPLinkName)) { + return; } } + + qCDebug(LinkManagerLog) << "New auto-connect UDP port added"; + UDPConfiguration *udpConfig = new UDPConfiguration(_defaultUDPLinkName); + udpConfig->setDynamic(true); + SharedLinkConfigurationPtr config = addConfiguration(udpConfig); + createConnectedLink(config); } -void LinkManager::_addMAVLinkForwardingLink(void) +void LinkManager::_addMAVLinkForwardingLink() { - if (_toolbox->settingsManager()->appSettings()->forwardMavlink()->rawValue().toBool()) { - bool foundMAVLinkForwardingLink = false; - - for (int i=0; i<_rgLinks.count(); i++) { - SharedLinkConfigurationPtr linkConfig = _rgLinks[i]->linkConfiguration(); - if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _mavlinkForwardingLinkName) { - foundMAVLinkForwardingLink = true; - // TODO: should we check if the host/port matches the mavlinkForwardHostName setting and update if it does not match? - break; - } - } + if (!_toolbox->settingsManager()->appSettings()->forwardMavlink()->rawValue().toBool()) { + return; + } - if (!foundMAVLinkForwardingLink) { - QString hostName = _toolbox->settingsManager()->appSettings()->forwardMavlinkHostName()->rawValue().toString(); - _createDynamicForwardLink(_mavlinkForwardingLinkName, hostName); + for (SharedLinkInterfacePtr &link : _rgLinks) { + const SharedLinkConfigurationPtr linkConfig = link->linkConfiguration(); + if ((linkConfig->type() == LinkConfiguration::TypeUdp) && (linkConfig->name() == _mavlinkForwardingLinkName)) { + // TODO: should we check if the host/port matches the mavlinkForwardHostName setting and update if it does not match? + return; } } + + const QString hostName = _toolbox->settingsManager()->appSettings()->forwardMavlinkHostName()->rawValue().toString(); + _createDynamicForwardLink(_mavlinkForwardingLinkName, hostName); } #ifdef QGC_ZEROCONF_ENABLED -void LinkManager::_addZeroConfAutoConnectLink(void) +void LinkManager::_addZeroConfAutoConnectLink() { if (!_autoConnectSettings->autoConnectZeroConf()->rawValue().toBool()) { return; @@ -426,9 +418,9 @@ void LinkManager::_addZeroConfAutoConnectLink(void) server.reset(new QMdnsEngine::Server()); browser.reset(new QMdnsEngine::Browser(server.get(), QMdnsEngine::MdnsBrowseType)); - auto checkIfConnectionLinkExist = [this](LinkConfiguration::LinkType linkType, const QString& linkName){ - for (const auto& link : std::as_const(_rgLinks)) { - SharedLinkConfigurationPtr linkConfig = link->linkConfiguration(); + auto checkIfConnectionLinkExist = [this](LinkConfiguration::LinkType linkType, const QString &linkName) { + for (const SharedLinkConfigurationPtr &link : std::as_const(_rgLinks)) { + const SharedLinkConfigurationPtr linkConfig = link->linkConfiguration(); if (linkConfig->type() == linkType && linkConfig->name() == linkName) { return true; } @@ -437,28 +429,28 @@ void LinkManager::_addZeroConfAutoConnectLink(void) return false; }; - connect(browser.get(), &QMdnsEngine::Browser::serviceAdded, this, [checkIfConnectionLinkExist, this](const QMdnsEngine::Service &service) { + (void) connect(browser.get(), &QMdnsEngine::Browser::serviceAdded, this, [checkIfConnectionLinkExist, this](const QMdnsEngine::Service &service) { qCDebug(LinkManagerVerboseLog) << "Found Zero-Conf:" << service.type() << service.name() << service.hostname() << service.port() << service.attributes(); - if(!service.type().startsWith("_mavlink")) { + if (!service.type().startsWith("_mavlink")) { return; } // Windows dont accept trailling dots in mdns // http://www.dns-sd.org/trailingdotsindomainnames.html QString hostname = service.hostname(); - if(hostname.endsWith('.')) { + if (hostname.endsWith('.')) { hostname.chop(1); } - if(service.type().startsWith("_mavlink._udp")) { + if (service.type().startsWith("_mavlink._udp")) { static QString udpName("ZeroConf UDP"); if (checkIfConnectionLinkExist(LinkConfiguration::TypeUdp, udpName)) { qCDebug(LinkManagerVerboseLog) << "Connection already exist"; return; } - auto link = new UDPConfiguration(udpName); + UDPConfiguration link = new UDPConfiguration(udpName); link->addHost(hostname, service.port()); link->setAutoConnect(true); link->setDynamic(true); @@ -467,14 +459,14 @@ void LinkManager::_addZeroConfAutoConnectLink(void) return; } - if(service.type().startsWith("_mavlink._tcp")) { + if (service.type().startsWith("_mavlink._tcp")) { static QString tcpName("ZeroConf TCP"); if (checkIfConnectionLinkExist(LinkConfiguration::TypeTcp, tcpName)) { qCDebug(LinkManagerVerboseLog) << "Connection already exist"; return; } - auto link = new TCPConfiguration(tcpName); + TCPConfiguration link = new TCPConfiguration(tcpName); link->setHost(hostname); link->setPort(service.port()); link->setAutoConnect(true); @@ -487,7 +479,7 @@ void LinkManager::_addZeroConfAutoConnectLink(void) } #endif -void LinkManager::_updateAutoConnectLinks(void) +void LinkManager::_updateAutoConnectLinks() { if (_connectionsSuspended || qgcApp()->runningUnitTests()) { return; @@ -501,15 +493,14 @@ void LinkManager::_updateAutoConnectLinks(void) // check to see if nmea gps is configured for UDP input, if so, set it up to connect if (_autoConnectSettings->autoConnectNmeaPort()->cookedValueString() == "UDP Port") { - if (_nmeaSocket.localPort() != _autoConnectSettings->nmeaUdpPort()->rawValue().toUInt() - || _nmeaSocket.state() != UdpIODevice::BoundState) { + if ((_nmeaSocket->localPort() != _autoConnectSettings->nmeaUdpPort()->rawValue().toUInt()) + || (_nmeaSocket->state() != UdpIODevice::BoundState)) { qCDebug(LinkManagerLog) << "Changing port for UDP NMEA stream"; - _nmeaSocket.close(); - _nmeaSocket.bind(QHostAddress::AnyIPv4, _autoConnectSettings->nmeaUdpPort()->rawValue().toUInt()); - _toolbox->qgcPositionManager()->setNmeaSourceDevice(&_nmeaSocket); + _nmeaSocket->close(); + _nmeaSocket->bind(QHostAddress::AnyIPv4, _autoConnectSettings->nmeaUdpPort()->rawValue().toUInt()); + _toolbox->qgcPositionManager()->setNmeaSourceDevice(_nmeaSocket); } #ifndef NO_SERIAL_LINK - // close serial port if (_nmeaPort) { _nmeaPort->close(); delete _nmeaPort; @@ -518,130 +509,15 @@ void LinkManager::_updateAutoConnectLinks(void) } #endif } else { - _nmeaSocket.close(); + _nmeaSocket->close(); } #ifndef NO_SERIAL_LINK - QStringList currentPorts; - QList portList; -#ifdef Q_OS_ANDROID - // Android builds only support a single serial connection. Repeatedly calling availablePorts after that one serial - // port is connected leaks file handles due to a bug somewhere in android serial code. In order to work around that - // bug after we connect the first serial port we stop probing for additional ports. - if (!_isSerialPortConnected()) { - portList = QGCSerialPortInfo::availablePorts(); - } -#else - portList = QGCSerialPortInfo::availablePorts(); + _addSerialAutoConnectLink(); #endif - - // Iterate Comm Ports - for (const QGCSerialPortInfo &portInfo: portList) { - qCDebug(LinkManagerVerboseLog) << "-----------------------------------------------------"; - qCDebug(LinkManagerVerboseLog) << "portName: " << portInfo.portName(); - qCDebug(LinkManagerVerboseLog) << "systemLocation: " << portInfo.systemLocation(); - qCDebug(LinkManagerVerboseLog) << "description: " << portInfo.description(); - qCDebug(LinkManagerVerboseLog) << "manufacturer: " << portInfo.manufacturer(); - qCDebug(LinkManagerVerboseLog) << "serialNumber: " << portInfo.serialNumber(); - qCDebug(LinkManagerVerboseLog) << "vendorIdentifier: " << portInfo.vendorIdentifier(); - qCDebug(LinkManagerVerboseLog) << "productIdentifier: " << portInfo.productIdentifier(); - - // Save port name - currentPorts << portInfo.systemLocation(); - - QGCSerialPortInfo::BoardType_t boardType; - QString boardName; - - // check to see if nmea gps is configured for current Serial port, if so, set it up to connect - if (portInfo.systemLocation().trimmed() == _autoConnectSettings->autoConnectNmeaPort()->cookedValueString()) { - if (portInfo.systemLocation().trimmed() != _nmeaDeviceName) { - _nmeaDeviceName = portInfo.systemLocation().trimmed(); - qCDebug(LinkManagerLog) << "Configuring nmea port" << _nmeaDeviceName; - QSerialPort* newPort = new QSerialPort(portInfo, this); - _nmeaBaud = _autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt(); - newPort->setBaudRate(static_cast(_nmeaBaud)); - qCDebug(LinkManagerLog) << "Configuring nmea baudrate" << _nmeaBaud; - // This will stop polling old device if previously set - _toolbox->qgcPositionManager()->setNmeaSourceDevice(newPort); - if (_nmeaPort) { - delete _nmeaPort; - } - _nmeaPort = newPort; - } else if (_autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt() != _nmeaBaud) { - _nmeaBaud = _autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt(); - _nmeaPort->setBaudRate(static_cast(_nmeaBaud)); - qCDebug(LinkManagerLog) << "Configuring nmea baudrate" << _nmeaBaud; - } - } else if (portInfo.getBoardInfo(boardType, boardName)) { - // Should we be auto-connecting to this board type? - if (!_allowAutoConnectToBoard(boardType)) { - continue; - } - - if (portInfo.isBootloader()) { - // Don't connect to bootloader - qCDebug(LinkManagerLog) << "Waiting for bootloader to finish" << portInfo.systemLocation(); - continue; - } - if (_portAlreadyConnected(portInfo.systemLocation()) || _autoConnectRTKPort == portInfo.systemLocation()) { - qCDebug(LinkManagerVerboseLog) << "Skipping existing autoconnect" << portInfo.systemLocation(); - } else if (!_autoconnectPortWaitList.contains(portInfo.systemLocation())) { - // We don't connect to the port the first time we see it. The ability to correctly detect whether we - // are in the bootloader is flaky from a cross-platform standpoint. So by putting it on a wait list - // and only connect on the second pass we leave enough time for the board to boot up. - qCDebug(LinkManagerLog) << "Waiting for next autoconnect pass" << portInfo.systemLocation() << boardName; - _autoconnectPortWaitList[portInfo.systemLocation()] = 1; - } else if (++_autoconnectPortWaitList[portInfo.systemLocation()] * _autoconnectUpdateTimerMSecs > _autoconnectConnectDelayMSecs) { - SerialConfiguration* pSerialConfig = nullptr; - _autoconnectPortWaitList.remove(portInfo.systemLocation()); - switch (boardType) { - case QGCSerialPortInfo::BoardTypePixhawk: - pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed())); - pSerialConfig->setUsbDirect(true); - break; - case QGCSerialPortInfo::BoardTypePX4Flow: - pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed())); - break; - case QGCSerialPortInfo::BoardTypeSiKRadio: - pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed())); - break; - case QGCSerialPortInfo::BoardTypeOpenPilot: - pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed())); - break; - case QGCSerialPortInfo::BoardTypeRTKGPS: - qCDebug(LinkManagerLog) << "RTK GPS auto-connected" << portInfo.portName().trimmed(); - _autoConnectRTKPort = portInfo.systemLocation(); - _toolbox->gpsManager()->connectGPS(portInfo.systemLocation(), boardName); - break; - default: - qCWarning(LinkManagerLog) << "Internal error: Unknown board type" << boardType; - continue; - } - if (pSerialConfig) { - qCDebug(LinkManagerLog) << "New auto-connect port added: " << pSerialConfig->name() << portInfo.systemLocation(); - pSerialConfig->setBaud (boardType == QGCSerialPortInfo::BoardTypeSiKRadio ? 57600 : 115200); - pSerialConfig->setDynamic (true); - pSerialConfig->setPortName (portInfo.systemLocation()); - pSerialConfig->setAutoConnect(true); - - SharedLinkConfigurationPtr sharedConfig(pSerialConfig); - createConnectedLink(sharedConfig, boardType == QGCSerialPortInfo::BoardTypePX4Flow); - } - } - } - } - - // Check for RTK GPS connection gone - if (!_autoConnectRTKPort.isEmpty() && !currentPorts.contains(_autoConnectRTKPort)) { - qCDebug(LinkManagerLog) << "RTK GPS disconnected" << _autoConnectRTKPort; - _toolbox->gpsManager()->disconnectGPS(); - _autoConnectRTKPort.clear(); - } - -#endif // NO_SERIAL_LINK } -void LinkManager::shutdown(void) +void LinkManager::shutdown() { setConnectionsSuspended(tr("Shutdown")); disconnectAll(); @@ -652,57 +528,60 @@ void LinkManager::shutdown(void) } } -QStringList LinkManager::linkTypeStrings(void) const +QStringList LinkManager::linkTypeStrings() const { //-- Must follow same order as enum LinkType in LinkConfiguration.h static QStringList list; - if(!list.size()) - { + if (!list.isEmpty()) { + return list; + } + #ifndef NO_SERIAL_LINK - list += tr("Serial"); + list += tr("Serial"); #endif - list += tr("UDP"); - list += tr("TCP"); + list += tr("UDP"); + list += tr("TCP"); #ifdef QGC_ENABLE_BLUETOOTH - list += "Bluetooth"; + list += tr("Bluetooth"); #endif #ifdef QT_DEBUG - list += tr("Mock Link"); + list += tr("Mock Link"); #endif #ifndef QGC_AIRLINK_DISABLED - list += tr("AirLink"); + list += tr("AirLink"); #endif - list += tr("Log Replay"); - if (list.size() != static_cast(LinkConfiguration::TypeLast)) { - qWarning() << "Internal error"; - } + list += tr("Log Replay"); + + if (list.size() != static_cast(LinkConfiguration::TypeLast)) { + qCWarning(LinkManagerLog) << "Internal error"; } + return list; } -bool LinkManager::endConfigurationEditing(LinkConfiguration* config, LinkConfiguration* editedConfig) +void LinkManager::endConfigurationEditing(LinkConfiguration *config, LinkConfiguration *editedConfig) { - if (config && editedConfig) { - config->copyFrom(editedConfig); - saveLinkConfigurationList(); - emit config->nameChanged(config->name()); - // Discard temporary duplicate - delete editedConfig; - } else { - qWarning() << "Internal error"; + if (!config || !editedConfig) { + qCWarning(LinkManagerLog) << "Internal error"; + return; } - return true; + + config->copyFrom(editedConfig); + saveLinkConfigurationList(); + emit config->nameChanged(config->name()); + // Discard temporary duplicate + delete editedConfig; } -bool LinkManager::endCreateConfiguration(LinkConfiguration* config) +void LinkManager::endCreateConfiguration(LinkConfiguration *config) { - if (config) { - addConfiguration(config); - saveLinkConfigurationList(); - } else { - qWarning() << "Internal error"; + if (!config) { + qCWarning(LinkManagerLog) << "Internal error"; + return; } - return true; + + addConfiguration(config); + saveLinkConfigurationList(); } LinkConfiguration* LinkManager::createConfiguration(int type, const QString& name) @@ -712,40 +591,43 @@ LinkConfiguration* LinkManager::createConfiguration(int type, const QString& nam _updateSerialPorts(); } #endif + return LinkConfiguration::createSettings(type, name); } -LinkConfiguration* LinkManager::startConfigurationEditing(LinkConfiguration* config) +LinkConfiguration *LinkManager::startConfigurationEditing(LinkConfiguration* config) { - if (config) { -#ifndef NO_SERIAL_LINK - if (config->type() == LinkConfiguration::TypeSerial) { - _updateSerialPorts(); - } -#endif - return LinkConfiguration::duplicateSettings(config); - } else { - qWarning() << "Internal error"; + if (!config) { + qCWarning(LinkManagerLog) << "Internal error"; return nullptr; } + +#ifndef NO_SERIAL_LINK + if (config->type() == LinkConfiguration::TypeSerial) { + _updateSerialPorts(); + } +#endif + + return LinkConfiguration::duplicateSettings(config); } void LinkManager::removeConfiguration(LinkConfiguration* config) { - if (config) { - LinkInterface* link = config->link(); - if (link) { - link->disconnect(); - } + if (!config) { + qCWarning(LinkManagerLog) << "Internal error"; + return; + } - _removeConfiguration(config); - saveLinkConfigurationList(); - } else { - qWarning() << "Internal error"; + LinkInterface* const link = config->link(); + if (link) { + link->disconnect(); } + + _removeConfiguration(config); + saveLinkConfigurationList(); } -void LinkManager::createMavlinkForwardingSupportLink(void) +void LinkManager::createMavlinkForwardingSupportLink() { QString hostName = _toolbox->settingsManager()->appSettings()->forwardMavlinkAPMSupportHostName()->rawValue().toString(); _createDynamicForwardLink(_mavlinkForwardingSupportLinkName, hostName); @@ -753,131 +635,263 @@ void LinkManager::createMavlinkForwardingSupportLink(void) emit mavlinkSupportForwardingEnabledChanged(); } -void LinkManager::_removeConfiguration(LinkConfiguration* config) +void LinkManager::_removeConfiguration(const LinkConfiguration *config) { - _qmlConfigurations.removeOne(config); - for (int i=0; i<_rgLinkConfigs.count(); i++) { - if (_rgLinkConfigs[i].get() == config) { - _rgLinkConfigs.removeAt(i); + (void) _qmlConfigurations->removeOne(config); + + for (auto it = _rgLinkConfigs.begin(); it != _rgLinkConfigs.end(); ++it) { + if (it->get() == config) { + _rgLinkConfigs.erase(it); return; } } - qWarning() << "LinkManager::_removeConfiguration called with unknown config"; + + qCWarning(LinkManagerLog) << Q_FUNC_INFO << "called with unknown config"; } -bool LinkManager::isBluetoothAvailable(void) +bool LinkManager::isBluetoothAvailable() { return QGCDeviceInfo::isBluetoothAvailable(); } -bool LinkManager::containsLink(LinkInterface* link) +bool LinkManager::containsLink(const LinkInterface *link) { - for (int i=0; i<_rgLinks.count(); i++) { - if (_rgLinks[i].get() == link) { + for (const SharedLinkInterfacePtr &sharedLink : _rgLinks) { + if (sharedLink.get() == link) { return true; } } + return false; } -SharedLinkConfigurationPtr LinkManager::addConfiguration(LinkConfiguration* config) +SharedLinkConfigurationPtr LinkManager::addConfiguration(LinkConfiguration *config) { - _qmlConfigurations.append(config); - _rgLinkConfigs.append(SharedLinkConfigurationPtr(config)); + (void) _qmlConfigurations->append(config); + (void) _rgLinkConfigs.append(SharedLinkConfigurationPtr(config)); + return _rgLinkConfigs.last(); } -void LinkManager::startAutoConnectedLinks(void) +void LinkManager::startAutoConnectedLinks() { - SharedLinkConfigurationPtr conf; - for(int i = 0; i < _rgLinkConfigs.count(); i++) { - conf = _rgLinkConfigs[i]; - if (conf->isAutoConnect()) - createConnectedLink(conf); + for (SharedLinkConfigurationPtr &sharedConfig : _rgLinkConfigs) { + if (sharedConfig->isAutoConnect()) { + createConnectedLink(sharedConfig); + } } } -uint8_t LinkManager::allocateMavlinkChannel(void) +uint8_t LinkManager::allocateMavlinkChannel() { - // Find a mavlink channel to use for this link for (uint8_t mavlinkChannel = 0; mavlinkChannel < MAVLINK_COMM_NUM_BUFFERS; mavlinkChannel++) { - if (!(_mavlinkChannelsUsedBitMask & 1 << mavlinkChannel)) { - mavlink_reset_channel_status(mavlinkChannel); - // Start the channel on Mav 1 protocol - mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel); - mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - _mavlinkChannelsUsedBitMask |= 1 << mavlinkChannel; - qCDebug(LinkManagerLog) << "allocateMavlinkChannel" << mavlinkChannel; - return mavlinkChannel; + if (_mavlinkChannelsUsedBitMask & (1 << mavlinkChannel)) { + continue; } + + mavlink_reset_channel_status(mavlinkChannel); + mavlink_status_t* const mavlinkStatus = mavlink_get_channel_status(mavlinkChannel); + mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + _mavlinkChannelsUsedBitMask |= (1 << mavlinkChannel); + qCDebug(LinkManagerLog) << "allocateMavlinkChannel" << mavlinkChannel; + return mavlinkChannel; } + qWarning(LinkManagerLog) << "allocateMavlinkChannel: all channels reserved!"; - return invalidMavlinkChannel(); // All channels reserved + return invalidMavlinkChannel(); } void LinkManager::freeMavlinkChannel(uint8_t channel) { qCDebug(LinkManagerLog) << "freeMavlinkChannel" << channel; + if (invalidMavlinkChannel() == channel) { return; } + _mavlinkChannelsUsedBitMask &= ~(1 << channel); } -LogReplayLink* LinkManager::startLogReplay(const QString& logFile) +LogReplayLink *LinkManager::startLogReplay(const QString &logFile) { - LogReplayLinkConfiguration* linkConfig = new LogReplayLinkConfiguration(tr("Log Replay")); + LogReplayLinkConfiguration* const linkConfig = new LogReplayLinkConfiguration(tr("Log Replay")); linkConfig->setLogFilename(logFile); linkConfig->setName(linkConfig->logFilenameShort()); SharedLinkConfigurationPtr sharedConfig = addConfiguration(linkConfig); if (createConnectedLink(sharedConfig)) { return qobject_cast(sharedConfig->link()); - } else { - return nullptr; } + + return nullptr; } -void LinkManager::_createDynamicForwardLink(const char* linkName, QString hostName) +void LinkManager::_createDynamicForwardLink(const char *linkName, const QString &hostName) { - UDPConfiguration* udpConfig = new UDPConfiguration(linkName); - udpConfig->setDynamic(true); + UDPConfiguration* const 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; + qCDebug(LinkManagerLog) << "New dynamic MAVLink forwarding port added:" << linkName << " hostname:" << hostName; } -bool LinkManager::isLinkUSBDirect(LinkInterface* link) +bool LinkManager::isLinkUSBDirect(const LinkInterface *link) { #ifndef NO_SERIAL_LINK - SerialLink* serialLink = qobject_cast(link); - if (serialLink) { - SharedLinkConfigurationPtr config = serialLink->linkConfiguration(); - if (config) { - SerialConfiguration* serialConfig = qobject_cast(config.get()); - if (serialConfig && serialConfig->usbDirect()) { - return link; - } - } + const SerialLink* const serialLink = qobject_cast(link); + if (!serialLink) { + return false; + } + + const SharedLinkConfigurationPtr config = serialLink->linkConfiguration(); + if (!config) { + return false; + } + + const SerialConfiguration* const serialConfig = qobject_cast(config.get()); + if (serialConfig && serialConfig->usbDirect()) { + return link; } #endif return false; } -void LinkManager::resetMavlinkSigning(void) +void LinkManager::resetMavlinkSigning() { - for (const SharedLinkInterfacePtr& sharedLink: _rgLinks) { + for (const SharedLinkInterfacePtr &sharedLink: _rgLinks) { sharedLink->initMavlinkSigning(); } } #ifndef NO_SERIAL_LINK // Serial Only Functions +void LinkManager::_addSerialAutoConnectLink() +{ + if (_connectionsSuspended || qgcApp()->runningUnitTests()) { + return; + } + + QList portList; +#ifdef Q_OS_ANDROID + // Android builds only support a single serial connection. Repeatedly calling availablePorts after that one serial + // port is connected leaks file handles due to a bug somewhere in android serial code. In order to work around that + // bug after we connect the first serial port we stop probing for additional ports. + if (!_isSerialPortConnected()) { + portList = QGCSerialPortInfo::availablePorts(); + } +#else + portList = QGCSerialPortInfo::availablePorts(); +#endif + + QStringList currentPorts; + for (const QGCSerialPortInfo &portInfo: portList) { + qCDebug(LinkManagerVerboseLog) << "-----------------------------------------------------"; + qCDebug(LinkManagerVerboseLog) << "portName: " << portInfo.portName(); + qCDebug(LinkManagerVerboseLog) << "systemLocation: " << portInfo.systemLocation(); + qCDebug(LinkManagerVerboseLog) << "description: " << portInfo.description(); + qCDebug(LinkManagerVerboseLog) << "manufacturer: " << portInfo.manufacturer(); + qCDebug(LinkManagerVerboseLog) << "serialNumber: " << portInfo.serialNumber(); + qCDebug(LinkManagerVerboseLog) << "vendorIdentifier: " << portInfo.vendorIdentifier(); + qCDebug(LinkManagerVerboseLog) << "productIdentifier: " << portInfo.productIdentifier(); + + currentPorts << portInfo.systemLocation(); + + QGCSerialPortInfo::BoardType_t boardType; + QString boardName; + + // check to see if nmea gps is configured for current Serial port, if so, set it up to connect + if (portInfo.systemLocation().trimmed() == _autoConnectSettings->autoConnectNmeaPort()->cookedValueString()) { + if (portInfo.systemLocation().trimmed() != _nmeaDeviceName) { + _nmeaDeviceName = portInfo.systemLocation().trimmed(); + qCDebug(LinkManagerLog) << "Configuring nmea port" << _nmeaDeviceName; + QSerialPort* newPort = new QSerialPort(portInfo, this); + _nmeaBaud = _autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt(); + newPort->setBaudRate(static_cast(_nmeaBaud)); + qCDebug(LinkManagerLog) << "Configuring nmea baudrate" << _nmeaBaud; + // This will stop polling old device if previously set + _toolbox->qgcPositionManager()->setNmeaSourceDevice(newPort); + if (_nmeaPort) { + delete _nmeaPort; + } + _nmeaPort = newPort; + } else if (_autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt() != _nmeaBaud) { + _nmeaBaud = _autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt(); + _nmeaPort->setBaudRate(static_cast(_nmeaBaud)); + qCDebug(LinkManagerLog) << "Configuring nmea baudrate" << _nmeaBaud; + } + } else if (portInfo.getBoardInfo(boardType, boardName)) { + // Should we be auto-connecting to this board type? + if (!_allowAutoConnectToBoard(boardType)) { + continue; + } + + if (portInfo.isBootloader()) { + // Don't connect to bootloader + qCDebug(LinkManagerLog) << "Waiting for bootloader to finish" << portInfo.systemLocation(); + continue; + } + if (_portAlreadyConnected(portInfo.systemLocation()) || _autoConnectRTKPort == portInfo.systemLocation()) { + qCDebug(LinkManagerVerboseLog) << "Skipping existing autoconnect" << portInfo.systemLocation(); + } else if (!_autoconnectPortWaitList.contains(portInfo.systemLocation())) { + // We don't connect to the port the first time we see it. The ability to correctly detect whether we + // are in the bootloader is flaky from a cross-platform standpoint. So by putting it on a wait list + // and only connect on the second pass we leave enough time for the board to boot up. + qCDebug(LinkManagerLog) << "Waiting for next autoconnect pass" << portInfo.systemLocation() << boardName; + _autoconnectPortWaitList[portInfo.systemLocation()] = 1; + } else if (++_autoconnectPortWaitList[portInfo.systemLocation()] * _autoconnectUpdateTimerMSecs > _autoconnectConnectDelayMSecs) { + SerialConfiguration* pSerialConfig = nullptr; + _autoconnectPortWaitList.remove(portInfo.systemLocation()); + switch (boardType) { + case QGCSerialPortInfo::BoardTypePixhawk: + pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed())); + pSerialConfig->setUsbDirect(true); + break; + case QGCSerialPortInfo::BoardTypePX4Flow: + pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed())); + break; + case QGCSerialPortInfo::BoardTypeSiKRadio: + pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed())); + break; + case QGCSerialPortInfo::BoardTypeOpenPilot: + pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed())); + break; + case QGCSerialPortInfo::BoardTypeRTKGPS: + qCDebug(LinkManagerLog) << "RTK GPS auto-connected" << portInfo.portName().trimmed(); + _autoConnectRTKPort = portInfo.systemLocation(); + _toolbox->gpsManager()->connectGPS(portInfo.systemLocation(), boardName); + break; + default: + qCWarning(LinkManagerLog) << "Internal error: Unknown board type" << boardType; + continue; + } + + if (pSerialConfig) { + qCDebug(LinkManagerLog) << "New auto-connect port added: " << pSerialConfig->name() << portInfo.systemLocation(); + pSerialConfig->setBaud(boardType == QGCSerialPortInfo::BoardTypeSiKRadio ? 57600 : 115200); + pSerialConfig->setDynamic(true); + pSerialConfig->setPortName(portInfo.systemLocation()); + pSerialConfig->setAutoConnect(true); + + SharedLinkConfigurationPtr sharedConfig(pSerialConfig); + createConnectedLink(sharedConfig, boardType == QGCSerialPortInfo::BoardTypePX4Flow); + } + } + } + } + + // Check for RTK GPS connection gone + if (!_autoConnectRTKPort.isEmpty() && !currentPorts.contains(_autoConnectRTKPort)) { + qCDebug(LinkManagerLog) << "RTK GPS disconnected" << _autoConnectRTKPort; + _toolbox->gpsManager()->disconnectGPS(); + _autoConnectRTKPort.clear(); + } +} + bool LinkManager::_allowAutoConnectToBoard(QGCSerialPortInfo::BoardType_t boardType) { switch (boardType) { @@ -934,7 +948,7 @@ void LinkManager::_updateSerialPorts() _commPortDisplayList.clear(); const QList portList = QGCSerialPortInfo::availablePorts(); for (const QGCSerialPortInfo &info: portList) { - const QString port = info.systemLocation().trimmed(); // + " " + info.description(); + const QString port = info.systemLocation().trimmed(); _commPortList += port; _commPortDisplayList += SerialConfiguration::cleanPortDisplayname(port); } @@ -942,7 +956,7 @@ void LinkManager::_updateSerialPorts() QStringList LinkManager::serialPortStrings() { - if (!_commPortDisplayList.size()) { + if (_commPortDisplayList.isEmpty()) { _updateSerialPorts(); } @@ -951,7 +965,7 @@ QStringList LinkManager::serialPortStrings() QStringList LinkManager::serialPorts() { - if (!_commPortList.size()) { + if (_commPortList.isEmpty()) { _updateSerialPorts(); } @@ -963,7 +977,7 @@ QStringList LinkManager::serialBaudRates() return SerialConfiguration::supportedBaudRates(); } -bool LinkManager::_isSerialPortConnected() +bool LinkManager::_isSerialPortConnected() const { for (const SharedLinkInterfacePtr &link: _rgLinks) { if (qobject_cast(link.get())) { diff --git a/src/Comms/LinkManager.h b/src/Comms/LinkManager.h index 4cbc435593c..e357160d4ba 100644 --- a/src/Comms/LinkManager.h +++ b/src/Comms/LinkManager.h @@ -9,93 +9,86 @@ #pragma once +#include +#include +#include + +#include + +#include "QGCToolbox.h" #include "LinkConfiguration.h" #include "LinkInterface.h" -#include "QGCToolbox.h" -#include "QmlObjectListModel.h" -#include "UdpIODevice.h" #ifndef NO_SERIAL_LINK #include "QGCSerialPortInfo.h" #endif -#include -#include -#include -#include - -#include - Q_DECLARE_LOGGING_CATEGORY(LinkManagerLog) Q_DECLARE_LOGGING_CATEGORY(LinkManagerVerboseLog) -class QGCApplication; -class MAVLinkProtocol; -class UDPConfiguration; class AutoConnectSettings; class LogReplayLink; -#ifndef NO_SERIAL_LINK - class SerialLink; -#endif +class MAVLinkProtocol; +class QGCApplication; +class QTimer; +class SerialLink; +class UDPConfiguration; +class UdpIODevice; +class QmlObjectListModel; /// @brief Manage communication links -/// -/// The Link Manager organizes the physical Links. It can manage arbitrary -/// links and takes care of connecting them as well assigning the correct -/// protocol instance to transport the link data into the application. - +/// The Link Manager organizes the physical Links. It can manage arbitrary +/// links and takes care of connecting them as well assigning the correct +/// protocol instance to transport the link data into the application. class LinkManager : public QGCTool { Q_OBJECT + Q_MOC_INCLUDE("QmlObjectListModel.h") + Q_PROPERTY(bool isBluetoothAvailable READ isBluetoothAvailable NOTIFY isBluetoothAvailableChanged) + Q_PROPERTY(QmlObjectListModel *linkConfigurations READ _qmlLinkConfigurations CONSTANT) + Q_PROPERTY(QStringList linkTypeStrings READ linkTypeStrings CONSTANT) + Q_PROPERTY(bool mavlinkSupportForwardingEnabled READ mavlinkSupportForwardingEnabled NOTIFY mavlinkSupportForwardingEnabledChanged) public: - LinkManager(QGCApplication* app, QGCToolbox* toolbox); + LinkManager(QGCApplication *app, QGCToolbox *toolbox); ~LinkManager(); - Q_PROPERTY(bool isBluetoothAvailable READ isBluetoothAvailable NOTIFY isBluetoothAvailableChanged) - Q_PROPERTY(QmlObjectListModel* linkConfigurations READ _qmlLinkConfigurations CONSTANT) - Q_PROPERTY(QStringList linkTypeStrings READ linkTypeStrings CONSTANT) - Q_PROPERTY(bool mavlinkSupportForwardingEnabled READ mavlinkSupportForwardingEnabled NOTIFY mavlinkSupportForwardingEnabledChanged) + // Override from QGCTool + void setToolbox(QGCToolbox *toolbox) final; /// 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 void createMavlinkForwardingSupportLink (void); - - // Called to signal app shutdown. Disconnects all links while turning off auto-connect. - Q_INVOKABLE void shutdown(void); - - Q_INVOKABLE LogReplayLink* startLogReplay(const QString& logFile); - - // Property accessors - - bool isBluetoothAvailable (void); - - QList links (void) { return _rgLinks; } - QStringList linkTypeStrings (void) const; - bool mavlinkSupportForwardingEnabled (void) { return _mavlinkSupportForwardingEnabled; } + 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 void endConfigurationEditing(LinkConfiguration *config, LinkConfiguration *editedConfig); + Q_INVOKABLE void endCreateConfiguration(LinkConfiguration *config); + Q_INVOKABLE void removeConfiguration(LinkConfiguration *config); + Q_INVOKABLE void createMavlinkForwardingSupportLink(); + /// Called to signal app shutdown. Disconnects all links while turning off auto-connect. + Q_INVOKABLE void shutdown(); + Q_INVOKABLE LogReplayLink *startLogReplay(const QString &logFile); + + QList links() { return _rgLinks; } + QStringList linkTypeStrings() const; + bool mavlinkSupportForwardingEnabled() const { return _mavlinkSupportForwardingEnabled; } void loadLinkConfigurationList(); void saveLinkConfigurationList(); /// Suspend automatic confguration updates (during link maintenance for instance) - void suspendConfigurationUpdates(bool suspend); + void suspendConfigurationUpdates(bool suspend) { _configUpdateSuspended = suspend; } /// Sets the flag to suspend the all new connections /// @param reason User visible reason to suspend connections - void setConnectionsSuspended(QString reason); + void setConnectionsSuspended(const QString &reason) { _connectionsSuspended = true; _connectionsSuspendedReason = reason; } /// Sets the flag to allow new connections to be made - void setConnectionsAllowed(void) { _connectionsSuspended = false; } + void setConnectionsAllowed() { _connectionsSuspended = false; } /// Creates, connects (and adds) a link based on the given configuration instance. - bool createConnectedLink(SharedLinkConfigurationPtr& config, bool isPX4Flow = false); + bool createConnectedLink(SharedLinkConfigurationPtr &config, bool isPX4Flow = false); - // This should only be used by Qml code - Q_INVOKABLE void createConnectedLink(LinkConfiguration* config); + /// This should only be used by Qml code + Q_INVOKABLE void createConnectedLink(const LinkConfiguration *config); /// Returns pointer to the mavlink forwarding link, or nullptr if it does not exist SharedLinkInterfacePtr mavlinkForwardingLink(); @@ -106,82 +99,74 @@ class LinkManager : public QGCTool /// Re-initilize the mavlink signing for all links. Used when the signing key changes. void resetMavlinkSigning(); - void disconnectAll(void); - -#ifdef QT_DEBUG - // Only used by unit test tp restart after a shutdown - void restart(void) { setConnectionsAllowed(); } -#endif - - // Override from QGCTool - virtual void setToolbox(QGCToolbox *toolbox); - - static constexpr uint8_t invalidMavlinkChannel(void) { return std::numeric_limits::max(); } + void disconnectAll(); /// Allocates a mavlink channel for use - /// @return Mavlink channel index, invalidMavlinkChannel() for no channels available + /// @return Mavlink channel index, invalidMavlinkChannel() for no channels available uint8_t allocateMavlinkChannel(void); void freeMavlinkChannel(uint8_t channel); /// If you are going to hold a reference to a LinkInterface* in your object you must reference count it /// by using this method to get access to the shared pointer. - SharedLinkInterfacePtr sharedLinkInterfacePointerForLink(LinkInterface* link, bool ignoreNull=false); + SharedLinkInterfacePtr sharedLinkInterfacePointerForLink(LinkInterface *link); - bool containsLink(LinkInterface* link); + bool containsLink(const LinkInterface *link); - static bool isLinkUSBDirect(LinkInterface* link); + SharedLinkConfigurationPtr addConfiguration(LinkConfiguration *config); - SharedLinkConfigurationPtr addConfiguration(LinkConfiguration* config); + void startAutoConnectedLinks(); - void startAutoConnectedLinks(void); + static bool isBluetoothAvailable(); + + static bool isLinkUSBDirect(const LinkInterface *link); + + static constexpr uint8_t invalidMavlinkChannel() { return std::numeric_limits::max(); } signals: void mavlinkSupportForwardingEnabledChanged(); void isBluetoothAvailableChanged(); private slots: - void _linkDisconnected (void); + void _linkDisconnected(); private: - QmlObjectListModel* _qmlLinkConfigurations (void) { return &_qmlConfigurations; } - bool _connectionsSuspendedMsg (void); - void _updateAutoConnectLinks (void); - void _removeConfiguration (LinkConfiguration* config); - void _addUDPAutoConnectLink (void); + QmlObjectListModel *_qmlLinkConfigurations(); + bool _connectionsSuspendedMsg(); + void _updateAutoConnectLinks(); + void _removeConfiguration(const LinkConfiguration *config); + void _addUDPAutoConnectLink(); + void _addMAVLinkForwardingLink(); + void _createDynamicForwardLink(const char *linkName, const QString &hostName); #ifdef QGC_ZEROCONF_ENABLED - void _addZeroConfAutoConnectLink (void); + void _addZeroConfAutoConnectLink(); #endif - void _addMAVLinkForwardingLink (void); - void _createDynamicForwardLink (const char* linkName, QString hostName); - - bool _configUpdateSuspended; ///< true: stop updating configuration list - bool _configurationsLoaded; ///< true: Link configurations have been loaded - bool _connectionsSuspended; ///< true: all new connections should not be allowed - QString _connectionsSuspendedReason; ///< User visible reason for suspension - QTimer _portListTimer; - uint32_t _mavlinkChannelsUsedBitMask; - - AutoConnectSettings* _autoConnectSettings; - MAVLinkProtocol* _mavlinkProtocol; - QList _rgLinks; - QList _rgLinkConfigs; - QmlObjectListModel _qmlConfigurations; + bool _configUpdateSuspended = false; ///< true: stop updating configuration list + bool _configurationsLoaded = false; ///< true: Link configurations have been loaded + bool _connectionsSuspended = false; ///< true: all new connections should not be allowed + bool _mavlinkSupportForwardingEnabled = false; + uint32_t _mavlinkChannelsUsedBitMask = 1; + QString _connectionsSuspendedReason; ///< User visible reason for suspension - UdpIODevice _nmeaSocket; + QTimer *_portListTimer = nullptr; + AutoConnectSettings *_autoConnectSettings = nullptr; + MAVLinkProtocol *_mavlinkProtocol = nullptr; + UdpIODevice *_nmeaSocket = nullptr; + QmlObjectListModel *_qmlConfigurations = nullptr; - bool _mavlinkSupportForwardingEnabled = false; + QList _rgLinks; + QList _rgLinkConfigs; - static constexpr const char* _defaultUDPLinkName = "UDP Link (AutoConnect)"; - static constexpr const char* _mavlinkForwardingLinkName = "MAVLink Forwarding Link"; - static constexpr const char* _mavlinkForwardingSupportLinkName = "MAVLink Support Forwarding Link"; + static constexpr const char* _defaultUDPLinkName = "UDP Link (AutoConnect)"; + static constexpr const char* _mavlinkForwardingLinkName = "MAVLink Forwarding Link"; + static constexpr const char* _mavlinkForwardingSupportLinkName = "MAVLink Support Forwarding Link"; - static constexpr int _autoconnectUpdateTimerMSecs = 1000; + static constexpr int _autoconnectUpdateTimerMSecs = 1000; #ifdef Q_OS_WIN // Have to manually let the bootloader go by on Windows to get a working connect - static constexpr int _autoconnectConnectDelayMSecs = 6000; + static constexpr int _autoconnectConnectDelayMSecs = 6000; #else - static constexpr int _autoconnectConnectDelayMSecs = 1000; + static constexpr int _autoconnectConnectDelayMSecs = 1000; #endif #ifndef NO_SERIAL_LINK @@ -199,9 +184,10 @@ private slots: void commPortsChanged(); private: - bool _isSerialPortConnected(); + bool _isSerialPortConnected() const; void _updateSerialPorts(); bool _allowAutoConnectToBoard(QGCSerialPortInfo::BoardType_t boardType); + void _addSerialAutoConnectLink(); bool _portAlreadyConnected(const QString &portName); QMap _autoconnectPortWaitList; ///< key: QGCSerialPortInfo::systemLocation, value: wait count diff --git a/src/Comms/LogReplayLink.cc b/src/Comms/LogReplayLink.cc index 38f1ffd6a8c..53a86112d1d 100644 --- a/src/Comms/LogReplayLink.cc +++ b/src/Comms/LogReplayLink.cc @@ -15,6 +15,7 @@ #ifndef __mobile__ #include "MAVLinkProtocol.h" #endif +#include "MAVLinkLib.h" #include #include @@ -26,16 +27,16 @@ LogReplayLinkConfiguration::LogReplayLinkConfiguration(const QString& name) } -LogReplayLinkConfiguration::LogReplayLinkConfiguration(LogReplayLinkConfiguration* copy) +LogReplayLinkConfiguration::LogReplayLinkConfiguration(const LogReplayLinkConfiguration* copy) : LinkConfiguration(copy) { _logFilename = copy->logFilename(); } -void LogReplayLinkConfiguration::copyFrom(LinkConfiguration *source) +void LogReplayLinkConfiguration::copyFrom(const LinkConfiguration *source) { LinkConfiguration::copyFrom(source); - auto* ssource = qobject_cast(source); + const LogReplayLinkConfiguration* ssource = qobject_cast(source); if (ssource) { _logFilename = ssource->logFilename(); } else { diff --git a/src/Comms/LogReplayLink.h b/src/Comms/LogReplayLink.h index 353404dbe24..45e1a27fbe4 100644 --- a/src/Comms/LogReplayLink.h +++ b/src/Comms/LogReplayLink.h @@ -11,7 +11,6 @@ #include "LinkConfiguration.h" #include "LinkInterface.h" -#include "QGCMAVLink.h" #include #include @@ -19,6 +18,8 @@ class LinkManager; class MAVLinkProtocol; +typedef struct __mavlink_message mavlink_message_t; + class LogReplayLinkConfiguration : public LinkConfiguration { Q_OBJECT @@ -27,16 +28,16 @@ class LogReplayLinkConfiguration : public LinkConfiguration Q_PROPERTY(QString fileName READ logFilename WRITE setLogFilename NOTIFY fileNameChanged) LogReplayLinkConfiguration(const QString& name); - LogReplayLinkConfiguration(LogReplayLinkConfiguration* copy); + LogReplayLinkConfiguration(const LogReplayLinkConfiguration* copy); - QString logFilename(void) { return _logFilename; } + QString logFilename(void) const { return _logFilename; } void setLogFilename(const QString logFilename) { _logFilename = logFilename; emit fileNameChanged(); } QString logFilenameShort(void); // Virtuals from LinkConfiguration - LinkType type (void) override { return LinkConfiguration::TypeLogReplay; } - void copyFrom (LinkConfiguration* source) override; + LinkType type (void) const override { return LinkConfiguration::TypeLogReplay; } + void copyFrom (const LinkConfiguration* source) override; void loadSettings (QSettings& settings, const QString& root) override; void saveSettings (QSettings& settings, const QString& root) override; QString settingsURL (void) override { return "LogReplaySettings.qml"; } diff --git a/src/Comms/MAVLinkProtocol.cc b/src/Comms/MAVLinkProtocol.cc index 906233700d5..4653333e1cd 100644 --- a/src/Comms/MAVLinkProtocol.cc +++ b/src/Comms/MAVLinkProtocol.cc @@ -190,7 +190,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Since receiveBytes signals cross threads we can end up with signals in the queue // that come through after the link is disconnected. For these we just drop the data // since the link is closed. - SharedLinkInterfacePtr linkPtr = _linkMgr->sharedLinkInterfacePointerForLink(link, true); + SharedLinkInterfacePtr linkPtr = _linkMgr->sharedLinkInterfacePointerForLink(link); if (!linkPtr) { qCDebug(MAVLinkProtocolLog) << "receiveBytes: link gone!" << b.size() << " bytes arrived too late"; return; diff --git a/src/Comms/MockLink/MockLink.cc b/src/Comms/MockLink/MockLink.cc index ee69dd8fb57..48b9010ec38 100644 --- a/src/Comms/MockLink/MockLink.cc +++ b/src/Comms/MockLink/MockLink.cc @@ -1434,7 +1434,7 @@ MockConfiguration::MockConfiguration(const QString& name) } -MockConfiguration::MockConfiguration(MockConfiguration* source) +MockConfiguration::MockConfiguration(const MockConfiguration* source) : LinkConfiguration(source) { _firmwareType = source->_firmwareType; @@ -1444,10 +1444,10 @@ MockConfiguration::MockConfiguration(MockConfiguration* source) _failureMode = source->_failureMode; } -void MockConfiguration::copyFrom(LinkConfiguration *source) +void MockConfiguration::copyFrom(const LinkConfiguration *source) { LinkConfiguration::copyFrom(source); - auto* usource = qobject_cast(source); + const MockConfiguration* usource = qobject_cast(source); if (!usource) { qCWarning(MockLinkLog) << "dynamic_cast failed" << source << usource; diff --git a/src/Comms/MockLink/MockLink.h b/src/Comms/MockLink/MockLink.h index 0618d99e5ae..853caf542f1 100644 --- a/src/Comms/MockLink/MockLink.h +++ b/src/Comms/MockLink/MockLink.h @@ -32,7 +32,7 @@ class MockConfiguration : public LinkConfiguration public: MockConfiguration(const QString& name); - MockConfiguration(MockConfiguration* source); + MockConfiguration(const MockConfiguration* source); Q_PROPERTY(int firmware READ firmware WRITE setFirmware NOTIFY firmwareChanged) Q_PROPERTY(int vehicle READ vehicle WRITE setVehicle NOTIFY vehicleChanged) @@ -72,8 +72,8 @@ class MockConfiguration : public LinkConfiguration void setFailureMode(FailureMode_t failureMode) { _failureMode = failureMode; } // Overrides from LinkConfiguration - LinkType type (void) override { return LinkConfiguration::TypeMock; } - void copyFrom (LinkConfiguration* source) override; + LinkType type (void) const override { return LinkConfiguration::TypeMock; } + void copyFrom (const LinkConfiguration* source) override; void loadSettings (QSettings& settings, const QString& root) override; void saveSettings (QSettings& settings, const QString& root) override; QString settingsURL (void) override { return "MockLinkSettings.qml"; } diff --git a/src/Comms/SerialLink.cc b/src/Comms/SerialLink.cc index 9023767f32f..8af332f7be7 100644 --- a/src/Comms/SerialLink.cc +++ b/src/Comms/SerialLink.cc @@ -9,21 +9,22 @@ #include "SerialLink.h" #include "QGC.h" -#include "QGCApplication.h" #include "QGCLoggingCategory.h" #ifdef Q_OS_ANDROID +#include "QGCApplication.h" #include "LinkManager.h" #include "qserialportinfo.h" #else #include #endif +#include #include QGC_LOGGING_CATEGORY(SerialLinkLog, "SerialLinkLog") SerialLink::SerialLink(SharedLinkConfigurationPtr& config, bool isPX4Flow) : LinkInterface(config, isPX4Flow) - , _serialConfig(qobject_cast(config.get())) + , _serialConfig(qobject_cast(config.get())) { qRegisterMetaType(); qCDebug(SerialLinkLog) << "Create SerialLink portName:baud:flowControl:parity:dataButs:stopBits" << _serialConfig->portName() << _serialConfig->baud() << _serialConfig->flowControl() @@ -293,7 +294,7 @@ SerialConfiguration::SerialConfiguration(const QString& name) : LinkConfiguratio _usbDirect = false; } -SerialConfiguration::SerialConfiguration(SerialConfiguration* copy) : LinkConfiguration(copy) +SerialConfiguration::SerialConfiguration(const SerialConfiguration* copy) : LinkConfiguration(copy) { _baud = copy->baud(); _flowControl = copy->flowControl(); @@ -305,10 +306,10 @@ SerialConfiguration::SerialConfiguration(SerialConfiguration* copy) : LinkConfig _usbDirect = copy->_usbDirect; } -void SerialConfiguration::copyFrom(LinkConfiguration *source) +void SerialConfiguration::copyFrom(const LinkConfiguration *source) { LinkConfiguration::copyFrom(source); - auto* ssource = qobject_cast(source); + const SerialConfiguration* ssource = qobject_cast(source); if (ssource) { _baud = ssource->baud(); _flowControl = ssource->flowControl(); diff --git a/src/Comms/SerialLink.h b/src/Comms/SerialLink.h index 39af036ea2b..b2cd9e1cc1f 100644 --- a/src/Comms/SerialLink.h +++ b/src/Comms/SerialLink.h @@ -37,7 +37,7 @@ class SerialConfiguration : public LinkConfiguration public: SerialConfiguration(const QString& name); - SerialConfiguration(SerialConfiguration* copy); + SerialConfiguration(const SerialConfiguration* copy); Q_PROPERTY(int baud READ baud WRITE setBaud NOTIFY baudChanged) Q_PROPERTY(int dataBits READ dataBits WRITE setDataBits NOTIFY dataBitsChanged) @@ -70,13 +70,13 @@ class SerialConfiguration : public LinkConfiguration static QString cleanPortDisplayname(const QString name); /// From LinkConfiguration - LinkType type () { return LinkConfiguration::TypeSerial; } - void copyFrom (LinkConfiguration* source); - void loadSettings (QSettings& settings, const QString& root); - void saveSettings (QSettings& settings, const QString& root); + LinkType type () const override { return LinkConfiguration::TypeSerial; } + void copyFrom (const LinkConfiguration* source) override; + void loadSettings (QSettings& settings, const QString& root) override; + void saveSettings (QSettings& settings, const QString& root) override; void updateSettings (); - QString settingsURL () { return "SerialSettings.qml"; } - QString settingsTitle () { return tr("Serial Link Settings"); } + QString settingsURL () override { return "SerialSettings.qml"; } + QString settingsTitle () override { return tr("Serial Link Settings"); } signals: void baudChanged (); @@ -140,5 +140,5 @@ private slots: volatile bool _stopp = false; QMutex _stoppMutex; ///< Mutex for accessing _stopp QByteArray _transmitBuffer; ///< An internal buffer for receiving data from member functions and actually transmitting them via the serial port. - SerialConfiguration* _serialConfig = nullptr; + const SerialConfiguration* _serialConfig = nullptr; }; diff --git a/src/Comms/TCPLink.cc b/src/Comms/TCPLink.cc index 3cfbae67cb1..94557c30915 100644 --- a/src/Comms/TCPLink.cc +++ b/src/Comms/TCPLink.cc @@ -16,7 +16,7 @@ TCPLink::TCPLink(SharedLinkConfigurationPtr& config) : LinkInterface(config) - , _tcpConfig(qobject_cast(config.get())) + , _tcpConfig(qobject_cast(config.get())) , _socket(nullptr) , _socketIsConnected(false) { @@ -162,16 +162,16 @@ TCPConfiguration::TCPConfiguration(const QString& name) : LinkConfiguration(name _host = QLatin1String("0.0.0.0"); } -TCPConfiguration::TCPConfiguration(TCPConfiguration* source) : LinkConfiguration(source) +TCPConfiguration::TCPConfiguration(const TCPConfiguration* source) : LinkConfiguration(source) { _port = source->port(); _host = source->host(); } -void TCPConfiguration::copyFrom(LinkConfiguration *source) +void TCPConfiguration::copyFrom(const LinkConfiguration *source) { LinkConfiguration::copyFrom(source); - auto* usource = qobject_cast(source); + const TCPConfiguration* usource = qobject_cast(source); Q_ASSERT(usource != nullptr); _port = usource->port(); _host = usource->host(); diff --git a/src/Comms/TCPLink.h b/src/Comms/TCPLink.h index da00b83790a..726bf632671 100644 --- a/src/Comms/TCPLink.h +++ b/src/Comms/TCPLink.h @@ -35,7 +35,7 @@ class TCPConfiguration : public LinkConfiguration Q_PROPERTY(QString host READ host WRITE setHost NOTIFY hostChanged) TCPConfiguration(const QString& name); - TCPConfiguration(TCPConfiguration* source); + TCPConfiguration(const TCPConfiguration* source); quint16 port (void) const { return _port; } QString host (void) const { return _host; } @@ -43,8 +43,8 @@ class TCPConfiguration : public LinkConfiguration void setHost (const QString host); //LinkConfiguration overrides - LinkType type (void) override { return LinkConfiguration::TypeTcp; } - void copyFrom (LinkConfiguration* source) override; + LinkType type (void) const override { return LinkConfiguration::TypeTcp; } + void copyFrom (const LinkConfiguration* source) override; void loadSettings (QSettings& settings, const QString& root) override; void saveSettings (QSettings& settings, const QString& root) override; QString settingsURL (void) override { return "TcpSettings.qml"; } @@ -91,7 +91,7 @@ private slots: void _writeDebugBytes (const QByteArray data); #endif - TCPConfiguration* _tcpConfig; + const TCPConfiguration* _tcpConfig; QTcpSocket* _socket; bool _socketIsConnected; diff --git a/src/Comms/UDPLink.cc b/src/Comms/UDPLink.cc index 1f195c7a626..ec92d15cad2 100644 --- a/src/Comms/UDPLink.cc +++ b/src/Comms/UDPLink.cc @@ -64,7 +64,7 @@ UDPLink::UDPLink(SharedLinkConfigurationPtr& config) : LinkInterface (config) , _running (false) , _socket (nullptr) - , _udpConfig (qobject_cast(config.get())) + , _udpConfig (qobject_cast(config.get())) , _connectState (false) #if defined(QGC_ZEROCONF_ENABLED) , _dnssServiceRef (nullptr) @@ -323,7 +323,7 @@ UDPConfiguration::UDPConfiguration(const QString& name) : LinkConfiguration(name } } -UDPConfiguration::UDPConfiguration(UDPConfiguration* source) : LinkConfiguration(source) +UDPConfiguration::UDPConfiguration(const UDPConfiguration* source) : LinkConfiguration(source) { _copyFrom(source); } @@ -333,15 +333,15 @@ UDPConfiguration::~UDPConfiguration() _clearTargetHosts(); } -void UDPConfiguration::copyFrom(LinkConfiguration *source) +void UDPConfiguration::copyFrom(const LinkConfiguration *source) { LinkConfiguration::copyFrom(source); _copyFrom(source); } -void UDPConfiguration::_copyFrom(LinkConfiguration *source) +void UDPConfiguration::_copyFrom(const LinkConfiguration *source) { - auto* usource = qobject_cast(source); + const UDPConfiguration* usource = qobject_cast(source); if (usource) { _localPort = usource->localPort(); _clearTargetHosts(); diff --git a/src/Comms/UDPLink.h b/src/Comms/UDPLink.h index 71926cc5f13..0c151a09a9e 100644 --- a/src/Comms/UDPLink.h +++ b/src/Comms/UDPLink.h @@ -48,7 +48,7 @@ class UDPConfiguration : public LinkConfiguration Q_PROPERTY(QStringList hostList READ hostList NOTIFY hostListChanged) UDPConfiguration(const QString& name); - UDPConfiguration(UDPConfiguration* source); + UDPConfiguration(const UDPConfiguration* source); ~UDPConfiguration(); quint16 localPort () const{ return _localPort; } @@ -64,12 +64,12 @@ class UDPConfiguration : public LinkConfiguration Q_INVOKABLE void removeHost (const QString host); void setLocalPort(quint16 port); - QStringList hostList (void) { return _hostList; } - const QList targetHosts (void) { return _targetHosts; } + QStringList hostList (void) const { return _hostList; } + const QList targetHosts (void) const { return _targetHosts; } /// LinkConfiguration overrides - LinkType type (void) override { return LinkConfiguration::TypeUdp; } - void copyFrom (LinkConfiguration* source) override; + LinkType type (void) const override { return LinkConfiguration::TypeUdp; } + void copyFrom (const LinkConfiguration* source) override; void loadSettings (QSettings& settings, const QString& root) override; void saveSettings (QSettings& settings, const QString& root) override; QString settingsURL (void) override { return "UdpSettings.qml"; } @@ -82,7 +82,7 @@ class UDPConfiguration : public LinkConfiguration private: void _updateHostList (void); void _clearTargetHosts (void); - void _copyFrom (LinkConfiguration *source); + void _copyFrom (const LinkConfiguration *source); QList _targetHosts; QStringList _hostList; @@ -125,7 +125,7 @@ private slots: bool _running; QUdpSocket* _socket; - UDPConfiguration* _udpConfig; + const UDPConfiguration* _udpConfig; bool _connectState; QList _sessionTargets; QMutex _sessionTargetsMutex; diff --git a/src/QmlControls/QmlObjectListModel.h b/src/QmlControls/QmlObjectListModel.h index 0c932da9703..6c8b93c2ef4 100644 --- a/src/QmlControls/QmlObjectListModel.h +++ b/src/QmlControls/QmlObjectListModel.h @@ -40,11 +40,11 @@ class QmlObjectListModel : public QAbstractListModel QObjectList swapObjectList (const QObjectList& newlist); void clear (); QObject* removeAt (int i); - QObject* removeOne (QObject* object) { return removeAt(indexOf(object)); } + QObject* removeOne (const QObject* object) { return removeAt(indexOf(object)); } void insert (int i, QObject* object); void insert (int i, QList objects); - bool contains (QObject* object) { return _objectList.indexOf(object) != -1; } - int indexOf (QObject* object) { return _objectList.indexOf(object); } + bool contains (const QObject* object) { return _objectList.indexOf(object) != -1; } + int indexOf (const QObject* object) { return _objectList.indexOf(object); } /// Moves an item to a new position void move(int from, int to); diff --git a/test/qgcunittest/UnitTest.cc b/test/qgcunittest/UnitTest.cc index 52b923b8906..9b230a9444f 100644 --- a/test/qgcunittest/UnitTest.cc +++ b/test/qgcunittest/UnitTest.cc @@ -96,7 +96,7 @@ void UnitTest::init(void) _linkManager = qgcApp()->toolbox()->linkManager(); } - _linkManager->restart(); + _linkManager->setConnectionsAllowed(); // Force offline vehicle back to defaults AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings();