LinkManager.cc 20.3 KB
Newer Older
pixhawk's avatar
pixhawk committed
1
/*=====================================================================
lm's avatar
lm committed
2 3 4

QGroundControl Open Source Ground Control Station

5
(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
lm's avatar
lm committed
6 7 8 9

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
pixhawk's avatar
pixhawk committed
10 11 12
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
lm's avatar
lm committed
13 14

    QGROUNDCONTROL is distributed in the hope that it will be useful,
pixhawk's avatar
pixhawk committed
15 16 17
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
lm's avatar
lm committed
18

pixhawk's avatar
pixhawk committed
19
    You should have received a copy of the GNU General Public License
lm's avatar
lm committed
20 21
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

pixhawk's avatar
pixhawk committed
22
======================================================================*/
23

pixhawk's avatar
pixhawk committed
24 25 26 27 28 29 30 31 32 33
/**
 * @file
 *   @brief Brief Description
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QList>
#include <QApplication>
34
#include <QDebug>
dogmaphobic's avatar
dogmaphobic committed
35 36

#ifndef __ios__
dogmaphobic's avatar
dogmaphobic committed
37 38 39
#ifdef __android__
#include "qserialportinfo.h"
#else
40
#include <QSerialPortInfo>
dogmaphobic's avatar
dogmaphobic committed
41
#endif
dogmaphobic's avatar
dogmaphobic committed
42
#endif
43

44 45
#include "LinkManager.h"
#include "MainWindow.h"
Don Gagne's avatar
Don Gagne committed
46
#include "QGCMessageBox.h"
47
#include "QGCApplication.h"
48
#include "SerialPortIds.h"
49

Don Gagne's avatar
Don Gagne committed
50
IMPLEMENT_QGC_SINGLETON(LinkManager, LinkManager)
Don Gagne's avatar
Don Gagne committed
51
QGC_LOGGING_CATEGORY(LinkManagerLog, "LinkManagerLog")
52

pixhawk's avatar
pixhawk committed
53 54
/**
 * @brief Private singleton constructor
55
 *
pixhawk's avatar
pixhawk committed
56 57
 * This class implements the singleton design pattern and has therefore only a private constructor.
 **/
58 59 60 61 62
LinkManager::LinkManager(QObject* parent)
    : QGCSingleton(parent)
    , _configUpdateSuspended(false)
    , _configurationsLoaded(false)
    , _connectionsSuspended(false)
63
    , _mavlinkChannelsUsedBitMask(0)
64
    , _nullSharedLink(NULL)
pixhawk's avatar
pixhawk committed
65
{
dogmaphobic's avatar
dogmaphobic committed
66
#ifndef __ios__
67 68
    connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateConfigurationList);
    _portListTimer.start(1000);
dogmaphobic's avatar
dogmaphobic committed
69
#endif
pixhawk's avatar
pixhawk committed
70 71 72 73
}

LinkManager::~LinkManager()
{
74 75 76 77 78 79
    // Clear configuration list
    while(_linkConfigurations.count()) {
        LinkConfiguration* pLink = _linkConfigurations.at(0);
        if(pLink) delete pLink;
        _linkConfigurations.removeAt(0);
    }
80
    Q_ASSERT_X(_links.count() == 0, "LinkManager", "LinkManager::_shutdown should have been called previously");
pixhawk's avatar
pixhawk committed
81 82
}

