LinkManager.cc 17.9 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 37
#ifdef __android__
#include "qserialportinfo.h"
#else
38
#include <QSerialPortInfo>
dogmaphobic's avatar
dogmaphobic committed
39
#endif
40

41 42
#include "LinkManager.h"
#include "MainWindow.h"
Don Gagne's avatar
Don Gagne committed
43
#include "QGCMessageBox.h"
44
#include "QGCApplication.h"
45

Don Gagne's avatar
Don Gagne committed
46
IMPLEMENT_QGC_SINGLETON(LinkManager, LinkManager)
47

48

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

LinkManager::~LinkManager()
{
68 69 70 71 72 73
    // Clear configuration list
    while(_linkConfigurations.count()) {
        LinkConfiguration* pLink = _linkConfigurations.at(0);
        if(pLink) delete pLink;
        _linkConfigurations.removeAt(0);
    }
74
    Q_ASSERT_X(_links.count() == 0, "LinkManager", "LinkManager::_shutdown should have been called previously");
pixhawk's avatar
pixhawk committed
75 76
}

77
LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config)
78 79 80 81 82 83 84 85 86 87
{
    Q_ASSERT(config);
    LinkInterface* pLink = NULL;
    switch(config->type()) {
        case LinkConfiguration::TypeSerial:
            pLink = new SerialLink(dynamic_cast<SerialConfiguration*>(config));
            break;
        case LinkConfiguration::TypeUdp:
            pLink = new UDPLink(dynamic_cast<UDPConfiguration*>(config));
            break;
88 89 90
        case LinkConfiguration::TypeTcp:
            pLink = new TCPLink(dynamic_cast<TCPConfiguration*>(config));
            break;
91
#ifdef QT_DEBUG
92 93 94
        case LinkConfiguration::TypeMock:
            pLink = new MockLink(dynamic_cast<MockConfiguration*>(config));
            break;
95
#endif
96 97
    }
    if(pLink) {
98 99
        _addLink(pLink);
        connectLink(pLink);
100 101 102 103
    }
    return pLink;
}

104
LinkInterface* LinkManager::createConnectedLink(const QString& name)
105 106 107 108 109
{
    Q_ASSERT(name.isEmpty() == false);
    for(int i = 0; i < _linkConfigurations.count(); i++) {
        LinkConfiguration* conf = _linkConfigurations.at(i);
        if(conf && conf->name() == name)
110
            return createConnectedLink(conf);
111 112 113 114
    }
    return NULL;
}

115
void LinkManager::_addLink(LinkInterface* link)
pixhawk's avatar
pixhawk committed
116
{
117
    Q_ASSERT(link);
118

119
    _linkListMutex.lock();
120

121
    if (!containsLink(link)) {
122 123 124
        // Find a mavlink channel to use for this link
        for (int i=0; i<32; i++) {
            if (!(_mavlinkChannelsUsedBitMask && 1 << i)) {
125
                mavlink_reset_channel_status(i);
126 127 128 129 130 131
                link->_setMavlinkChannel(i);
                _mavlinkChannelsUsedBitMask |= i << i;
                break;
            }
        }
        
132
        _links.append(QSharedPointer<LinkInterface>(link));
133
        _linkListMutex.unlock();
134
        emit newLink(link);
135
    } else {
136
        _linkListMutex.unlock();
137
    }
138

139 140
    // MainWindow may be around when doing things like running unit tests
    if (MainWindow::instance()) {
141
        connect(link, &LinkInterface::communicationError, qgcApp(), &QGCApplication::criticalMessageBoxOnMainThread);
142
    }
143

144 145 146 147 148
    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);
149

150 151
    connect(link, &LinkInterface::connected, this, &LinkManager::_linkConnected);
    connect(link, &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected);
152
}
pixhawk's avatar
pixhawk committed
153 154 155

