LinkManager.cc 15.5 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>
35
#include <QSerialPortInfo>
36

37 38
#include "LinkManager.h"
#include "MainWindow.h"
Don Gagne's avatar
Don Gagne committed
39
#include "QGCMessageBox.h"
40
#include "QGCApplication.h"
41

Don Gagne's avatar
Don Gagne committed
42
IMPLEMENT_QGC_SINGLETON(LinkManager, LinkManager)
43

44

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

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

73
LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config)
74 75 76 77 78 79 80 81 82 83
{
    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;
84 85 86
        case LinkConfiguration::TypeTcp:
            pLink = new TCPLink(dynamic_cast<TCPConfiguration*>(config));
            break;
87
#ifdef UNITTEST_BUILD
88 89 90
        case LinkConfiguration::TypeMock:
            pLink = new MockLink(dynamic_cast<MockConfiguration*>(config));
            break;
91
#endif
92 93
    }
    if(pLink) {
94 95
        _addLink(pLink);
        connectLink(pLink);
96 97 98 99
    }
    return pLink;
}

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

111
void LinkManager::_addLink(LinkInterface* link)
pixhawk's avatar
pixhawk committed
112
{
113
    Q_ASSERT(link);
114

115
    _linkListMutex.lock();
116

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

135 136
    // MainWindow may be around when doing things like running unit tests
    if (MainWindow::instance()) {
137
        connect(link, &LinkInterface::communicationError, qgcApp(), &QGCApplication::criticalMessageBoxOnMainThread);
138
    }
139

140 141 142 143 144
    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);
145

146 147
    connect(link, &LinkInterface::connected, this, &LinkManager::_linkConnected);
    connect(link, &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected);
148
}
pixhawk's avatar
pixhawk committed
149 150 151

bool LinkManager::connectAll()
{
152 153 154
    if (_connectionsSuspendedMsg()) {
        return false;
    }
155

156 157
    bool allConnected = true;

158 159 160
    foreach (SharedLinkInterface sharedLink, _links) {
        Q_ASSERT(sharedLink.data());
        if (!sharedLink.data()->_connect()) {
161 162
            allConnected = false;
        }
163 164 165
    }

    return allConnected;
pixhawk's avatar
pixhawk committed
166 167 168 169
}

bool LinkManager::disconnectAll()
{
170 171
    bool allDisconnected = true;

172 173 174 175
    // Make a copy so the list is modified out from under us
    QList<SharedLinkInterface> links = _links;

    foreach (SharedLinkInterface sharedLink, links) {
176
        Q_ASSERT(sharedLink.data());
177
        if (!disconnectLink(sharedLink.data())) {
178 179
            allDisconnected = false;
        }
180 181 182
    }

    return allDisconnected;
pixhawk's avatar
pixhawk committed
183 184 185 186
}

bool LinkManager::connectLink(LinkInterface* link)
{
187
    Q_ASSERT(link);
188

189 190 191 192
    if (_connectionsSuspendedMsg()) {
        return false;
    }

193 194 195 196 197
    if (link->_connect()) {
        return true;
    } else {
        return false;
    }
pixhawk's avatar
pixhawk committed
198 199 200 201
}

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

215
void LinkManager::_deleteLink(LinkInterface* link)
216
{
217
    Q_ASSERT(link);
218

219
    _linkListMutex.lock();
220 221 222
    
    // Free up the mavlink channel associated with this link
    _mavlinkChannelsUsedBitMask &= ~(1 << link->getMavlinkChannel());
223

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

235
    _linkListMutex.unlock();
236

Don Gagne's avatar
Don Gagne committed
237
    // Emit removal of link
238
    emit linkDeleted(link);
pixhawk's avatar
pixhawk committed
239 240 241 242 243 244 245
}

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

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

void LinkManager::setConnectionsSuspended(QString reason)
{
    _connectionsSuspended = true;
    _connectionsSuspendedReason = reason;
    Q_ASSERT(!reason.isEmpty());
}
274 275 276

void LinkManager::_shutdown(void)
{
277 278
    while (_links.count() != 0) {
        disconnectLink(_links[0].data());
279 280
    }
}
281 282 283 284 285 286 287 288 289 290

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

void LinkManager::_linkDisconnected(void)
{
    emit linkDisconnected((LinkInterface*)sender());
}
291 292 293 294 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 335 336 337 338 339 340 341 342 343 344 345 346 347 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

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());
    QString root(LinkConfiguration::settingsRoot());
    settings.setValue(root + "/count", _linkConfigurations.count());
    int index = 0;
    foreach (LinkConfiguration* pLink, _linkConfigurations) {
        Q_ASSERT(pLink != NULL);
        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);
    }
    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;
374 375 376 377
                                case LinkConfiguration::TypeTcp:
                                    pLink = (LinkConfiguration*)new TCPConfiguration(name);
                                    pLink->setPreferred(preferred);
                                    break;
378
#ifdef UNITTEST_BUILD
379 380 381 382
                                case LinkConfiguration::TypeMock:
                                    pLink = (LinkConfiguration*)new MockConfiguration(name);
                                    pLink->setPreferred(false);
                                    break;
383
#endif
384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443
                            }
                            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();
    }
    // 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;
    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
        // Is this a PX4?
        if (portInfo.vendorIdentifier() == 9900) {
444
            SerialConfiguration* pSerial = _findSerialConfiguration(portInfo.systemLocation());
445 446 447 448 449 450 451 452 453 454 455
            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()));
                pSerial->setPreferred(true);
                pSerial->setBaud(115200);
456
                pSerial->setPortName(portInfo.systemLocation());
457 458 459 460 461 462 463 464 465 466 467
                addLinkConfiguration(pSerial);
                saveList = true;
            }
        }
    }
    // Save configuration list, which will also trigger a signal for the UI
    if(saveList) {
        saveLinkConfigurationList();
    }
}

468 469 470 471 472 473 474 475 476 477 478 479 480 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
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;
}