83
LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config)
84 85 86 87
{
    Q_ASSERT(config);
    LinkInterface* pLink = NULL;
    switch(config->type()) {
dogmaphobic's avatar
dogmaphobic committed
88
#ifndef __ios__
89 90 91
        case LinkConfiguration::TypeSerial:
            pLink = new SerialLink(dynamic_cast<SerialConfiguration*>(config));
            break;
dogmaphobic's avatar
dogmaphobic committed
92
#endif
93 94 95
        case LinkConfiguration::TypeUdp:
            pLink = new UDPLink(dynamic_cast<UDPConfiguration*>(config));
            break;
96 97 98
        case LinkConfiguration::TypeTcp:
            pLink = new TCPLink(dynamic_cast<TCPConfiguration*>(config));
            break;
Don Gagne's avatar
Don Gagne committed
99 100 101
        case LinkConfiguration::TypeLogReplay:
            pLink = new LogReplayLink(dynamic_cast<LogReplayLinkConfiguration*>(config));
            break;
102
#ifdef QT_DEBUG
103 104 105
        case LinkConfiguration::TypeMock:
            pLink = new MockLink(dynamic_cast<MockConfiguration*>(config));
            break;
106
#endif
107 108
    }
    if(pLink) {
109 110
        _addLink(pLink);
        connectLink(pLink);
111 112 113 114
    }
    return pLink;
}

115
LinkInterface* LinkManager::createConnectedLink(const QString& name)
116 117 118 119 120
{
    Q_ASSERT(name.isEmpty() == false);
    for(int i = 0; i < _linkConfigurations.count(); i++) {
        LinkConfiguration* conf = _linkConfigurations.at(i);
        if(conf && conf->name() == name)
121
            return createConnectedLink(conf);
122 123 124 125
    }
    return NULL;
}

126
void LinkManager::_addLink(LinkInterface* link)
pixhawk's avatar
pixhawk committed
127
{
128
    Q_ASSERT(link);
129

130
    _linkListMutex.lock();
131

132
    if (!containsLink(link)) {
133 134 135
        // Find a mavlink channel to use for this link
        for (int i=0; i<32; i++) {
            if (!(_mavlinkChannelsUsedBitMask && 1 << i)) {
136
                mavlink_reset_channel_status(i);
137 138 139 140 141 142
                link->_setMavlinkChannel(i);
                _mavlinkChannelsUsedBitMask |= i << i;
                break;
            }
        }
        
143
        _links.append(QSharedPointer<LinkInterface>(link));
144
        _linkListMutex.unlock();
145
        emit newLink(link);
146
    } else {
147
        _linkListMutex.unlock();
148
    }
149

150 151
    // MainWindow may be around when doing things like running unit tests
    if (MainWindow::instance()) {
152
        connect(link, &LinkInterface::communicationError, qgcApp(), &QGCApplication::criticalMessageBoxOnMainThread);
153
    }
154

155 156 157 158 159
    MAVLinkProtocol* mavlink = MAVLinkProtocol::instance();
    connect(link, &LinkInterface::bytesReceived, mavlink, &MAVLinkProtocol::receiveBytes);
    connect(link, &LinkInterface::connected, mavlink, &MAVLinkProtocol::linkConnected);
    connect(link, &LinkInterface::disconnected, mavlink, &MAVLinkProtocol::linkDisconnected);
    mavlink->resetMetadataForLink(link);
160

161 162
    connect(link, &LinkInterface::connected, this, &LinkManager::_linkConnected);
    connect(link, &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected);
163
}
pixhawk's avatar
pixhawk committed
164 165 166

bool LinkManager::connectAll()
{
167 168 169
    if (_connectionsSuspendedMsg()) {
        return false;
    }
170

171 172
    bool allConnected = true;

173 174 175
    foreach (SharedLinkInterface sharedLink, _links) {
        Q_ASSERT(sharedLink.data());
        if (!sharedLink.data()->_connect()) {
176 177
            allConnected = false;
        }
178 179 180
    }

    return allConnected;
pixhawk's avatar
pixhawk committed
181 182 183 184
}

bool LinkManager::disconnectAll()
{
185 186
    bool allDisconnected = true;

187 188 189 190
    // Make a copy so the list is modified out from under us
    QList<SharedLinkInterface> links = _links;

    foreach (SharedLinkInterface sharedLink, links) {
191
        Q_ASSERT(sharedLink.data());
192
        if (!disconnectLink(sharedLink.data())) {
193 194
            allDisconnected = false;
        }
195 196 197
    }

    return allDisconnected;
pixhawk's avatar
pixhawk committed
198 199 200 201
}