bool LinkManager::connectAll()
{
156 157 158
    if (_connectionsSuspendedMsg()) {
        return false;
    }
159

160 161
    bool allConnected = true;

162 163 164
    foreach (SharedLinkInterface sharedLink, _links) {
        Q_ASSERT(sharedLink.data());
        if (!sharedLink.data()->_connect()) {
165 166
            allConnected = false;
        }
167 168 169
    }

    return allConnected;
pixhawk's avatar
pixhawk committed
170 171 172 173
}

bool LinkManager::disconnectAll()
{
174 175
    bool allDisconnected = true;

176 177 178 179
    // Make a copy so the list is modified out from under us
    QList<SharedLinkInterface> links = _links;

    foreach (SharedLinkInterface sharedLink, links) {
180
        Q_ASSERT(sharedLink.data());
181
        if (!disconnectLink(sharedLink.data())) {
182 183
            allDisconnected = false;
        }
184 185 186
    }

    return allDisconnected;
pixhawk's avatar
pixhawk committed
187 188 189 190
}

bool LinkManager::connectLink(LinkInterface* link)
{
191
    Q_ASSERT(link);
192

193 194 195 196
    if (_connectionsSuspendedMsg()) {
        return false;
    }

197 198 199 200 201
    if (link->_connect()) {
        return true;
    } else {
        return false;
    }
pixhawk's avatar
pixhawk committed
202 203 204 205
}

bool LinkManager::disconnectLink(LinkInterface* link)
{
206
    Q_ASSERT(link);
207
    if (link->_disconnect()) {
208 209 210 211
        LinkConfiguration* config = link->getLinkConfiguration();
        if(config) {
            config->setLink(NULL);
        }
212
        _deleteLink(link);
213 214 215 216
        return true;
    } else {
        return false;
    }
pixhawk's avatar
pixhawk committed
217 218
}

219
void LinkManager::_deleteLink(LinkInterface* link)
220
{
221
    Q_ASSERT(link);
222

223
    _linkListMutex.lock();
224 225 226
    
    // Free up the mavlink channel associated with this link
    _mavlinkChannelsUsedBitMask &= ~(1 << link->getMavlinkChannel());
227

228 229 230 231 232 233 234 235
    bool found = false;
    for (int i=0; i<_links.count(); i++) {
        if (_links[i].data() == link) {
            _links.removeAt(i);
            found = true;
            break;
        }
    }
236
    Q_UNUSED(found);
237
    Q_ASSERT(found);
238

239
    _linkListMutex.unlock();
240

Don Gagne's avatar
Don Gagne committed
241
    // Emit removal of link
242
    emit linkDeleted(link);
pixhawk's avatar
pixhawk committed
243 244 245 246 247 248 249
}

/**
 *
 */
const QList<LinkInterface*> LinkManager::getLinks()
{
250 251 252 253
    QList<LinkInterface*> list;
    
    foreach (SharedLinkInterface sharedLink, _links) {
        list << sharedLink.data();
254
    }
255 256
    
    return list;
257
}
258 259 260 261 262 263

/// @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
264 265
        QGCMessageBox::information(tr("Connect not allowed"),
                                   tr("Connect not allowed: %1").arg(_connectionsSuspendedReason));
266 267 268 269 270 271 272 273 274 275 276 277
        return true;
    } else {
        return false;
    }
}

void LinkManager::setConnectionsSuspended(QString reason)
{
    _connectionsSuspended = true;
    _connectionsSuspendedReason = reason;
    Q_ASSERT(!reason.isEmpty());
}
278 279 280

void LinkManager::_shutdown(void)
{
281 282
    while (_links.count() != 0) {
        disconnectLink(_links[0].data());
283 284
    }
}
285 286 287 288 289 290 291 292 293 294

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

void LinkManager::_linkDisconnected(void)
{
    emit linkDisconnected((LinkInterface*)sender());
}
295 296 297 298 299 300 301 302 303 304 305 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

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
335 336 337 338 339 340 341 342 343 344
        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);
        }
345
    }
dogmaphobic's avatar
dogmaphobic committed
346
    QString root(LinkConfiguration::settingsRoot());
347
    settings.setValue(root + "/count", index);
348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380
    emit linkConfigurationChanged();
}

