diff --git a/src/Comms/LinkManager.cc b/src/Comms/LinkManager.cc index 8375ff45cb1a..306f33f96de0 100644 --- a/src/Comms/LinkManager.cc +++ b/src/Comms/LinkManager.cc @@ -107,6 +107,18 @@ void LinkManager::createConnectedLink(const LinkConfiguration *config) bool LinkManager::createConnectedLink(SharedLinkConfigurationPtr &config) { +#ifndef QGC_NO_SERIAL_LINK + // If we're creating a serial link, disconnect any existing autoconnect link on the same port first. + // This prevents "Access denied" errors on Windows when the user manually connects to a port + // that autoconnect has already opened. + if (config->type() == LinkConfiguration::TypeSerial) { + const SerialConfiguration *newSerialConfig = qobject_cast(config.get()); + if (newSerialConfig && !config->isAutoConnect()) { + _disconnectAutoConnectLink(newSerialConfig->portName()); + } + } +#endif + SharedLinkInterfacePtr link = nullptr; switch(config->type()) { @@ -915,6 +927,38 @@ void LinkManager::_addSerialAutoConnectLink() continue; } if (_portAlreadyConnected(portInfo.systemLocation()) || (_autoConnectRTKPort == portInfo.systemLocation())) { + // Check for zombie autoconnect links: port is open but no MAVLink data received + if (_autoConnectRTKPort != portInfo.systemLocation()) { + SharedLinkInterfacePtr existingLink; + { + QMutexLocker locker(&_linksMutex); + const QString searchPort = portInfo.systemLocation().trimmed(); + for (const SharedLinkInterfacePtr &linkInterface : _rgLinks) { + const SharedLinkConfigurationPtr linkConfig = linkInterface->linkConfiguration(); + if (linkConfig && linkConfig->isAutoConnect()) { + const SerialConfiguration *serialConfig = qobject_cast(linkConfig.get()); + if (serialConfig && (serialConfig->portName() == searchPort)) { + existingLink = linkInterface; + break; + } + } + } + } + + if (existingLink && !existingLink->decodedFirstMavlinkPacket()) { + _autoconnectNoMavlinkCount[portInfo.systemLocation()] += _autoconnectUpdateTimerMSecs; + if (_autoconnectNoMavlinkCount[portInfo.systemLocation()] > _autoconnectNoMavlinkTimeoutMSecs) { + qCWarning(LinkManagerLog) << "Autoconnect link on" << portInfo.systemLocation() + << "has not received MAVLink data - disconnecting to retry"; + _autoconnectNoMavlinkCount.remove(portInfo.systemLocation()); + existingLink->disconnect(); + continue; + } + } else { + // Link is communicating, clear any timeout counter + _autoconnectNoMavlinkCount.remove(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 @@ -999,6 +1043,32 @@ bool LinkManager::_allowAutoConnectToBoard(QGCSerialPortInfo::BoardType_t boardT return false; } +void LinkManager::_disconnectAutoConnectLink(const QString &portName) +{ + const QString searchPort = portName.trimmed(); + SharedLinkInterfacePtr linkToDisconnect; + + { + QMutexLocker locker(&_linksMutex); + for (const SharedLinkInterfacePtr &linkInterface : _rgLinks) { + const SharedLinkConfigurationPtr linkConfig = linkInterface->linkConfiguration(); + if (!linkConfig || !linkConfig->isAutoConnect()) { + continue; + } + const SerialConfiguration *serialConfig = qobject_cast(linkConfig.get()); + if (serialConfig && (serialConfig->portName() == searchPort)) { + linkToDisconnect = linkInterface; + break; + } + } + } + + if (linkToDisconnect) { + qCDebug(LinkManagerLog) << "Disconnecting existing autoconnect link on port" << searchPort << "to allow manual connection"; + linkToDisconnect->disconnect(); + } +} + bool LinkManager::_portAlreadyConnected(const QString &portName) { QMutexLocker locker(&_linksMutex); @@ -1008,6 +1078,13 @@ bool LinkManager::_portAlreadyConnected(const QString &portName) const SharedLinkConfigurationPtr linkConfig = linkInterface->linkConfiguration(); const SerialConfiguration* const serialConfig = qobject_cast(linkConfig.get()); if (serialConfig && (serialConfig->portName() == searchPort)) { + // Also verify the link is actually connected. The serial port open happens + // asynchronously, so the link may be in _rgLinks but not truly connected yet + // (or the connection may have failed). If not connected, allow autoconnect to retry. + if (!linkInterface->isConnected()) { + qCDebug(LinkManagerLog) << "Port in link list but not connected, allowing retry:" << searchPort; + return false; + } return true; } } diff --git a/src/Comms/LinkManager.h b/src/Comms/LinkManager.h index 964b1ecfeb7d..ce9c5c083ba1 100644 --- a/src/Comms/LinkManager.h +++ b/src/Comms/LinkManager.h @@ -161,6 +161,8 @@ private slots: #else static constexpr int _autoconnectConnectDelayMSecs = 1000; #endif + // If an autoconnect serial link hasn't received any MAVLink data within this time, disconnect and retry + static constexpr int _autoconnectNoMavlinkTimeoutMSecs = 10000; #ifndef QGC_NO_SERIAL_LINK private: @@ -183,10 +185,12 @@ private slots: bool _allowAutoConnectToBoard(QGCSerialPortInfo::BoardType_t boardType) const; void _addSerialAutoConnectLink(); bool _portAlreadyConnected(const QString &portName); + void _disconnectAutoConnectLink(const QString &portName); void _filterCompositePorts(QList &portList); UdpIODevice *_nmeaSocket = nullptr; QMap _autoconnectPortWaitList; ///< key: QGCSerialPortInfo::systemLocation, value: wait count + QMap _autoconnectNoMavlinkCount; ///< key: port systemLocation, value: seconds since autoconnect with no MAVLink data QList _activeLinkCheckList; ///< List of links we are waiting for a vehicle to show up on QStringList _commPortList; QStringList _commPortDisplayList; diff --git a/src/Comms/MAVLinkProtocol.cc b/src/Comms/MAVLinkProtocol.cc index f74b70cfae72..372e401d86c0 100644 --- a/src/Comms/MAVLinkProtocol.cc +++ b/src/Comms/MAVLinkProtocol.cc @@ -102,6 +102,10 @@ void MAVLinkProtocol::receiveBytes(LinkInterface *link, const QByteArray &data) continue; } + if (!link->decodedFirstMavlinkPacket()) { + link->setDecodedFirstMavlinkPacket(true); + } + // It's ok to get v1 HEARTBEAT messages on a v2 link: // PX4 defaults to sending V1 then switches to V2 after receiving a V2 message from GCS // ArduPilot always sends both versions