bool LinkManager::connectLink(LinkInterface* link)
{
202
    Q_ASSERT(link);
203

204 205 206 207
    if (_connectionsSuspendedMsg()) {
        return false;
    }

208 209 210 211 212
    if (link->_connect()) {
        return true;
    } else {
        return false;
    }
pixhawk's avatar
pixhawk committed
213 214 215 216
}

bool LinkManager::disconnectLink(LinkInterface* link)
{
217
    Q_ASSERT(link);
218
    if (link->_disconnect()) {
219 220 221 222
        LinkConfiguration* config = link->getLinkConfiguration();
        if(config) {
            config->setLink(NULL);
        }
223
        _deleteLink(link);
224 225 226 227
        return true;
    } else {
        return false;
    }
pixhawk's avatar
pixhawk committed
228 229
}

230
void LinkManager::_deleteLink(LinkInterface* link)
231
{
232
    Q_ASSERT(link);
233

234
    _linkListMutex.lock();
235 236 237
    
    // Free up the mavlink channel associated with this link
    _mavlinkChannelsUsedBitMask &= ~(1 << link->getMavlinkChannel());
238

239 240 241 242 243 244 245 246
    bool found = false;
    for (int i=0; i<_links.count(); i++) {
        if (_links[i].data() == link) {
            _links.removeAt(i);
            found = true;
            break;
        }
    }
247
    Q_UNUSED(found);
248
    Q_ASSERT(found);
249

250
    _linkListMutex.unlock();
251

Don Gagne's avatar
Don Gagne committed
252
    // Emit removal of link
253
    emit linkDeleted(link);
pixhawk's avatar
pixhawk committed
254 255 256 257 258 259 260
}

/**
 *
 */
const QList<LinkInterface*> LinkManager::getLinks()
{
261 262 263 264
    QList<LinkInterface*> list;
    
    foreach (SharedLinkInterface sharedLink, _links) {
        list << sharedLink.data();
265
    }
266 267
    
    return list;
268
}
269 270 271 272 273 274

/// @brief If all new connections should be suspended a message is displayed to the user and true
///         is returned;
bool LinkManager::_connectionsSuspendedMsg(void)
{
    if (_connectionsSuspended) {
Don Gagne's avatar
Don Gagne committed
275 276
        QGCMessageBox::information(tr("Connect not allowed"),
                                   tr("Connect not allowed: %1").arg(_connectionsSuspendedReason));
277 278 279 280 281 282 283 284 285 286 287 288
        return true;
    } else {
        return false;
    }
}

void LinkManager::setConnectionsSuspended(QString reason)
{
    _connectionsSuspended = true;
    _connectionsSuspendedReason = reason;
    Q_ASSERT(!reason.isEmpty());
}
289 290 291

void LinkManager::_shutdown(void)
{
292 293
    while (_links.count() != 0) {
        disconnectLink(_links[0].data());
294 295
    }
}
296 297 298 299 300 301 302 303 304 305

void LinkManager::_linkConnected(void)
{
    emit linkConnected((LinkInterface*)sender());
}

void LinkManager::_linkDisconnected(void)
{
    emit linkDisconnected((LinkInterface*)sender());
}
306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345

void LinkManager::addLinkConfiguration(LinkConfiguration* link)
{
    Q_ASSERT(link != NULL);
    //-- If not there already, add it
    int idx = _linkConfigurations.indexOf(link);
    if(idx < 0)
    {
        _linkConfigurations.append(link);
    }
}

void LinkManager::removeLinkConfiguration(LinkConfiguration *link)
{
    Q_ASSERT(link != NULL);
    int idx = _linkConfigurations.indexOf(link);
    if(idx >= 0)
    {
        _linkConfigurations.removeAt(idx);
        delete link;
    }
}

const QList<LinkConfiguration*> LinkManager::getLinkConfigurationList()
{
    return _linkConfigurations;
}

void LinkManager::suspendConfigurationUpdates(bool suspend)
{
    _configUpdateSuspended = suspend;
}