void LinkManager::loadLinkConfigurationList()
{
    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) {
                                case LinkConfiguration::TypeSerial:
                                    pLink = (LinkConfiguration*)new SerialConfiguration(name);
                                    pLink->setPreferred(preferred);
                                    break;
                                case LinkConfiguration::TypeUdp:
                                    pLink = (LinkConfiguration*)new UDPConfiguration(name);
                                    pLink->setPreferred(preferred);
                                    break;
381 382 383 384
                                case LinkConfiguration::TypeTcp:
                                    pLink = (LinkConfiguration*)new TCPConfiguration(name);
                                    pLink->setPreferred(preferred);
                                    break;
385
#ifdef QT_DEBUG
386 387 388 389
                                case LinkConfiguration::TypeMock:
                                    pLink = (LinkConfiguration*)new MockConfiguration(name);
                                    pLink->setPreferred(false);
                                    break;
390
#endif
391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411
                            }
                            if(pLink) {
                                // Have the instance load its own values
                                pLink->loadSettings(settings, root);
                                addLinkConfiguration(pLink);
                            }
                        } 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." ;
            }
        }
        emit linkConfigurationChanged();
    }
412 413 414 415 416 417 418 419 420
    
    // Debug buids always add MockLink automatically
#ifdef QT_DEBUG
    MockConfiguration* pMock = new MockConfiguration("Mock Link");
    pMock->setDynamic(true);
    addLinkConfiguration(pMock);
    emit linkConfigurationChanged();
#endif
    
421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445
    // Enable automatic PX4 hunting
    _configurationsLoaded = true;
}

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

void LinkManager::_updateConfigurationList(void)
{
    if (_configUpdateSuspended || !_configurationsLoaded) {
        return;
    }
    bool saveList = false;
dogmaphobic's avatar
dogmaphobic committed
446
    QStringList currentPorts;
447 448 449 450 451 452 453 454 455 456 457 458
    QList<QSerialPortInfo> portList = QSerialPortInfo::availablePorts();
    // Iterate Comm Ports
    foreach (QSerialPortInfo portInfo, portList) {
#if 0
        qDebug() << "-----------------------------------------------------";
        qDebug() << "portName:         " << portInfo.portName();
        qDebug() << "systemLocation:   " << portInfo.systemLocation();
        qDebug() << "description:      " << portInfo.description();
        qDebug() << "manufacturer:     " << portInfo.manufacturer();
        qDebug() << "serialNumber:     " << portInfo.serialNumber();
        qDebug() << "vendorIdentifier: " << portInfo.vendorIdentifier();
#endif
dogmaphobic's avatar
dogmaphobic committed
459 460
        // Save port name
        currentPorts << portInfo.systemLocation();
461 462
        // Is this a PX4?
        if (portInfo.vendorIdentifier() == 9900) {
463
            SerialConfiguration* pSerial = _findSerialConfiguration(portInfo.systemLocation());
464 465 466 467 468 469 470 471 472
            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
                pSerial = new SerialConfiguration(QString("Pixhawk on %1").arg(portInfo.portName().trimmed()));
dogmaphobic's avatar
dogmaphobic committed
473
                pSerial->setDynamic(true);
474 475
                pSerial->setPreferred(true);
                pSerial->setBaud(115200);
476
                pSerial->setPortName(portInfo.systemLocation());
477 478 479 480
                addLinkConfiguration(pSerial);
                saveList = true;
            }
        }
dogmaphobic's avatar
dogmaphobic committed
481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508
        // Is this an FTDI Chip? It could be a 3DR Modem
        if (portInfo.vendorIdentifier() == 1027) {
            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) {
509 510 511 512 513 514
                // 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
515 516 517 518 519 520 521 522
                }
            }
        }
    }
    // Now remove all links that are gone
    foreach (LinkConfiguration* pDelete, _confToDelete) {
        removeLinkConfiguration(pDelete);
        saveList = true;
523 524 525 526 527 528 529
    }
    // Save configuration list, which will also trigger a signal for the UI
    if(saveList) {
        saveLinkConfigurationList();
    }
}

530 531 532 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 559 560 561 562 563 564
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;
}