LinkManager.cc 14.8 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
    , _nullSharedLink(NULL)
pixhawk's avatar
pixhawk committed
56
{
57 58
    connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateConfigurationList);
    _portListTimer.start(1000);
pixhawk's avatar
pixhawk committed
59 60 61 62
}

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

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

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

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

114
    _linkListMutex.lock();
115

116 117
    if (!containsLink(link)) {
        _links.append(QSharedPointer<LinkInterface>(link));
118
        _linkListMutex.unlock();
119
        emit newLink(link);
120
    } else {
121
        _linkListMutex.unlock();
122
    }
123

124 125
    // MainWindow may be around when doing things like running unit tests
    if (MainWindow::instance()) {
126
        connect(link, &LinkInterface::communicationError, qgcApp(), &QGCApplication::criticalMessageBoxOnMainThread);
127
    }
128

129 130 131 132 133
    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);
134

135 136
    connect(link, &LinkInterface::connected, this, &LinkManager::_linkConnected);
    connect(link, &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected);
137
}
pixhawk's avatar
pixhawk committed
138 139 140

bool LinkManager::connectAll()
{
141 142 143
    if (_connectionsSuspendedMsg()) {
        return false;
    }
144

145 146
    bool allConnected = true;

147 148 149
    foreach (SharedLinkInterface sharedLink, _links) {
        Q_ASSERT(sharedLink.data());
        if (!sharedLink.data()->_connect()) {
150 151
            allConnected = false;
        }
152 153 154
    }

    return allConnected;
pixhawk's avatar
pixhawk committed
155 156 157 158
}

bool LinkManager::disconnectAll()
{
159 160
    bool allDisconnected = true;

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

    return allDisconnected;
pixhawk's avatar
pixhawk committed
169 170 171 172
}

bool LinkManager::connectLink(LinkInterface* link)
{
173
    Q_ASSERT(link);
174

175 176 177 178
    if (_connectionsSuspendedMsg()) {
        return false;
    }

179 180 181 182 183
    if (link->_connect()) {
        return true;
    } else {
        return false;
    }
pixhawk's avatar
pixhawk committed
184 185 186 187
}

bool LinkManager::disconnectLink(LinkInterface* link)
{
188
    Q_ASSERT(link);
189
    if (link->_disconnect()) {
190 191 192 193
        LinkConfiguration* config = link->getLinkConfiguration();
        if(config) {
            config->setLink(NULL);
        }
194
        _deleteLink(link);
195 196 197 198
        return true;
    } else {
        return false;
    }
pixhawk's avatar
pixhawk committed
199 200
}

201
void LinkManager::_deleteLink(LinkInterface* link)
202
{
203
    Q_ASSERT(link);
204

205
    _linkListMutex.lock();
206

207 208 209 210 211 212 213 214
    bool found = false;
    for (int i=0; i<_links.count(); i++) {
        if (_links[i].data() == link) {
            _links.removeAt(i);
            found = true;
            break;
        }
    }
215
    Q_UNUSED(found);
216
    Q_ASSERT(found);
217

218
    _linkListMutex.unlock();
219

Don Gagne's avatar
Don Gagne committed
220
    // Emit removal of link
221
    emit linkDeleted(link);
pixhawk's avatar
pixhawk committed
222 223 224 225 226 227 228
}

/**
 *
 */
const QList<LinkInterface*> LinkManager::getLinks()
{
229 230 231 232
    QList<LinkInterface*> list;
    
    foreach (SharedLinkInterface sharedLink, _links) {
        list << sharedLink.data();
233
    }
234 235
    
    return list;
236
}
237 238 239 240 241 242

/// @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
243 244
        QGCMessageBox::information(tr("Connect not allowed"),
                                   tr("Connect not allowed: %1").arg(_connectionsSuspendedReason));
245 246 247 248 249 250 251 252 253 254 255 256
        return true;
    } else {
        return false;
    }
}

void LinkManager::setConnectionsSuspended(QString reason)
{
    _connectionsSuspended = true;
    _connectionsSuspendedReason = reason;
    Q_ASSERT(!reason.isEmpty());
}
257 258 259

void LinkManager::_shutdown(void)
{
260 261
    while (_links.count() != 0) {
        disconnectLink(_links[0].data());
262 263
    }
}
264 265 266 267 268 269 270 271 272 273

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

void LinkManager::_linkDisconnected(void)
{
    emit linkDisconnected((LinkInterface*)sender());
}
274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 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

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;
357 358 359 360
                                case LinkConfiguration::TypeTcp:
                                    pLink = (LinkConfiguration*)new TCPConfiguration(name);
                                    pLink->setPreferred(preferred);
                                    break;
361
#ifdef UNITTEST_BUILD
362 363 364 365
                                case LinkConfiguration::TypeMock:
                                    pLink = (LinkConfiguration*)new MockConfiguration(name);
                                    pLink->setPreferred(false);
                                    break;
366
#endif
367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 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
                            }
                            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) {
427
            SerialConfiguration* pSerial = _findSerialConfiguration(portInfo.systemLocation());
428 429 430 431 432 433 434 435 436 437 438
            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);
439
                pSerial->setPortName(portInfo.systemLocation());
440 441 442 443 444 445 446 447 448 449 450
                addLinkConfiguration(pSerial);
                saveList = true;
            }
        }
    }
    // Save configuration list, which will also trigger a signal for the UI
    if(saveList) {
        saveLinkConfigurationList();
    }
}

451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490
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;
}