void LinkManager::saveLinkConfigurationList()
{
    QSettings settings;
    settings.remove(LinkConfiguration::settingsRoot());
    int index = 0;
    foreach (LinkConfiguration* pLink, _linkConfigurations) {
        Q_ASSERT(pLink != NULL);
dogmaphobic's avatar
dogmaphobic committed
346 347 348 349 350 351 352 353 354 355
        if(!pLink->isDynamic())
        {
            QString root = LinkConfiguration::settingsRoot();
            root += QString("/Link%1").arg(index++);
            settings.setValue(root + "/name", pLink->name());
            settings.setValue(root + "/type", pLink->type());
            settings.setValue(root + "/preferred", pLink->isPreferred());
            // Have the instance save its own values
            pLink->saveSettings(settings, root);
        }
356
    }
dogmaphobic's avatar
dogmaphobic committed
357
    QString root(LinkConfiguration::settingsRoot());
358
    settings.setValue(root + "/count", index);
359 360 361 362 363
    emit linkConfigurationChanged();
}

void LinkManager::loadLinkConfigurationList()
{
364 365
    bool udpExists = false;
    bool linksChanged = false;
366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385
    QSettings settings;
    // Is the group even there?
    if(settings.contains(LinkConfiguration::settingsRoot() + "/count")) {
        // Find out how many configurations we have
        int count = settings.value(LinkConfiguration::settingsRoot() + "/count").toInt();
        for(int i = 0; i < count; i++) {
            QString root(LinkConfiguration::settingsRoot());
            root += QString("/Link%1").arg(i);
            if(settings.contains(root + "/type")) {
                int type = settings.value(root + "/type").toInt();
                if(type < LinkConfiguration::TypeLast) {
                    if(settings.contains(root + "/name")) {
                        QString name = settings.value(root + "/name").toString();
                        if(!name.isEmpty()) {
                            bool preferred = false;
                            if(settings.contains(root + "/preferred")) {
                                preferred = settings.value(root + "/preferred").toBool();
                            }
                            LinkConfiguration* pLink = NULL;
                            switch(type) {
dogmaphobic's avatar
dogmaphobic committed
386
#ifndef __ios__
387 388 389 390
                                case LinkConfiguration::TypeSerial:
                                    pLink = (LinkConfiguration*)new SerialConfiguration(name);
                                    pLink->setPreferred(preferred);
                                    break;
dogmaphobic's avatar
dogmaphobic committed
391
#endif
392 393 394 395
                                case LinkConfiguration::TypeUdp:
                                    pLink = (LinkConfiguration*)new UDPConfiguration(name);
                                    pLink->setPreferred(preferred);
                                    break;
396 397 398 399
                                case LinkConfiguration::TypeTcp:
                                    pLink = (LinkConfiguration*)new TCPConfiguration(name);
                                    pLink->setPreferred(preferred);
                                    break;
Don Gagne's avatar
Don Gagne committed
400 401 402 403
                                case LinkConfiguration::TypeLogReplay:
                                    pLink = (LinkConfiguration*)new LogReplayLinkConfiguration(name);
                                    pLink->setPreferred(preferred);
                                    break;
404
#ifdef QT_DEBUG
405 406 407 408
                                case LinkConfiguration::TypeMock:
                                    pLink = (LinkConfiguration*)new MockConfiguration(name);
                                    pLink->setPreferred(false);
                                    break;
409
#endif
410 411 412 413 414
                            }
                            if(pLink) {
                                // Have the instance load its own values
                                pLink->loadSettings(settings, root);
                                addLinkConfiguration(pLink);
415 416 417 418 419 420 421 422
                                linksChanged = true;
                                // Check for UDP links
                                if(pLink->type() == LinkConfiguration::TypeUdp) {
                                    UDPConfiguration* uLink = dynamic_cast<UDPConfiguration*>(pLink);
                                    if(uLink && uLink->localPort() == QGC_UDP_LOCAL_PORT) {
                                        udpExists = true;
                                    }
                                }
423 424 425 426 427 428 429 430 431 432 433 434 435 436 437
                            }
                        } else {
                            qWarning() << "Link Configuration " << root << " has an empty name." ;
                        }
                    } else {
                        qWarning() << "Link Configuration " << root << " has no name." ;
                    }
                } else {
                    qWarning() << "Link Configuration " << root << " an invalid type: " << type;
                }
            } else {
                qWarning() << "Link Configuration " << root << " has no type." ;
            }
        }
    }
438 439 440 441 442 443
    
    // Debug buids always add MockLink automatically
#ifdef QT_DEBUG
    MockConfiguration* pMock = new MockConfiguration("Mock Link");
    pMock->setDynamic(true);
    addLinkConfiguration(pMock);
444
    linksChanged = true;
445
#endif
446 447 448 449 450 451 452 453 454

    //-- If we don't have a configured UDP link, create a default one
    if(!udpExists) {
        UDPConfiguration* uLink = new UDPConfiguration("Default UDP Link");
        uLink->setLocalPort(QGC_UDP_LOCAL_PORT);
        uLink->setDynamic();
        addLinkConfiguration(uLink);
        linksChanged = true;
    }
455
    
456 457 458 459
    if(linksChanged) {
        emit linkConfigurationChanged();
    }
    // Enable automatic Serial PX4/3DR Radio hunting
460 461 462
    _configurationsLoaded = true;
}

