Use QSharedPointer for cross-thread Link references

Also many LinkManager API changes to further isolate all link
management inside LinkManager.

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

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
             }

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
     

src/comm/LinkInterface.h

@@ -37,6 +37,7 @@ along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
 #include <QMutex>
 #include <QMutexLocker>
 #include <QMetaType>
+#include <QSharedPointer>
 
 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*>("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*>("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<LinkInterface> SharedLinkInterface;
+
 #endif // _LINKINTERFACE_H_

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<LinkInterface>(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,33 @@ 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_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 +225,13 @@ void LinkManager::deleteLink(LinkInterface* link)
  */
 const QList<LinkInterface*> LinkManager::getLinks()
 {
-    _linkListMutex.lock();
-    QList<LinkInterface*> ret(_links);
-    _linkListMutex.unlock();
-    return ret;
-}
-
-const QList<SerialLink *> LinkManager::getSerialLinks()
-{
-    _linkListMutex.lock();
-    QList<SerialLink*> s;
-
-    foreach (LinkInterface* link, _links)
-    {
-        Q_ASSERT(link);
-
-        SerialLink* serialLink = qobject_cast<SerialLink*>(link);
-
-        if (serialLink)
-            s.append(serialLink);
+    QList<LinkInterface*> 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 +256,8 @@ void LinkManager::setConnectionsSuspended(QString reason)
 
 void LinkManager::_shutdown(void)
 {
-    QList<LinkInterface*> links = _links;
-    foreach(LinkInterface* link, links) {
-        disconnectLink(link);
-        deleteLink(link);
+    while (_links.count() != 0) {
+        disconnectLink(_links[0].data());
     }
 }
 
@@ -489,3 +447,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;
+}

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<LinkConfiguration*>   _linkConfigurations;    ///< List of configured links
-    QList<LinkInterface*>       _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<SharedLinkInterface>  _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

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,15 @@ 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_ASSERT(found);
         
         if (_connectedLinks.count() == 0) {
             // Last link is gone, close out logging

src/comm/MAVLinkProtocol.h

@@ -285,7 +285,10 @@ private:
     void _startLogging(void);
     void _stopLogging(void);
     
-    QList<LinkInterface*> _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<SharedLinkInterface> _connectedLinks;
     
     bool _logSuspendError;      ///< true: Logging suspended due to error
     bool _logSuspendReplay;     ///< true: Logging suspended due to replay

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()

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);

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();

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);

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<LinkInterface*> 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);

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
     

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
     

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
-    QList<LinkInterface*>link_list = LinkManager::instance()->getLinks();
-
-    foreach(LinkInterface* link, link_list)
-    {
-        if (link)
-        {
-            SerialLink* serial = dynamic_cast<SerialLink*>(link);
-            if(serial != 0)
-            {
-                for(int i=0; i<links.size(); i++)
-                {
-                    if(serial != links.at(i))
-                    {
-                        if (link->isConnected()) {
-                            qDebug()<<"Antenna tracking: Forwarding Over link: "<<serial->getName()<<" "<<serial;
-                            sendMessage(serial, message);
-                        }
-                    }
-                }
-            }
-        }
-    }
-}
-
 /**
 * Send a message to the link that is connected.
 * @param link that the message will be sent to
@@ -3274,9 +3243,9 @@ const QString& UAS::getShortMode() const
 */
 void UAS::addLink(LinkInterface* link)
 {
-    if (!links.contains(link))
+    if (!_containsLink(link))
     {
-        links.append(link);
+        _links.append(LinkManager::instance()->sharedPointerForLink(link));
         qCDebug(UASLog) << "addLink:" << QString("%1").arg((ulong)link, 0, 16);
         connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
     }