diff --git a/src/FactSystem/FactSystemTestBase.cc b/src/FactSystem/FactSystemTestBase.cc index d8bf606b56e682aa3b5829760936adc6d1989b0f..d50a60cc6ea47909438de0ccbb47706898e28e50 100644 --- a/src/FactSystem/FactSystemTestBase.cc +++ b/src/FactSystem/FactSystemTestBase.cc @@ -48,7 +48,7 @@ void FactSystemTestBase::_init(MAV_AUTOPILOT autopilot) MockLink* link = new MockLink(); link->setAutopilotType(autopilot); - _linkMgr->addLink(link); + _linkMgr->_addLink(link); _linkMgr->connectLink(link); // Wait for the uas to work it's way through the various threads diff --git a/src/VehicleSetup/SetupViewButtons.qml b/src/VehicleSetup/SetupViewButtons.qml index c810ff38c08bc219c9b7c1183c8fb95a6e0d1467..c604a70cc7b8535d14f6763966dae6a6f47adbda 100644 --- a/src/VehicleSetup/SetupViewButtons.qml +++ b/src/VehicleSetup/SetupViewButtons.qml @@ -35,7 +35,7 @@ Rectangle { QGCLabel { width: parent.width - text: "You must be connected to your board to use all available setup options." + text: "Full setup options are only available when connected to vehicle and full parameter list has completed downloading." wrapMode: Text.WordWrap horizontalAlignment: Text.AlignHCenter } diff --git a/src/VehicleSetup/SetupViewTest.cc b/src/VehicleSetup/SetupViewTest.cc index 113072edbdd36ba75e46bc857618823651398aa3..7a1ef75c24b5db8f66c4bab2e37fd2012431cdee 100644 --- a/src/VehicleSetup/SetupViewTest.cc +++ b/src/VehicleSetup/SetupViewTest.cc @@ -64,7 +64,7 @@ void SetupViewTest::_clickThrough_test(void) MockLink* link = new MockLink(); Q_CHECK_PTR(link); link->setAutopilotType(MAV_AUTOPILOT_PX4); - LinkManager::instance()->addLink(link); + LinkManager::instance()->_addLink(link); linkMgr->connectLink(link); QTest::qWait(5000); // Give enough time for UI to settle and heartbeats to go through diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index ac0cd704ea1b373e8251051c25593150c9d7f04a..a4722d2b9dc1a26217d86e35b4431a5ef082b928 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -37,6 +37,7 @@ along with PIXHAWK. If not, see . #include #include #include +#include class LinkManager; class LinkConfiguration; @@ -50,38 +51,10 @@ class LinkInterface : public QThread { Q_OBJECT - // Only LinkManager is allowed to _connect, _disconnect or delete a link + // Only LinkManager is allowed to create/delete or _connect/_disconnect a link friend class LinkManager; public: - LinkInterface() : - QThread(0), - _ownedByLinkManager(false), - _deletedByLinkManager(false), - _flaggedForDeletion(false) - { - // Initialize everything for the data rate calculation buffers. - inDataIndex = 0; - outDataIndex = 0; - - // Initialize our data rate buffers. - memset(inDataWriteAmounts, 0, sizeof(inDataWriteAmounts)); - memset(inDataWriteTimes, 0, sizeof(inDataWriteTimes)); - memset(outDataWriteAmounts,0, sizeof(outDataWriteAmounts)); - memset(outDataWriteTimes, 0, sizeof(outDataWriteTimes)); - - qRegisterMetaType("LinkInterface*"); - } - - /** - * @brief Destructor - * LinkManager take ownership of Links once they are added to it. Once added to LinkManager - * use LinkManager::deleteLink to remove if necessary. - **/ - virtual ~LinkInterface() { - Q_ASSERT(!_ownedByLinkManager || _deletedByLinkManager); - } - /** * @brief Get link configuration (if used) * @return A pointer to the instance of LinkConfiguration if supported. NULL otherwise. @@ -169,7 +142,25 @@ public slots: * @param length The length of the data array **/ virtual void writeBytes(const char *bytes, qint64 length) = 0; - + +protected: + // Links are only created by LinkManager so constructor is not public + LinkInterface() : + QThread(0) + { + // Initialize everything for the data rate calculation buffers. + inDataIndex = 0; + outDataIndex = 0; + + // Initialize our data rate buffers. + memset(inDataWriteAmounts, 0, sizeof(inDataWriteAmounts)); + memset(inDataWriteTimes, 0, sizeof(inDataWriteTimes)); + memset(outDataWriteAmounts,0, sizeof(outDataWriteAmounts)); + memset(outDataWriteTimes, 0, sizeof(outDataWriteTimes)); + + qRegisterMetaType("LinkInterface*"); + } + signals: /** @@ -338,10 +329,8 @@ private: * @return True if connection could be terminated, false otherwise **/ virtual bool _disconnect(void) = 0; - - bool _ownedByLinkManager; ///< true: This link has been added to LinkManager, false: Link not added to LinkManager - bool _deletedByLinkManager; ///< true: Link being deleted from LinkManager, false: error, Links should only be deleted from LinkManager - bool _flaggedForDeletion; ///< true: Garbage colletion ready }; +typedef QSharedPointer SharedLinkInterface; + #endif // _LINKINTERFACE_H_ diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index c2e906316852818bc621ff3fbae7c38f71d85038..694f092ab0fabb6c70767e3821d0944b133e4482 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -52,6 +52,7 @@ LinkManager::LinkManager(QObject* parent) , _configUpdateSuspended(false) , _configurationsLoaded(false) , _connectionsSuspended(false) + , _nullSharedLink(NULL) { connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateConfigurationList); _portListTimer.start(1000); @@ -68,7 +69,7 @@ LinkManager::~LinkManager() Q_ASSERT_X(_links.count() == 0, "LinkManager", "LinkManager::_shutdown should have been called previously"); } -LinkInterface* LinkManager::createLink(LinkConfiguration* config) +LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config) { Q_ASSERT(config); LinkInterface* pLink = NULL; @@ -89,33 +90,31 @@ LinkInterface* LinkManager::createLink(LinkConfiguration* config) #endif } if(pLink) { - addLink(pLink); + _addLink(pLink); + connectLink(pLink); } return pLink; } -LinkInterface* LinkManager::createLink(const QString& name) +LinkInterface* LinkManager::createConnectedLink(const QString& name) { Q_ASSERT(name.isEmpty() == false); for(int i = 0; i < _linkConfigurations.count(); i++) { LinkConfiguration* conf = _linkConfigurations.at(i); if(conf && conf->name() == name) - return createLink(conf); + return createConnectedLink(conf); } return NULL; } -void LinkManager::addLink(LinkInterface* link) +void LinkManager::_addLink(LinkInterface* link) { Q_ASSERT(link); - // Take ownership for delete - link->_ownedByLinkManager = true; - _linkListMutex.lock(); - if (!_links.contains(link)) { - _links.append(link); + if (!containsLink(link)) { + _links.append(QSharedPointer(link)); _linkListMutex.unlock(); emit newLink(link); } else { @@ -145,14 +144,12 @@ bool LinkManager::connectAll() bool allConnected = true; - _linkListMutex.lock(); - foreach (LinkInterface* link, _links) { - Q_ASSERT(link); - if (!link->_connect()) { + foreach (SharedLinkInterface sharedLink, _links) { + Q_ASSERT(sharedLink.data()); + if (!sharedLink.data()->_connect()) { allConnected = false; } } - _linkListMutex.unlock(); return allConnected; } @@ -161,15 +158,12 @@ bool LinkManager::disconnectAll() { bool allDisconnected = true; - _linkListMutex.lock(); - foreach (LinkInterface* link, _links) - { - Q_ASSERT(link); - if (!link->_disconnect()) { + foreach (SharedLinkInterface sharedLink, _links) { + Q_ASSERT(sharedLink.data()); + if (!sharedLink.data()->_disconnect()) { allDisconnected = false; } } - _linkListMutex.unlock(); return allDisconnected; } @@ -197,51 +191,34 @@ bool LinkManager::disconnectLink(LinkInterface* link) if(config) { config->setLink(NULL); } - // Link is now done and over with. We can't yet delete it because it - // takes a while for the MAVLink protocol to take notice of it. We - // flag it for delayed deletion for final clean up. - link->_flaggedForDeletion = true; - QTimer::singleShot(1000, this, &LinkManager::_delayedDeleteLink); + _deleteLink(link); return true; } else { return false; } } -void LinkManager::_delayedDeleteLink() -{ - _linkListMutex.lock(); - foreach (LinkInterface* link, _links) - { - Q_ASSERT(link); - if (link->_flaggedForDeletion) { - qDebug() << "Link deleted: " << link->getName(); - _linkListMutex.unlock(); - deleteLink(link); - return; - } - } - _linkListMutex.unlock(); -} - -void LinkManager::deleteLink(LinkInterface* link) +void LinkManager::_deleteLink(LinkInterface* link) { Q_ASSERT(link); _linkListMutex.lock(); - Q_ASSERT(_links.contains(link)); - _links.removeOne(link); - Q_ASSERT(!_links.contains(link)); + bool found = false; + for (int i=0; i<_links.count(); i++) { + if (_links[i].data() == link) { + _links.removeAt(i); + found = true; + break; + } + } + Q_UNUSED(found); + Q_ASSERT(found); _linkListMutex.unlock(); // Emit removal of link emit linkDeleted(link); - - Q_ASSERT(link->_ownedByLinkManager); - link->_deletedByLinkManager = true; // Signal that this is a valid delete - delete link; } /** @@ -249,29 +226,13 @@ void LinkManager::deleteLink(LinkInterface* link) */ const QList LinkManager::getLinks() { - _linkListMutex.lock(); - QList ret(_links); - _linkListMutex.unlock(); - return ret; -} - -const QList LinkManager::getSerialLinks() -{ - _linkListMutex.lock(); - QList s; - - foreach (LinkInterface* link, _links) - { - Q_ASSERT(link); - - SerialLink* serialLink = qobject_cast(link); - - if (serialLink) - s.append(serialLink); + QList list; + + foreach (SharedLinkInterface sharedLink, _links) { + list << sharedLink.data(); } - _linkListMutex.unlock(); - - return s; + + return list; } /// @brief If all new connections should be suspended a message is displayed to the user and true @@ -296,10 +257,8 @@ void LinkManager::setConnectionsSuspended(QString reason) void LinkManager::_shutdown(void) { - QList links = _links; - foreach(LinkInterface* link, links) { - disconnectLink(link); - deleteLink(link); + while (_links.count() != 0) { + disconnectLink(_links[0].data()); } } @@ -489,3 +448,43 @@ void LinkManager::_updateConfigurationList(void) } } +bool LinkManager::containsLink(LinkInterface* link) +{ + bool found = false; + + foreach (SharedLinkInterface sharedLink, _links) { + if (sharedLink.data() == link) { + found = true; + break; + } + } + + return found; +} + +bool LinkManager::anyConnectedLinks(void) +{ + bool found = false; + + foreach (SharedLinkInterface sharedLink, _links) { + if (sharedLink.data()->isConnected()) { + found = true; + break; + } + } + + return found; +} + +SharedLinkInterface& LinkManager::sharedPointerForLink(LinkInterface* link) +{ + for (int i=0; i<_links.count(); i++) { + if (_links[i].data() == link) { + return _links[i]; + } + } + + // This should never happen + Q_ASSERT(false); + return _nullSharedLink; +} diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 1553f07a6bef5c3e1e6376c3cdfd463310d89516..3d438225a8eb5272edc5cbf0dcd954a1884a760e 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -103,21 +103,18 @@ public: /// Sets the flag to allow new connections to be made void setConnectionsAllowed(void) { _connectionsSuspended = false; } - /// Creates (and adds) a link based on the given configuration instance. LinkManager takes ownership of this object. To delete - /// it, call LinkManager::deleteLink. - LinkInterface* createLink(LinkConfiguration* config); + /// Creates, connects (and adds) a link based on the given configuration instance. + LinkInterface* createConnectedLink(LinkConfiguration* config); - /// Creates (and adds) a link based on the given configuration name. LinkManager takes ownership of this object. To delete - /// it, call LinkManager::deleteLink. - LinkInterface* createLink(const QString& name); + /// Creates, connects (and adds) a link based on the given configuration name. + LinkInterface* createConnectedLink(const QString& name); - /// Adds the link to the LinkManager. LinkManager takes ownership of this object. To delete - /// it, call LinkManager::deleteLink. - void addLink(LinkInterface* link); - - /// Deletes the specified link. Will disconnect if connected. - // TODO Will also crash if called. MAVLink protocol is not handling the disconnect properly. - void deleteLink(LinkInterface* link); + /// Returns true if the link manager is holding this link + bool containsLink(LinkInterface* link); + + /// Returns the QSharedPointer for this link. You must use SharedLinkInterface if you are going to + /// keep references to a link in a thread other than the main ui thread. + SharedLinkInterface& sharedPointerForLink(LinkInterface* link); /// Re-connects all existing links bool connectAll(); @@ -130,6 +127,14 @@ public: /// Disconnect the specified link bool disconnectLink(LinkInterface* link); + + /// Returns true if there are any connected links + bool anyConnectedLinks(void); + + // The following APIs are public but should not be called in normal use. The are mainly exposed + // here for unit test code. + void _deleteLink(LinkInterface* link); + void _addLink(LinkInterface* link); signals: void newLink(LinkInterface* link); @@ -141,13 +146,12 @@ signals: private slots: void _linkConnected(void); void _linkDisconnected(void); - void _delayedDeleteLink(); private: /// All access to LinkManager is through LinkManager::instance LinkManager(QObject* parent = NULL); ~LinkManager(); - + virtual void _shutdown(void); bool _connectionsSuspendedMsg(void); @@ -155,7 +159,12 @@ private: SerialConfiguration* _findSerialConfiguration(const QString& portName); QList _linkConfigurations; ///< List of configured links - QList _links; ///< List of available links + + /// List of available links kept as QSharedPointers. We use QSharedPointer since + /// there are other objects that maintain copies of these links in other threads. + /// The reference counting allows for orderly deletion. + QList _links; + QMutex _linkListMutex; ///< Mutex for thread safe access to _links list bool _configUpdateSuspended; ///< true: stop updating configuration list @@ -163,6 +172,8 @@ private: bool _connectionsSuspended; ///< true: all new connections should not be allowed QString _connectionsSuspendedReason; ///< User visible reason for suspension QTimer _portListTimer; + + SharedLinkInterface _nullSharedLink; }; #endif diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 3986d027719cdb1ebfa77c6bf480d3c8b63779ab..5d76b046b56d9c69bc673022aa6e6c0a7f0fa051 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -175,8 +175,12 @@ void MAVLinkProtocol::_linkStatusChanged(LinkInterface* link, bool connected) Q_ASSERT(link); if (connected) { - Q_ASSERT(!_connectedLinks.contains(link)); - _connectedLinks.append(link); + foreach (SharedLinkInterface sharedLink, _connectedLinks) { + Q_ASSERT(sharedLink.data() != link); + } + + // Use the same shared pointer as LinkManager + _connectedLinks.append(LinkManager::instance()->sharedPointerForLink(link)); if (_connectedLinks.count() == 1) { // This is the first link, we need to start logging @@ -192,8 +196,16 @@ void MAVLinkProtocol::_linkStatusChanged(LinkInterface* link, bool connected) link->writeBytes(cmd, strlen(cmd)); link->writeBytes(init, 4); } else { - Q_ASSERT(_connectedLinks.contains(link)); - _connectedLinks.removeOne(link); + bool found = false; + for (int i=0; i<_connectedLinks.count(); i++) { + if (_connectedLinks[i].data() == link) { + found = true; + _connectedLinks.removeAt(i); + break; + } + } + Q_UNUSED(found); + Q_ASSERT(found); if (_connectedLinks.count() == 0) { // Last link is gone, close out logging diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index efdd4e4a553905dc6f6b04c62445c29881aaf14f..8d94f32529cbee44bc8b2e108117e39c980c497a 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -285,7 +285,10 @@ private: void _startLogging(void); void _stopLogging(void); - QList _connectedLinks; ///< List of all links connected to protocol + /// List of all links connected to protocol. We keep SharedLinkInterface objects + /// which are QSharedPointer's in order to maintain reference counts across threads. + /// This way Link deletion works correctly. + QList _connectedLinks; bool _logSuspendError; ///< true: Logging suspended due to error bool _logSuspendReplay; ///< true: Logging suspended due to replay diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 0403823767f4cfbc1ca35f8d94d9e44757381ddd..1508c161aac2399737189ac9d18c203f97fab496 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -106,7 +106,7 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile srand(QTime::currentTime().msec()); maxTimeNoise = 0; this->id = getNextLinkId(); - LinkManager::instance()->addLink(this); + LinkManager::instance()->_addLink(this); } MAVLinkSimulationLink::~MAVLinkSimulationLink() diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index 181c5cfa912fee1f52fa1871b2ff8adbf5bf8274..4b6400addb9062c0c3ab80cb662f8c384cd66889 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -102,12 +102,11 @@ private: class SerialLink : public LinkInterface { Q_OBJECT + friend class SerialConfiguration; + friend class LinkManager; + public: - - SerialLink(SerialConfiguration* config); - ~SerialLink(); - // LinkInterface LinkConfiguration* getLinkConfiguration(); @@ -154,9 +153,13 @@ private slots: void _rerouteDisconnected(void); private: + // Links are only created/destroyed by LinkManager so constructor/destructor is not public + SerialLink(SerialConfiguration* config); + ~SerialLink(); + // From LinkInterface - bool _connect(void); - bool _disconnect(void); + virtual bool _connect(void); + virtual bool _disconnect(void); // Internal methods void _emitLinkError(const QString& errorMsg); diff --git a/src/comm/TCPLink.h b/src/comm/TCPLink.h index 36ce8815addbd509d39518ce7623a487ef439791..c0140e1e6481ec3bd23e43a1b33b93c3a1439a18 100644 --- a/src/comm/TCPLink.h +++ b/src/comm/TCPLink.h @@ -115,12 +115,12 @@ private: class TCPLink : public LinkInterface { Q_OBJECT + friend class TCPLinkUnitTest; friend class TCPConfiguration; -public: - TCPLink(TCPConfiguration* config); - ~TCPLink(); + friend class LinkManager; +public: QTcpSocket* getSocket(void) { return _socket; } void signalBytesWritten(void); @@ -159,9 +159,13 @@ protected: virtual void run(void); private: + // Links are only created/destroyed by LinkManager so constructor/destructor is not public + TCPLink(TCPConfiguration* config); + ~TCPLink(); + // From LinkInterface - bool _connect(void); - bool _disconnect(void); + virtual bool _connect(void); + virtual bool _disconnect(void); bool _hardwareConnect(); void _restartConnection(); diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index 578de01c121e15f3ac0d97d80334597265d4245a..1d6868745dd7c2ebc14400c100a19a378bb0f9fb 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -143,11 +143,11 @@ private: class UDPLink : public LinkInterface { Q_OBJECT + friend class UDPConfiguration; + friend class LinkManager; + public: - UDPLink(UDPConfiguration* config); - ~UDPLink(); - void requestReset() { } bool isConnected() const; QString getName() const; @@ -192,6 +192,10 @@ protected: int _id; private: + // Links are only created/destroyed by LinkManager so constructor/destructor is not public + UDPLink(UDPConfiguration* config); + ~UDPLink(); + // From LinkInterface virtual bool _connect(void); virtual bool _disconnect(void); diff --git a/src/qgcunittest/LinkManagerTest.cc b/src/qgcunittest/LinkManagerTest.cc index f613683dd719d74b1934a5a073519d12af4eb347..67d143b582271a61c43f3b2da5833a9841f91223 100644 --- a/src/qgcunittest/LinkManagerTest.cc +++ b/src/qgcunittest/LinkManagerTest.cc @@ -78,7 +78,7 @@ void LinkManagerTest::_add_test(void) Q_ASSERT(_linkMgr->getLinks().count() == 0); MockLink* link = new MockLink(); - _linkMgr->addLink(link); + _linkMgr->_addLink(link); QList links = _linkMgr->getLinks(); QCOMPARE(links.count(), 1); @@ -91,8 +91,8 @@ void LinkManagerTest::_delete_test(void) Q_ASSERT(_linkMgr->getLinks().count() == 0); MockLink* link = new MockLink(); - _linkMgr->addLink(link); - _linkMgr->deleteLink(link); + _linkMgr->_addLink(link); + _linkMgr->_deleteLink(link); QCOMPARE(_linkMgr->getLinks().count(), 0); } @@ -104,7 +104,7 @@ void LinkManagerTest::_addSignals_test(void) Q_ASSERT(_multiSpy->checkNoSignals() == true); MockLink* link = new MockLink(); - _linkMgr->addLink(link); + _linkMgr->_addLink(link); QCOMPARE(_multiSpy->checkOnlySignalByMask(newLinkSignalMask), true); QSignalSpy* spy = _multiSpy->getSpyByIndex(newLinkSignalIndex); @@ -125,10 +125,10 @@ void LinkManagerTest::_deleteSignals_test(void) Q_ASSERT(_multiSpy->checkNoSignals() == true); MockLink* link = new MockLink(); - _linkMgr->addLink(link); + _linkMgr->_addLink(link); _multiSpy->clearAllSignals(); - _linkMgr->deleteLink(link); + _linkMgr->_deleteLink(link); QCOMPARE(_multiSpy->checkOnlySignalByMask(linkDeletedSignalMask), true); QSignalSpy* spy = _multiSpy->getSpyByIndex(linkDeletedSignalIndex); diff --git a/src/qgcunittest/MainWindowTest.cc b/src/qgcunittest/MainWindowTest.cc index 2b007bb6735e6615d4725cab1ae6bc592c213bb4..aa7a54cfbdfb757f425b5e9b37b6c7520f68d30d 100644 --- a/src/qgcunittest/MainWindowTest.cc +++ b/src/qgcunittest/MainWindowTest.cc @@ -67,7 +67,7 @@ void MainWindowTest::_connectWindowClose_test(MAV_AUTOPILOT autopilot) MockLink* link = new MockLink(); Q_CHECK_PTR(link); link->setAutopilotType(autopilot); - LinkManager::instance()->addLink(link); + LinkManager::instance()->_addLink(link); linkMgr->connectLink(link); QTest::qWait(5000); // Give enough time for UI to settle and heartbeats to go through diff --git a/src/qgcunittest/MavlinkLogTest.cc b/src/qgcunittest/MavlinkLogTest.cc index a870b3b9dabde2ccec80e9a2ad36ac29a6175056..a34c69954590e5429f6172627fb7dc7fe866e1bb 100644 --- a/src/qgcunittest/MavlinkLogTest.cc +++ b/src/qgcunittest/MavlinkLogTest.cc @@ -140,7 +140,7 @@ void MavlinkLogTest::_connectLog_test(void) MockLink* link = new MockLink(); Q_CHECK_PTR(link); - LinkManager::instance()->addLink(link); + LinkManager::instance()->_addLink(link); linkMgr->connectLink(link); QTest::qWait(5000); // Give enough time for UI to settle and heartbeats to go through diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 38fdf67ddca82291896422cbab9ed3a07906c6e4..1334370ff448cf45455e51f9913a72d8821f2343 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -380,8 +380,7 @@ bool UAS::getSelected() const void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { - if (!links.contains(link)) - { + if (!_containsLink(link)) { addLink(link); // qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName(); } @@ -873,8 +872,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } positionLock = true; isGlobalPositionKnown = true; - //TODO fix this hack for forwarding of global position for patch antenna tracking - //forwardMessage(message); } break; case MAVLINK_MSG_ID_GPS_RAW_INT: @@ -1725,49 +1722,21 @@ void UAS::sendMessage(mavlink_message_t message) return; } - if (links.count() < 1) { + if (_links.count() < 1) { qDebug() << "NO LINK AVAILABLE TO SEND!"; } // Emit message on all links that are currently connected - foreach (LinkInterface* link, links) { + foreach (SharedLinkInterface sharedLink, _links) { + LinkInterface* link = sharedLink.data(); + Q_ASSERT(link); + if (link->isConnected()) { sendMessage(link, message); } } } -/** -* Forward a message to all links that are currently connected. -* @param message that is to be forwarded -*/ -void UAS::forwardMessage(mavlink_message_t message) -{ - // Emit message on all links that are currently connected - QListlink_list = LinkManager::instance()->getLinks(); - - foreach(LinkInterface* link, link_list) - { - if (link) - { - SerialLink* serial = dynamic_cast(link); - if(serial != 0) - { - for(int i=0; iisConnected()) { - qDebug()<<"Antenna tracking: Forwarding Over link: "<getName()<<" "<sharedPointerForLink(link)); qCDebug(UASLog) << "addLink:" << QString("%1").arg((ulong)link, 0, 16); connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*))); } @@ -3285,16 +3254,16 @@ void UAS::addLink(LinkInterface* link) void UAS::removeLink(QObject* object) { qCDebug(UASLog) << "removeLink:" << QString("%1").arg((ulong)object, 0, 16); - qCDebug(UASLog) << "link count:" << links.count(); + qCDebug(UASLog) << "link count:" << _links.count(); - // Do not dynamic cast or de-reference QObject, since object is either in destructor or may have already - // been destroyed. + LinkInterface* link = dynamic_cast(object); - LinkInterface* link = (LinkInterface*)object; - - int index = links.indexOf(link); - Q_ASSERT(index != -1); - links.removeAt(index); + for (int i=0; i<_links.count(); i++) { + if (_links[i].data() == link) { + _links.removeAt(i); + break; + } + } } /** @@ -3302,7 +3271,13 @@ void UAS::removeLink(QObject* object) */ QList UAS::getLinks() { - return links; + QList list; + + foreach (SharedLinkInterface sharedLink, _links) { + list << sharedLink.data(); + } + + return list; } /** @@ -3429,3 +3404,14 @@ void UAS::unsetRCToParameterMap() sendMessage(message); } } + +bool UAS::_containsLink(LinkInterface* link) +{ + foreach (SharedLinkInterface sharedLink, _links) { + if (sharedLink.data() == link) { + return true; + } + } + + return false; +} diff --git a/src/uas/UAS.h b/src/uas/UAS.h index d6de3ab9c5e0adc5c789bb92425956ea5fbfd628..7375ca17381c53625bdf4fa8e5cf162b86d38164 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -341,7 +341,11 @@ protected: //COMMENTS FOR TEST UNIT /// LINK ID AND STATUS int uasId; ///< Unique system ID QMap components;///< IDs and names of all detected onboard components - QList links; ///< List of links this UAS can be reached by + + /// List of all links associated with this UAS. We keep SharedLinkInterface objects which are QSharedPointer's in order to + /// maintain reference counts across threads. This way Link deletion works correctly. + QList _links; + QList unknownPackets; ///< Packet IDs which are unknown and have been received MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance CommStatus commStatus; ///< Communication status @@ -812,9 +816,6 @@ public slots: /** @brief Send a message over all links this UAS can be reached with (!= all links) */ void sendMessage(mavlink_message_t message); - /** @brief Temporary Hack for sending packets to patch Antenna. Send a message over all serial links except for this UAS's */ - void forwardMessage(mavlink_message_t message); - /** @brief Set this UAS as the system currently in focus, e.g. in the main display widgets */ void setSelected(); @@ -951,6 +952,9 @@ protected slots: void writeSettings(); /** @brief Read settings from disk */ void readSettings(); + +private: + bool _containsLink(LinkInterface* link); }; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 25b3c679ccc0ee23ca2f9e3751307f9b29efdf3d..41c385311ec2db03253331e3f93d59a662fcf96f 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -642,15 +642,7 @@ void MainWindow::normalActionItemCallback() void MainWindow::closeEvent(QCloseEvent *event) { // Disallow window close if there are active connections - bool foundConnections = false; - foreach(LinkInterface* link, LinkManager::instance()->getLinks()) { - if (link->isConnected()) { - foundConnections = true; - break; - } - } - - if (foundConnections) { + if (LinkManager::instance()->anyConnectedLinks()) { QGCMessageBox::StandardButton button = QGCMessageBox::warning( tr("QGroundControl close"), @@ -658,9 +650,7 @@ void MainWindow::closeEvent(QCloseEvent *event) QMessageBox::Yes | QMessageBox::Cancel, QMessageBox::Cancel); if (button == QMessageBox::Yes) { - foreach(LinkInterface* link, LinkManager::instance()->getLinks()) { - LinkManager::instance()->disconnectLink(link); - } + LinkManager::instance()->disconnectAll(); } else { event->ignore(); return; @@ -669,11 +659,10 @@ void MainWindow::closeEvent(QCloseEvent *event) // This will process any remaining flight log save dialogs qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents); + // Should not be any active connections - foreach(LinkInterface* link, LinkManager::instance()->getLinks()) { - Q_UNUSED(link); - Q_ASSERT(!link->isConnected()); - } + Q_ASSERT(!LinkManager::instance()->anyConnectedLinks()); + _storeCurrentViewState(); storeSettings(); UASManager::instance()->storeSettings(); @@ -1355,11 +1344,7 @@ void MainWindow::restoreLastUsedConnection() if(settings.contains(key)) { QString connection = settings.value(key).toString(); // Create a link for it - LinkInterface* link = LinkManager::instance()->createLink(connection); - if(link) { - // Connect it - LinkManager::instance()->connectLink(link); - } + LinkManager::instance()->createConnectedLink(connection); } } diff --git a/src/ui/QGCLinkConfiguration.cc b/src/ui/QGCLinkConfiguration.cc index 936490a257028a6e9ae78653384e4bc9c68f1f82..fab86ed11137cbdeccf0f22bb4dc83c9d1f98246 100644 --- a/src/ui/QGCLinkConfiguration.cc +++ b/src/ui/QGCLinkConfiguration.cc @@ -104,10 +104,8 @@ void QGCLinkConfiguration::on_connectLinkButton_clicked() LinkManager::instance()->disconnectLink(link); } } else { - LinkInterface* link = LinkManager::instance()->createLink(config); + LinkInterface* link = LinkManager::instance()->createConnectedLink(config); if(link) { - // Connect it - LinkManager::instance()->connectLink(link); // Now go hunting for the parent so we can shut this down QWidget* pQw = parentWidget(); while(pQw) { diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index f4d12d8b8aef30d2d909c72109024fcd586a6586..f475312f3eecb095d4241d24baf5341a17875097 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -246,18 +246,7 @@ void QGCMAVLinkLogPlayer::updatePositionSliderUi(float percent) void QGCMAVLinkLogPlayer::_selectLogFileForPlayback(void) { // Disallow replay when any links are connected - - bool foundConnection = false; - LinkManager* linkMgr = LinkManager::instance(); - QList links = linkMgr->getLinks(); - foreach(LinkInterface* link, links) { - if (link->isConnected()) { - foundConnection = true; - break; - } - } - - if (foundConnection) { + if (LinkManager::instance()->anyConnectedLinks()) { QGCMessageBox::information(tr("Log Replay"), tr("You must close all connections prior to replaying a log.")); return; } @@ -326,7 +315,7 @@ bool QGCMAVLinkLogPlayer::loadLogFile(const QString& file) // If there's an existing MAVLinkSimulationLink() being used for an old file, // we replace it. if (logLink) { - LinkManager::instance()->deleteLink(logLink); + LinkManager::instance()->_deleteLink(logLink); } logLink = new MAVLinkSimulationLink(""); diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index 3d460488200d0589f47e944b2d677dc67135de81..77c22f92fed561f27ca19906fa9f8ae2d50c6aef 100644 --- a/src/ui/QGCToolBar.cc +++ b/src/ui/QGCToolBar.cc @@ -676,10 +676,8 @@ void QGCToolBar::_connectButtonClicked(bool checked) // Get the configuration name QString confName = _linkCombo->currentText(); // Create a link for it - LinkInterface* link = _linkMgr->createLink(confName); + LinkInterface* link = _linkMgr->createConnectedLink(confName); if(link) { - // Connect it - _linkMgr->connectLink(link); // Save last used connection MainWindow::instance()->saveLastUsedConnection(confName); } diff --git a/src/ui/toolbar/MainToolBar.cc b/src/ui/toolbar/MainToolBar.cc index 50f66d8a06798f54487d58804fed2898dfaa6af0..ad9de52a3ad7378898ade0da19df3d927412a3bc 100644 --- a/src/ui/toolbar/MainToolBar.cc +++ b/src/ui/toolbar/MainToolBar.cc @@ -163,10 +163,8 @@ void MainToolBar::onConnect(QString conf) // We don't want the combo box updating under our feet LinkManager::instance()->suspendConfigurationUpdates(true); // Create a link - LinkInterface* link = LinkManager::instance()->createLink(_currentConfig); + LinkInterface* link = LinkManager::instance()->createConnectedLink(_currentConfig); if(link) { - // Connect it - LinkManager::instance()->connectLink(link); // Save last used connection MainWindow::instance()->saveLastUsedConnection(_currentConfig); } diff --git a/src/ui/uas/UASListWidget.cc b/src/ui/uas/UASListWidget.cc index fa5194bb181ebc5d54e4743ba929e4f45be61914..3fe12a1a5ff6da0ed6fb381c9470f8a1e33a251e 100644 --- a/src/ui/uas/UASListWidget.cc +++ b/src/ui/uas/UASListWidget.cc @@ -118,7 +118,7 @@ void UASListWidget::updateStatus() LinkInterface* link = i.key(); // Paranoid sanity check - if (!LinkManager::instance()->getLinks().contains(link)) + if (!LinkManager::instance()->containsLink(link)) continue; if (!link)