dogmaphobic's avatar
dogmaphobic committed
463
#ifndef __ios__
464 465 466 467 468 469 470 471 472 473 474 475 476 477
SerialConfiguration* LinkManager::_findSerialConfiguration(const QString& portName)
{
    QString searchPort = portName.trimmed();
    foreach (LinkConfiguration* pLink, _linkConfigurations) {
        Q_ASSERT(pLink != NULL);
        if(pLink->type() == LinkConfiguration::TypeSerial) {
            SerialConfiguration* pSerial = dynamic_cast<SerialConfiguration*>(pLink);
            if(pSerial->portName() == searchPort) {
                return pSerial;
            }
        }
    }
    return NULL;
}
dogmaphobic's avatar
dogmaphobic committed
478
#endif
479

dogmaphobic's avatar
dogmaphobic committed
480
#ifndef __ios__
481 482 483 484 485 486
void LinkManager::_updateConfigurationList(void)
{
    if (_configUpdateSuspended || !_configurationsLoaded) {
        return;
    }
    bool saveList = false;
dogmaphobic's avatar
dogmaphobic committed
487
    QStringList currentPorts;
488 489 490
    QList<QSerialPortInfo> portList = QSerialPortInfo::availablePorts();
    // Iterate Comm Ports
    foreach (QSerialPortInfo portInfo, portList) {
Don Gagne's avatar
Don Gagne committed
491 492
#if 0
        // Too noisy for most logging, so turn on as needed
Don Gagne's avatar
Don Gagne committed
493 494 495 496 497 498 499
        qCDebug(LinkManagerLog) << "-----------------------------------------------------";
        qCDebug(LinkManagerLog) << "portName:         " << portInfo.portName();
        qCDebug(LinkManagerLog) << "systemLocation:   " << portInfo.systemLocation();
        qCDebug(LinkManagerLog) << "description:      " << portInfo.description();
        qCDebug(LinkManagerLog) << "manufacturer:     " << portInfo.manufacturer();
        qCDebug(LinkManagerLog) << "serialNumber:     " << portInfo.serialNumber();
        qCDebug(LinkManagerLog) << "vendorIdentifier: " << portInfo.vendorIdentifier();
Don Gagne's avatar
Don Gagne committed
500
#endif
dogmaphobic's avatar
dogmaphobic committed
501 502
        // Save port name
        currentPorts << portInfo.systemLocation();
503
        // Is this a PX4 and NOT in bootloader mode?
504
        if (portInfo.vendorIdentifier() == SerialPortIds::px4VendorId && !portInfo.description().contains("BL")) {
505
            SerialConfiguration* pSerial = _findSerialConfiguration(portInfo.systemLocation());
506 507 508 509 510 511 512 513
            if (pSerial) {
                //-- If this port is configured make sure it has the preferred flag set
                if(!pSerial->isPreferred()) {
                    pSerial->setPreferred(true);
                    saveList = true;
                }
            } else {
                // Lets create a new Serial configuration automatically
dogmaphobic's avatar
dogmaphobic committed
514 515
                if (portInfo.description() == "AeroCore") {
                    pSerial = new SerialConfiguration(QString("AeroCore on %1").arg(portInfo.portName().trimmed()));
Don Gagne's avatar
Don Gagne committed
516 517
                } else if (portInfo.description().contains("PX4Flow")) {
                    pSerial = new SerialConfiguration(QString("PX4Flow on %1").arg(portInfo.portName().trimmed()));
dogmaphobic's avatar
dogmaphobic committed
518 519 520 521 522
                } else if (portInfo.description().contains("PX4")) {
                    pSerial = new SerialConfiguration(QString("Pixhawk on %1").arg(portInfo.portName().trimmed()));
                } else {
                    continue;
                }
dogmaphobic's avatar
dogmaphobic committed
523
                pSerial->setDynamic(true);
524 525
                pSerial->setPreferred(true);
                pSerial->setBaud(115200);
526
                pSerial->setPortName(portInfo.systemLocation());
527 528 529 530
                addLinkConfiguration(pSerial);
                saveList = true;
            }
        }
dogmaphobic's avatar
dogmaphobic committed
531
        // Is this an FTDI Chip? It could be a 3DR Modem
532
        if (portInfo.vendorIdentifier() == SerialPortIds::threeDRRadioVendorId && portInfo.productIdentifier() == SerialPortIds::threeDRRadioProductId) {
dogmaphobic's avatar
dogmaphobic committed
533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558
            SerialConfiguration* pSerial = _findSerialConfiguration(portInfo.systemLocation());
            if (pSerial) {
                //-- If this port is configured make sure it has the preferred flag set, unless someone else already has it set.
                if(!pSerial->isPreferred() && !saveList) {
                    pSerial->setPreferred(true);
                    saveList = true;
                }
            } else {
                // Lets create a new Serial configuration automatically (an assumption at best)
                pSerial = new SerialConfiguration(QString("3DR Radio on %1").arg(portInfo.portName().trimmed()));
                pSerial->setDynamic(true);
                pSerial->setPreferred(true);
                pSerial->setBaud(57600);
                pSerial->setPortName(portInfo.systemLocation());
                addLinkConfiguration(pSerial);
                saveList = true;
            }
        }
    }
    // Now we go through the current configuration list and make sure any dynamic config has gone away
    QList<LinkConfiguration*>  _confToDelete;
    foreach (LinkConfiguration* pLink, _linkConfigurations) {
        Q_ASSERT(pLink != NULL);
        // We only care about dynamic links
        if(pLink->isDynamic()) {
            if(pLink->type() == LinkConfiguration::TypeSerial) {
559 560 561 562 563 564
                // Don't mess with connected link. Let it deal with the disapearing device.
                if(pLink->getLink() == NULL) {
                    SerialConfiguration* pSerial = dynamic_cast<SerialConfiguration*>(pLink);
                    if(!currentPorts.contains(pSerial->portName())) {
                        _confToDelete.append(pSerial);
                    }
dogmaphobic's avatar
dogmaphobic committed
565 566 567 568 569 570 571 572
                }
            }
        }
    }
    // Now remove all links that are gone
    foreach (LinkConfiguration* pDelete, _confToDelete) {
        removeLinkConfiguration(pDelete);
        saveList = true;
573 574 575 576 577 578
    }
    // Save configuration list, which will also trigger a signal for the UI
    if(saveList) {
        saveLinkConfigurationList();
    }
}
dogmaphobic's avatar
dogmaphobic committed
579
#endif
580

581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615
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;
}