LinkManager.cc 35.4 KB
Newer Older
1 2 3 4 5 6 7 8
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
lm's avatar
lm committed
9

pixhawk's avatar
pixhawk committed
10 11
#include <QList>
#include <QApplication>
12
#include <QDebug>
13
#include <QSignalSpy>
dogmaphobic's avatar
dogmaphobic committed
14

Gus Grubba's avatar
Gus Grubba committed
15
#ifndef NO_SERIAL_LINK
Don Gagne's avatar
Don Gagne committed
16
#include "QGCSerialPortInfo.h"
dogmaphobic's avatar
dogmaphobic committed
17
#endif
18

19
#include "LinkManager.h"
20
#include "QGCApplication.h"
Don Gagne's avatar
Don Gagne committed
21 22
#include "UDPLink.h"
#include "TCPLink.h"
23
#include "SettingsManager.h"
dogmaphobic's avatar
dogmaphobic committed
24
#ifdef QGC_ENABLE_BLUETOOTH
dogmaphobic's avatar
dogmaphobic committed
25 26
#include "BluetoothLink.h"
#endif
27

Don Gagne's avatar
Don Gagne committed
28
#ifndef __mobile__
DonLakeFlyer's avatar
DonLakeFlyer committed
29
#include "GPSManager.h"
30
#include "PositionManager.h"
Don Gagne's avatar
Don Gagne committed
31 32
#endif

Don Gagne's avatar
Don Gagne committed
33
QGC_LOGGING_CATEGORY(LinkManagerLog, "LinkManagerLog")
Don Gagne's avatar
Don Gagne committed
34 35
QGC_LOGGING_CATEGORY(LinkManagerVerboseLog, "LinkManagerVerboseLog")

Don Gagne's avatar
Don Gagne committed
36
const char* LinkManager::_defaultUPDLinkName =       "UDP Link (AutoConnect)";
37

38 39 40 41 42 43 44 45
const int LinkManager::_autoconnectUpdateTimerMSecs =   1000;
#ifdef Q_OS_WIN
// Have to manually let the bootloader go by on Windows to get a working connect
const int LinkManager::_autoconnectConnectDelayMSecs =  6000;
#else
const int LinkManager::_autoconnectConnectDelayMSecs =  1000;
#endif

46 47
LinkManager::LinkManager(QGCApplication* app, QGCToolbox* toolbox)
    : QGCTool(app, toolbox)
48 49 50
    , _configUpdateSuspended(false)
    , _configurationsLoaded(false)
    , _connectionsSuspended(false)
51
    , _mavlinkChannelsUsedBitMask(1)    // We never use channel 0 to avoid sequence numbering problems
52
    , _autoConnectSettings(NULL)
53
    , _mavlinkProtocol(NULL)
54
#ifndef __mobile__
55
    , _nmeaPort(NULL)
56
#endif
pixhawk's avatar
pixhawk committed
57
{
Don Gagne's avatar
Don Gagne committed
58 59 60 61
    qmlRegisterUncreatableType<LinkManager>         ("QGroundControl", 1, 0, "LinkManager",         "Reference only");
    qmlRegisterUncreatableType<LinkConfiguration>   ("QGroundControl", 1, 0, "LinkConfiguration",   "Reference only");
    qmlRegisterUncreatableType<LinkInterface>       ("QGroundControl", 1, 0, "LinkInterface",       "Reference only");

Gus Grubba's avatar
Gus Grubba committed
62
#ifndef NO_SERIAL_LINK
Don Gagne's avatar
Don Gagne committed
63 64 65
    _activeLinkCheckTimer.setInterval(_activeLinkCheckTimeoutMSecs);
    _activeLinkCheckTimer.setSingleShot(false);
    connect(&_activeLinkCheckTimer, &QTimer::timeout, this, &LinkManager::_activeLinkCheck);
Don Gagne's avatar
Don Gagne committed
66
#endif
pixhawk's avatar
pixhawk committed
67 68 69 70
}

LinkManager::~LinkManager()
{
71
#ifndef __mobile__
72
    delete _nmeaPort;
73
#endif
pixhawk's avatar
pixhawk committed
74 75
}

76 77
void LinkManager::setToolbox(QGCToolbox *toolbox)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
78
    QGCTool::setToolbox(toolbox);
79

DonLakeFlyer's avatar
DonLakeFlyer committed
80 81
    _autoConnectSettings = toolbox->settingsManager()->autoConnectSettings();
    _mavlinkProtocol = _toolbox->mavlinkProtocol();
82

83
    connect(_mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &LinkManager::_mavlinkMessageReceived);
84

Don Gagne's avatar
Don Gagne committed
85
    connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateAutoConnectLinks);
86
    _portListTimer.start(_autoconnectUpdateTimerMSecs); // timeout must be long enough to get past bootloader on second pass
87

88 89
}

Don Gagne's avatar
Don Gagne committed
90 91 92 93 94 95 96 97 98 99
// This should only be used by Qml code
void LinkManager::createConnectedLink(LinkConfiguration* config)
{
    for(int i = 0; i < _sharedConfigurations.count(); i++) {
        SharedLinkConfigurationPointer& sharedConf = _sharedConfigurations[i];
        if (sharedConf->name() == config->name())
            createConnectedLink(sharedConf);
    }
}

100
LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& config, bool isPX4Flow)
101
{
102 103 104 105 106
    if (!config) {
        qWarning() << "LinkManager::createConnectedLink called with NULL config";
        return NULL;
    }

107 108
    LinkInterface* pLink = NULL;
    switch(config->type()) {
Gus Grubba's avatar
Gus Grubba committed
109
#ifndef NO_SERIAL_LINK
DonLakeFlyer's avatar
DonLakeFlyer committed
110 111 112 113
    case LinkConfiguration::TypeSerial:
    {
        SerialConfiguration* serialConfig = dynamic_cast<SerialConfiguration*>(config.data());
        if (serialConfig) {
114
            pLink = new SerialLink(config, isPX4Flow);
DonLakeFlyer's avatar
DonLakeFlyer committed
115 116 117 118
            if (serialConfig->usbDirect()) {
                _activeLinkCheckList.append((SerialLink*)pLink);
                if (!_activeLinkCheckTimer.isActive()) {
                    _activeLinkCheckTimer.start();
Don Gagne's avatar
Don Gagne committed
119 120 121
                }
            }
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
122
    }
Don Gagne's avatar
Don Gagne committed
123
        break;
DonLakeFlyer's avatar
DonLakeFlyer committed
124 125
#else
    Q_UNUSED(isPX4Flow)
dogmaphobic's avatar
dogmaphobic committed
126
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
127 128 129 130 131 132
    case LinkConfiguration::TypeUdp:
        pLink = new UDPLink(config);
        break;
    case LinkConfiguration::TypeTcp:
        pLink = new TCPLink(config);
        break;
dogmaphobic's avatar
dogmaphobic committed
133
#ifdef QGC_ENABLE_BLUETOOTH
DonLakeFlyer's avatar
DonLakeFlyer committed
134 135 136
    case LinkConfiguration::TypeBluetooth:
        pLink = new BluetoothLink(config);
        break;
dogmaphobic's avatar
dogmaphobic committed
137
#endif
dogmaphobic's avatar
dogmaphobic committed
138
#ifndef __mobile__
DonLakeFlyer's avatar
DonLakeFlyer committed
139 140 141
    case LinkConfiguration::TypeLogReplay:
        pLink = new LogReplayLink(config);
        break;
dogmaphobic's avatar
dogmaphobic committed
142
#endif
143
#ifdef QT_DEBUG
DonLakeFlyer's avatar
DonLakeFlyer committed
144 145 146
    case LinkConfiguration::TypeMock:
        pLink = new MockLink(config);
        break;
147
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
148 149 150
    case LinkConfiguration::TypeLast:
    default:
        break;
151
    }
152 153

    if (pLink) {
154 155
        _addLink(pLink);
        connectLink(pLink);
156
    }
157

158 159 160
    return pLink;
}

Don Gagne's avatar
Don Gagne committed
161
LinkInterface* LinkManager::createConnectedLink(const QString& name)
162
{
DonLakeFlyer's avatar
DonLakeFlyer committed
163 164 165 166 167 168 169 170 171
    if (name.isEmpty()) {
        qWarning() << "Internal error";
    } else {
        for(int i = 0; i < _sharedConfigurations.count(); i++) {
            SharedLinkConfigurationPointer& conf = _sharedConfigurations[i];
            if (conf->name() == name) {
                return createConnectedLink(conf);
            }
        }
172 173 174 175
    }
    return NULL;
}

176
void LinkManager::_addLink(LinkInterface* link)
pixhawk's avatar
pixhawk committed
177
{
Don Gagne's avatar
Don Gagne committed
178
    if (thread() != QThread::currentThread()) {
179
        qWarning() << "_addLink called from incorrect thread";
Don Gagne's avatar
Don Gagne committed
180 181
        return;
    }
182

Don Gagne's avatar
Don Gagne committed
183 184 185
    if (!link) {
        return;
    }
186

187
    if (!containsLink(link)) {
188 189 190 191
        int mavlinkChannel = _reserveMavlinkChannel();
        if (mavlinkChannel != 0) {
            link->_setMavlinkChannel(mavlinkChannel);
        } else {
192
            qWarning() << "Ran out of mavlink channels";
193
            return;
194 195
        }

196
        _sharedLinks.append(SharedLinkInterfacePointer(link));
197 198
        emit newLink(link);
    }
199

Don Gagne's avatar
Don Gagne committed
200 201
    connect(link, &LinkInterface::communicationError,   _app,               &QGCApplication::criticalMessageBoxOnMainThread);
    connect(link, &LinkInterface::bytesReceived,        _mavlinkProtocol,   &MAVLinkProtocol::receiveBytes);
202

203
    _mavlinkProtocol->resetMetadataForLink(link);
204
    _mavlinkProtocol->setVersion(_mavlinkProtocol->getCurrentVersion());
205

206 207 208 209 210 211
    connect(link, &LinkInterface::connected,            this, &LinkManager::_linkConnected);
    connect(link, &LinkInterface::disconnected,         this, &LinkManager::_linkDisconnected);

    // This connection is queued since it will cloe the link. So we want the link emitter to return otherwise we would
    // close the link our from under itself.
    connect(link, &LinkInterface::connectionRemoved,    this, &LinkManager::_linkConnectionRemoved, Qt::QueuedConnection);
212
}
pixhawk's avatar
pixhawk committed
213

Don Gagne's avatar
Don Gagne committed
214
void LinkManager::disconnectAll(void)
pixhawk's avatar
pixhawk committed
215
{
Don Gagne's avatar
Don Gagne committed
216
    // Walk list in reverse order to preserve indices during delete
217 218
    for (int i=_sharedLinks.count()-1; i>=0; i--) {
        disconnectLink(_sharedLinks[i].data());
219
    }
pixhawk's avatar
pixhawk committed
220 221 222 223
}

bool LinkManager::connectLink(LinkInterface* link)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
224 225 226 227 228 229 230
    if (link) {
        if (_connectionsSuspendedMsg()) {
            return false;
        }
        return link->_connect();
    } else {
        qWarning() << "Internal error";
231 232
        return false;
    }
pixhawk's avatar
pixhawk committed
233 234
}

Don Gagne's avatar
Don Gagne committed
235
void LinkManager::disconnectLink(LinkInterface* link)
pixhawk's avatar
pixhawk committed
236
{
237
    if (!link || !containsLink(link)) {
238 239
        return;
    }
Don Gagne's avatar
Don Gagne committed
240

Don Gagne's avatar
Don Gagne committed
241
    link->_disconnect();
242

Don Gagne's avatar
Don Gagne committed
243
    LinkConfiguration* config = link->getLinkConfiguration();
244 245 246 247 248
    for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) {
        if (_sharedAutoconnectConfigurations[i].data() == config) {
            qCDebug(LinkManagerLog) << "Removing disconnected autoconnect config" << config->name();
            _sharedAutoconnectConfigurations.removeAt(i);
            break;
249
        }
250
    }
251

Don Gagne's avatar
Don Gagne committed
252
    _deleteLink(link);
pixhawk's avatar
pixhawk committed
253 254
}

255
void LinkManager::_deleteLink(LinkInterface* link)
256
{
Don Gagne's avatar
Don Gagne committed
257 258 259 260 261 262 263 264
    if (thread() != QThread::currentThread()) {
        qWarning() << "_deleteLink called from incorrect thread";
        return;
    }

    if (!link) {
        return;
    }
265

266
    // Free up the mavlink channel associated with this link
267
    _freeMavlinkChannel(link->mavlinkChannel());
268

269 270 271 272 273 274
    for (int i=0; i<_sharedLinks.count(); i++) {
        if (_sharedLinks[i].data() == link) {
            _sharedLinks.removeAt(i);
            break;
        }
    }
275

Don Gagne's avatar
Don Gagne committed
276
    // Emit removal of link
277
    emit linkDeleted(link);
pixhawk's avatar
pixhawk committed
278 279
}

280 281 282 283 284 285 286 287 288 289 290 291
SharedLinkInterfacePointer LinkManager::sharedLinkInterfacePointerForLink(LinkInterface* link)
{
    for (int i=0; i<_sharedLinks.count(); i++) {
        if (_sharedLinks[i].data() == link) {
            return _sharedLinks[i];
        }
    }

    qWarning() << "LinkManager::sharedLinkInterfaceForLink returning NULL";
    return SharedLinkInterfacePointer(NULL);
}

292 293 294 295 296
/// @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) {
297
        qgcApp()->showMessage(tr("Connect not allowed: %1").arg(_connectionsSuspendedReason));
298 299 300 301 302 303 304 305 306 307 308
        return true;
    } else {
        return false;
    }
}

void LinkManager::setConnectionsSuspended(QString reason)
{
    _connectionsSuspended = true;
    _connectionsSuspendedReason = reason;
}
309

310 311 312 313 314 315 316 317 318
void LinkManager::_linkConnected(void)
{
    emit linkConnected((LinkInterface*)sender());
}

void LinkManager::_linkDisconnected(void)
{
    emit linkDisconnected((LinkInterface*)sender());
}
319

320 321 322 323 324 325
void LinkManager::_linkConnectionRemoved(LinkInterface* link)
{
    // Link has been removed from system, disconnect it automatically
    disconnectLink(link);
}

326 327 328 329 330 331 332 333 334
void LinkManager::suspendConfigurationUpdates(bool suspend)
{
    _configUpdateSuspended = suspend;
}

void LinkManager::saveLinkConfigurationList()
{
    QSettings settings;
    settings.remove(LinkConfiguration::settingsRoot());
335
    int trueCount = 0;
336 337
    for (int i = 0; i < _sharedConfigurations.count(); i++) {
        SharedLinkConfigurationPointer linkConfig = _sharedConfigurations[i];
Don Gagne's avatar
Don Gagne committed
338
        if (linkConfig) {
339
            if (!linkConfig->isDynamic()) {
Don Gagne's avatar
Don Gagne committed
340
                QString root = LinkConfiguration::settingsRoot();
341
                root += QString("/Link%1").arg(trueCount++);
Don Gagne's avatar
Don Gagne committed
342 343
                settings.setValue(root + "/name", linkConfig->name());
                settings.setValue(root + "/type", linkConfig->type());
344
                settings.setValue(root + "/auto", linkConfig->isAutoConnect());
345
                settings.setValue(root + "/high_latency", linkConfig->isHighLatency());
Don Gagne's avatar
Don Gagne committed
346 347 348 349
                // Have the instance save its own values
                linkConfig->saveSettings(settings, root);
            }
        } else {
350
            qWarning() << "Internal error for link configuration in LinkManager";
dogmaphobic's avatar
dogmaphobic committed
351
        }
352
    }
dogmaphobic's avatar
dogmaphobic committed
353
    QString root(LinkConfiguration::settingsRoot());
354 355
    settings.setValue(root + "/count", trueCount);
    emit linkConfigurationsChanged();
356 357 358 359
}

void LinkManager::loadLinkConfigurationList()
{
360
    bool linksChanged = false;
361 362 363 364 365 366 367 368 369 370
    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();
371
                if((LinkConfiguration::LinkType)type < LinkConfiguration::TypeLast) {
372 373 374 375
                    if(settings.contains(root + "/name")) {
                        QString name = settings.value(root + "/name").toString();
                        if(!name.isEmpty()) {
                            LinkConfiguration* pLink = NULL;
376
                            bool autoConnect = settings.value(root + "/auto").toBool();
377
                            bool highLatency = settings.value(root + "/high_latency").toBool();
378
                            switch((LinkConfiguration::LinkType)type) {
Gus Grubba's avatar
Gus Grubba committed
379
#ifndef NO_SERIAL_LINK
DonLakeFlyer's avatar
DonLakeFlyer committed
380 381 382
                            case LinkConfiguration::TypeSerial:
                                pLink = (LinkConfiguration*)new SerialConfiguration(name);
                                break;
dogmaphobic's avatar
dogmaphobic committed
383
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
384 385 386 387 388 389
                            case LinkConfiguration::TypeUdp:
                                pLink = (LinkConfiguration*)new UDPConfiguration(name);
                                break;
                            case LinkConfiguration::TypeTcp:
                                pLink = (LinkConfiguration*)new TCPConfiguration(name);
                                break;
dogmaphobic's avatar
dogmaphobic committed
390
#ifdef QGC_ENABLE_BLUETOOTH
DonLakeFlyer's avatar
DonLakeFlyer committed
391 392 393
                            case LinkConfiguration::TypeBluetooth:
                                pLink = (LinkConfiguration*)new BluetoothConfiguration(name);
                                break;
dogmaphobic's avatar
dogmaphobic committed
394
#endif
dogmaphobic's avatar
dogmaphobic committed
395
#ifndef __mobile__
DonLakeFlyer's avatar
DonLakeFlyer committed
396 397 398
                            case LinkConfiguration::TypeLogReplay:
                                pLink = (LinkConfiguration*)new LogReplayLinkConfiguration(name);
                                break;
dogmaphobic's avatar
dogmaphobic committed
399
#endif
400
#ifdef QT_DEBUG
DonLakeFlyer's avatar
DonLakeFlyer committed
401 402 403
                            case LinkConfiguration::TypeMock:
                                pLink = (LinkConfiguration*)new MockConfiguration(name);
                                break;
404
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
405 406 407
                            default:
                            case LinkConfiguration::TypeLast:
                                break;
408 409
                            }
                            if(pLink) {
410 411
                                //-- Have the instance load its own values
                                pLink->setAutoConnect(autoConnect);
412
                                pLink->setHighLatency(highLatency);
413
                                pLink->loadSettings(settings, root);
414
                                addConfiguration(pLink);
415
                                linksChanged = true;
416 417
                            }
                        } else {
418
                            qWarning() << "Link Configuration" << root << "has an empty name." ;
419 420
                        }
                    } else {
421
                        qWarning() << "Link Configuration" << root << "has no name." ;
422 423
                    }
                } else {
424
                    qWarning() << "Link Configuration" << root << "an invalid type: " << type;
425 426
                }
            } else {
427
                qWarning() << "Link Configuration" << root << "has no type." ;
428 429 430
            }
        }
    }
431 432

    if(linksChanged) {
433
        emit linkConfigurationsChanged();
434 435
    }
    // Enable automatic Serial PX4/3DR Radio hunting
436 437 438
    _configurationsLoaded = true;
}

Gus Grubba's avatar
Gus Grubba committed
439
#ifndef NO_SERIAL_LINK
Don Gagne's avatar
Don Gagne committed
440
SerialConfiguration* LinkManager::_autoconnectConfigurationsContainsPort(const QString& portName)
441 442
{
    QString searchPort = portName.trimmed();
Don Gagne's avatar
Don Gagne committed
443

444 445
    for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) {
        SerialConfiguration* serialConfig = qobject_cast<SerialConfiguration*>(_sharedAutoconnectConfigurations[i].data());
Don Gagne's avatar
Don Gagne committed
446

447 448 449
        if (serialConfig) {
            if (serialConfig->portName() == searchPort) {
                return serialConfig;
450
            }
Don Gagne's avatar
Don Gagne committed
451 452
        } else {
            qWarning() << "Internal error";
453 454 455 456
        }
    }
    return NULL;
}
dogmaphobic's avatar
dogmaphobic committed
457
#endif
458

Don Gagne's avatar
Don Gagne committed
459
void LinkManager::_updateAutoConnectLinks(void)
460
{
Don Gagne's avatar
Don Gagne committed
461
    if (_connectionsSuspended || qgcApp()->runningUnitTests()) {
462 463
        return;
    }
Don Gagne's avatar
Don Gagne committed
464

Don Gagne's avatar
Don Gagne committed
465 466
    // Re-add UDP if we need to
    bool foundUDP = false;
467 468
    for (int i=0; i<_sharedLinks.count(); i++) {
        LinkConfiguration* linkConfig = _sharedLinks[i]->getLinkConfiguration();
Don Gagne's avatar
Don Gagne committed
469 470 471 472 473
        if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _defaultUPDLinkName) {
            foundUDP = true;
            break;
        }
    }
474
    if (!foundUDP && _autoConnectSettings->autoConnectUDP()->rawValue().toBool()) {
Don Gagne's avatar
Don Gagne committed
475
        qCDebug(LinkManagerLog) << "New auto-connect UDP port added";
476
        // Default UDPConfiguration is set up for autoconnect
Don Gagne's avatar
Don Gagne committed
477
        UDPConfiguration* udpConfig = new UDPConfiguration(_defaultUPDLinkName);
DonLakeFlyer's avatar
DonLakeFlyer committed
478
        udpConfig->setDynamic(true);
479 480
        SharedLinkConfigurationPointer config = addConfiguration(udpConfig);
        createConnectedLink(config);
481
        emit linkConfigurationsChanged();
Don Gagne's avatar
Don Gagne committed
482 483
    }

Gus Grubba's avatar
Gus Grubba committed
484
#ifndef NO_SERIAL_LINK
dogmaphobic's avatar
dogmaphobic committed
485
    QStringList currentPorts;
486 487 488 489 490 491
    QList<QGCSerialPortInfo> portList;

#ifdef __android__
    // Android builds only support a single serial connection. Repeatedly calling availablePorts after that one serial
    // port is connected leaks file handles due to a bug somewhere in android serial code. In order to work around that
    // bug after we connect the first serial port we stop probing for additional ports.
492
    if (!_sharedAutoconnectConfigurations.count()) {
493 494
        portList = QGCSerialPortInfo::availablePorts();
    }
495 496
#else
    portList = QGCSerialPortInfo::availablePorts();
497
#endif
Don Gagne's avatar
Don Gagne committed
498

499
    // Iterate Comm Ports
Don Gagne's avatar
Don Gagne committed
500
    foreach (QGCSerialPortInfo portInfo, portList) {
Don Gagne's avatar
Don Gagne committed
501 502 503 504 505 506 507 508 509
        qCDebug(LinkManagerVerboseLog) << "-----------------------------------------------------";
        qCDebug(LinkManagerVerboseLog) << "portName:          " << portInfo.portName();
        qCDebug(LinkManagerVerboseLog) << "systemLocation:    " << portInfo.systemLocation();
        qCDebug(LinkManagerVerboseLog) << "description:       " << portInfo.description();
        qCDebug(LinkManagerVerboseLog) << "manufacturer:      " << portInfo.manufacturer();
        qCDebug(LinkManagerVerboseLog) << "serialNumber:      " << portInfo.serialNumber();
        qCDebug(LinkManagerVerboseLog) << "vendorIdentifier:  " << portInfo.vendorIdentifier();
        qCDebug(LinkManagerVerboseLog) << "productIdentifier: " << portInfo.productIdentifier();

dogmaphobic's avatar
dogmaphobic committed
510 511
        // Save port name
        currentPorts << portInfo.systemLocation();
Don Gagne's avatar
Don Gagne committed
512

513 514
        QGCSerialPortInfo::BoardType_t boardType;
        QString boardName;
Don Gagne's avatar
Don Gagne committed
515

516
#ifndef __mobile__
517 518 519 520 521 522 523 524 525 526 527 528
        if (portInfo.systemLocation().trimmed() == _autoConnectSettings->autoConnectNmeaPort()->cookedValueString()) {
            if (portInfo.systemLocation().trimmed() != _nmeaDeviceName) {
                _nmeaDeviceName = portInfo.systemLocation().trimmed();
                qCDebug(LinkManagerLog) << "Configuring nmea port" << _nmeaDeviceName;
                QSerialPort* newPort = new QSerialPort(portInfo);

                _nmeaBaud = _autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt();
                newPort->setBaudRate(_nmeaBaud);
                qCDebug(LinkManagerLog) << "Configuring nmea baudrate" << _nmeaBaud;

                // This will stop polling old device if previously set
                _toolbox->qgcPositionManager()->setNmeaSourceDevice(newPort);
529

530 531 532 533 534 535 536 537 538 539
                if (_nmeaPort) {
                    delete _nmeaPort;
                }
                _nmeaPort = newPort;

            } else if (_autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt() != _nmeaBaud) {
                _nmeaBaud = _autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt();
                _nmeaPort->setBaudRate(_nmeaBaud);
                qCDebug(LinkManagerLog) << "Configuring nmea baudrate" << _nmeaBaud;
            }
540 541 542
        } else
#endif
        if (portInfo.getBoardInfo(boardType, boardName)) {
Don Gagne's avatar
Don Gagne committed
543 544
            if (portInfo.isBootloader()) {
                // Don't connect to bootloader
545
                qCDebug(LinkManagerLog) << "Waiting for bootloader to finish" << portInfo.systemLocation();
Don Gagne's avatar
Don Gagne committed
546 547
                continue;
            }
548

549
            if (_autoconnectConfigurationsContainsPort(portInfo.systemLocation()) || _autoConnectRTKPort == portInfo.systemLocation()) {
550 551 552 553 554 555
                qCDebug(LinkManagerVerboseLog) << "Skipping existing autoconnect" << portInfo.systemLocation();
            } else if (!_autoconnectWaitList.contains(portInfo.systemLocation())) {
                // We don't connect to the port the first time we see it. The ability to correctly detect whether we
                // are in the bootloader is flaky from a cross-platform standpoint. So by putting it on a wait list
                // and only connect on the second pass we leave enough time for the board to boot up.
                qCDebug(LinkManagerLog) << "Waiting for next autoconnect pass" << portInfo.systemLocation();
556 557
                _autoconnectWaitList[portInfo.systemLocation()] = 1;
            } else if (++_autoconnectWaitList[portInfo.systemLocation()] * _autoconnectUpdateTimerMSecs > _autoconnectConnectDelayMSecs) {
Don Gagne's avatar
Don Gagne committed
558 559
                SerialConfiguration* pSerialConfig = NULL;

560
                _autoconnectWaitList.remove(portInfo.systemLocation());
561

Don Gagne's avatar
Don Gagne committed
562
                switch (boardType) {
563
                case QGCSerialPortInfo::BoardTypePixhawk:
564
                    if (_autoConnectSettings->autoConnectPixhawk()->rawValue().toBool()) {
565
                        pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed()));
566 567 568
                        pSerialConfig->setUsbDirect(true);
                    }
                    break;
Don Gagne's avatar
Don Gagne committed
569
                case QGCSerialPortInfo::BoardTypePX4Flow:
570
                    if (_autoConnectSettings->autoConnectPX4Flow()->rawValue().toBool()) {
571
                        pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed()));
Don Gagne's avatar
Don Gagne committed
572
                    }
Don Gagne's avatar
Don Gagne committed
573
                    break;
574
                case QGCSerialPortInfo::BoardTypeSiKRadio:
575
                    if (_autoConnectSettings->autoConnectSiKRadio()->rawValue().toBool()) {
576
                        pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed()));
Don Gagne's avatar
Don Gagne committed
577 578
                    }
                    break;
579
                case QGCSerialPortInfo::BoardTypeOpenPilot:
580
                    if (_autoConnectSettings->autoConnectLibrePilot()->rawValue().toBool()) {
581
                        pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed()));
582 583
                    }
                    break;
Don Gagne's avatar
Don Gagne committed
584 585
#ifndef __mobile__
                case QGCSerialPortInfo::BoardTypeRTKGPS:
586
                    if (_autoConnectSettings->autoConnectRTKGPS()->rawValue().toBool() && !_toolbox->gpsManager()->connected()) {
587 588
                        qCDebug(LinkManagerLog) << "RTK GPS auto-connected" << portInfo.portName().trimmed();
                        _autoConnectRTKPort = portInfo.systemLocation();
589
                        _toolbox->gpsManager()->connectGPS(portInfo.systemLocation(), boardName);
Don Gagne's avatar
Don Gagne committed
590 591 592
                    }
                    break;
#endif
Don Gagne's avatar
Don Gagne committed
593 594
                default:
                    qWarning() << "Internal error";
Don Gagne's avatar
Don Gagne committed
595
                    continue;
dogmaphobic's avatar
dogmaphobic committed
596
                }
Don Gagne's avatar
Don Gagne committed
597

Don Gagne's avatar
Don Gagne committed
598 599
                if (pSerialConfig) {
                    qCDebug(LinkManagerLog) << "New auto-connect port added: " << pSerialConfig->name() << portInfo.systemLocation();
600
                    pSerialConfig->setBaud(boardType == QGCSerialPortInfo::BoardTypeSiKRadio ? 57600 : 115200);
Don Gagne's avatar
Don Gagne committed
601 602
                    pSerialConfig->setDynamic(true);
                    pSerialConfig->setPortName(portInfo.systemLocation());
603
                    _sharedAutoconnectConfigurations.append(SharedLinkConfigurationPointer(pSerialConfig));
604
                    createConnectedLink(_sharedAutoconnectConfigurations.last(), boardType == QGCSerialPortInfo::BoardTypePX4Flow);
Don Gagne's avatar
Don Gagne committed
605
                }
dogmaphobic's avatar
dogmaphobic committed
606 607 608
            }
        }
    }
Don Gagne's avatar
Don Gagne committed
609

610 611 612 613 614 615 616
#ifndef __android__
    // Android builds only support a single serial connection. Repeatedly calling availablePorts after that one serial
    // port is connected leaks file handles due to a bug somewhere in android serial code. In order to work around that
    // bug after we connect the first serial port we stop probing for additional ports. The means we must rely on
    // the port disconnecting itself when the radio is pulled to signal communication list as opposed to automatically
    // closing the Link.

dogmaphobic's avatar
dogmaphobic committed
617 618
    // Now we go through the current configuration list and make sure any dynamic config has gone away
    QList<LinkConfiguration*>  _confToDelete;
619 620 621 622 623 624 625
    for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) {
        SerialConfiguration* serialConfig = qobject_cast<SerialConfiguration*>(_sharedAutoconnectConfigurations[i].data());
        if (serialConfig) {
            if (!currentPorts.contains(serialConfig->portName())) {
                if (serialConfig->link()) {
                    if (serialConfig->link()->isConnected()) {
                        if (serialConfig->link()->active()) {
626 627 628 629 630 631
                            // We don't remove links which are still connected which have been active with a vehicle on them
                            // even though at this point the cable may have been pulled. Instead we wait for the user to
                            // Disconnect. Once the user disconnects, the link will be removed.
                            continue;
                        }
                    }
Don Gagne's avatar
Don Gagne committed
632
                }
633
                _confToDelete.append(serialConfig);
dogmaphobic's avatar
dogmaphobic committed
634
            }
Don Gagne's avatar
Don Gagne committed
635 636
        } else {
            qWarning() << "Internal error";
dogmaphobic's avatar
dogmaphobic committed
637 638
        }
    }
Don Gagne's avatar
Don Gagne committed
639

Don Gagne's avatar
Don Gagne committed
640
    // Now remove all configs that are gone
Don Gagne's avatar
Don Gagne committed
641
    foreach (LinkConfiguration* pDeleteConfig, _confToDelete) {
Don Gagne's avatar
Don Gagne committed
642
        qCDebug(LinkManagerLog) << "Removing unused autoconnect config" << pDeleteConfig->name();
Don Gagne's avatar
Don Gagne committed
643 644 645
        if (pDeleteConfig->link()) {
            disconnectLink(pDeleteConfig->link());
        }
646 647 648 649 650 651
        for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) {
            if (_sharedAutoconnectConfigurations[i].data() == pDeleteConfig) {
                _sharedAutoconnectConfigurations.removeAt(i);
                break;
            }
        }
652
    }
653 654

    // Check for RTK GPS connection gone
655
#if !defined(__mobile__)
656 657 658 659 660
    if (!_autoConnectRTKPort.isEmpty() && !currentPorts.contains(_autoConnectRTKPort)) {
        qCDebug(LinkManagerLog) << "RTK GPS disconnected" << _autoConnectRTKPort;
        _toolbox->gpsManager()->disconnectGPS();
        _autoConnectRTKPort.clear();
    }
661
#endif
662

663
#endif
Gus Grubba's avatar
Gus Grubba committed
664
#endif // NO_SERIAL_LINK
665 666
}

Don Gagne's avatar
Don Gagne committed
667 668
void LinkManager::shutdown(void)
{
669
    setConnectionsSuspended(tr("Shutdown"));
Don Gagne's avatar
Don Gagne committed
670
    disconnectAll();
Don Gagne's avatar
Don Gagne committed
671 672
}

673 674 675 676 677 678
QStringList LinkManager::linkTypeStrings(void) const
{
    //-- Must follow same order as enum LinkType in LinkConfiguration.h
    static QStringList list;
    if(!list.size())
    {
Gus Grubba's avatar
Gus Grubba committed
679
#ifndef NO_SERIAL_LINK
680 681 682 683
        list += "Serial";
#endif
        list += "UDP";
        list += "TCP";
dogmaphobic's avatar
dogmaphobic committed
684
#ifdef QGC_ENABLE_BLUETOOTH
dogmaphobic's avatar
dogmaphobic committed
685 686
        list += "Bluetooth";
#endif
dogmaphobic's avatar
dogmaphobic committed
687
#ifdef QT_DEBUG
688
        list += "Mock Link";
dogmaphobic's avatar
dogmaphobic committed
689 690
#endif
#ifndef __mobile__
691
        list += "Log Replay";
dogmaphobic's avatar
dogmaphobic committed
692
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
693 694 695
        if (list.size() != (int)LinkConfiguration::TypeLast) {
            qWarning() << "Internal error";
        }
696 697 698 699
    }
    return list;
}

700
void LinkManager::_updateSerialPorts()
701
{
702 703
    _commPortList.clear();
    _commPortDisplayList.clear();
Gus Grubba's avatar
Gus Grubba committed
704
#ifndef NO_SERIAL_LINK
705 706
    QList<QSerialPortInfo> portList = QSerialPortInfo::availablePorts();
    foreach (const QSerialPortInfo &info, portList)
707
    {
708 709 710
        QString port = info.systemLocation().trimmed();
        _commPortList += port;
        _commPortDisplayList += SerialConfiguration::cleanPortDisplayname(port);
711 712
    }
#endif
713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729
}

QStringList LinkManager::serialPortStrings(void)
{
    if(!_commPortDisplayList.size())
    {
        _updateSerialPorts();
    }
    return _commPortDisplayList;
}

QStringList LinkManager::serialPorts(void)
{
    if(!_commPortList.size())
    {
        _updateSerialPorts();
    }
730 731 732 733 734
    return _commPortList;
}

QStringList LinkManager::serialBaudRates(void)
{
Gus Grubba's avatar
Gus Grubba committed
735
#ifdef NO_SERIAL_LINK
736 737 738 739 740 741
    QStringList foo;
    return foo;
#else
    return SerialConfiguration::supportedBaudRates();
#endif
}
742 743 744

bool LinkManager::endConfigurationEditing(LinkConfiguration* config, LinkConfiguration* editedConfig)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
745 746 747 748 749 750 751 752 753 754 755
    if (config && editedConfig) {
        _fixUnnamed(editedConfig);
        config->copyFrom(editedConfig);
        saveLinkConfigurationList();
        // Tell link about changes (if any)
        config->updateSettings();
        // Discard temporary duplicate
        delete editedConfig;
    } else {
        qWarning() << "Internal error";
    }
756 757 758 759 760
    return true;
}

bool LinkManager::endCreateConfiguration(LinkConfiguration* config)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
761 762 763 764 765 766 767
    if (config) {
        _fixUnnamed(config);
        addConfiguration(config);
        saveLinkConfigurationList();
    } else {
        qWarning() << "Internal error";
    }
768 769 770 771 772
    return true;
}

LinkConfiguration* LinkManager::createConfiguration(int type, const QString& name)
{
Gus Grubba's avatar
Gus Grubba committed
773
#ifndef NO_SERIAL_LINK
774 775
    if((LinkConfiguration::LinkType)type == LinkConfiguration::TypeSerial)
        _updateSerialPorts();
dogmaphobic's avatar
dogmaphobic committed
776
#endif
777 778 779 780 781
    return LinkConfiguration::createSettings(type, name);
}

LinkConfiguration* LinkManager::startConfigurationEditing(LinkConfiguration* config)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
782
    if (config) {
Gus Grubba's avatar
Gus Grubba committed
783
#ifndef NO_SERIAL_LINK
DonLakeFlyer's avatar
DonLakeFlyer committed
784 785
        if(config->type() == LinkConfiguration::TypeSerial)
            _updateSerialPorts();
dogmaphobic's avatar
dogmaphobic committed
786
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
787 788 789 790 791
        return LinkConfiguration::duplicateSettings(config);
    } else {
        qWarning() << "Internal error";
        return NULL;
    }
792 793 794 795 796
}


void LinkManager::_fixUnnamed(LinkConfiguration* config)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
797 798 799 800
    if (config) {
        //-- Check for "Unnamed"
        if (config->name() == "Unnamed") {
            switch(config->type()) {
Gus Grubba's avatar
Gus Grubba committed
801
#ifndef NO_SERIAL_LINK
802 803
            case LinkConfiguration::TypeSerial: {
                QString tname = dynamic_cast<SerialConfiguration*>(config)->portName();
804
#ifdef Q_OS_WIN
805 806 807 808 809 810 811
                tname.replace("\\\\.\\", "");
#else
                tname.replace("/dev/cu.", "");
                tname.replace("/dev/", "");
#endif
                config->setName(QString("Serial Device on %1").arg(tname));
                break;
DonLakeFlyer's avatar
DonLakeFlyer committed
812
            }
813 814 815
#endif
            case LinkConfiguration::TypeUdp:
                config->setName(
DonLakeFlyer's avatar
DonLakeFlyer committed
816
                            QString("UDP Link on Port %1").arg(dynamic_cast<UDPConfiguration*>(config)->localPort()));
817 818
                break;
            case LinkConfiguration::TypeTcp: {
DonLakeFlyer's avatar
DonLakeFlyer committed
819 820 821 822
                TCPConfiguration* tconfig = dynamic_cast<TCPConfiguration*>(config);
                if(tconfig) {
                    config->setName(
                                QString("TCP Link %1:%2").arg(tconfig->address().toString()).arg((int)tconfig->port()));
823
                }
DonLakeFlyer's avatar
DonLakeFlyer committed
824
            }
825
                break;
dogmaphobic's avatar
dogmaphobic committed
826
#ifdef QGC_ENABLE_BLUETOOTH
dogmaphobic's avatar
dogmaphobic committed
827
            case LinkConfiguration::TypeBluetooth: {
DonLakeFlyer's avatar
DonLakeFlyer committed
828 829 830
                BluetoothConfiguration* tconfig = dynamic_cast<BluetoothConfiguration*>(config);
                if(tconfig) {
                    config->setName(QString("%1 (Bluetooth Device)").arg(tconfig->device().name));
dogmaphobic's avatar
dogmaphobic committed
831
                }
DonLakeFlyer's avatar
DonLakeFlyer committed
832
            }
dogmaphobic's avatar
dogmaphobic committed
833 834
                break;
#endif
dogmaphobic's avatar
dogmaphobic committed
835
#ifndef __mobile__
836
            case LinkConfiguration::TypeLogReplay: {
DonLakeFlyer's avatar
DonLakeFlyer committed
837 838 839
                LogReplayLinkConfiguration* tconfig = dynamic_cast<LogReplayLinkConfiguration*>(config);
                if(tconfig) {
                    config->setName(QString("Log Replay %1").arg(tconfig->logFilenameShort()));
840
                }
DonLakeFlyer's avatar
DonLakeFlyer committed
841
            }
842
                break;
dogmaphobic's avatar
dogmaphobic committed
843
#endif
844 845 846
#ifdef QT_DEBUG
            case LinkConfiguration::TypeMock:
                config->setName(
DonLakeFlyer's avatar
DonLakeFlyer committed
847
                            QString("Mock Link"));
848 849 850 851 852
                break;
#endif
            case LinkConfiguration::TypeLast:
            default:
                break;
DonLakeFlyer's avatar
DonLakeFlyer committed
853
            }
854
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
855 856
    } else {
        qWarning() << "Internal error";
857 858 859 860 861
    }
}

void LinkManager::removeConfiguration(LinkConfiguration* config)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
862 863 864 865 866
    if (config) {
        LinkInterface* iface = config->link();
        if(iface) {
            disconnectLink(iface);
        }
867

DonLakeFlyer's avatar
DonLakeFlyer committed
868 869 870 871 872
        _removeConfiguration(config);
        saveLinkConfigurationList();
    } else {
        qWarning() << "Internal error";
    }
873
}
874

875 876
bool LinkManager::isAutoconnectLink(LinkInterface* link)
{
877 878 879 880 881 882
    for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) {
        if (_sharedAutoconnectConfigurations[i].data() == link->getLinkConfiguration()) {
            return true;
        }
    }
    return false;
883
}
dogmaphobic's avatar
dogmaphobic committed
884 885 886 887 888

bool LinkManager::isBluetoothAvailable(void)
{
    return qgcApp()->isBluetoothAvailable();
}
Don Gagne's avatar
Don Gagne committed
889

Gus Grubba's avatar
Gus Grubba committed
890
#ifndef NO_SERIAL_LINK
Don Gagne's avatar
Don Gagne committed
891 892
void LinkManager::_activeLinkCheck(void)
{
893
    SerialLink* link = NULL;
Don Gagne's avatar
Don Gagne committed
894 895 896
    bool found = false;

    if (_activeLinkCheckList.count() != 0) {
897
        link = _activeLinkCheckList.takeFirst();
898
        if (containsLink(link) && link->isConnected()) {
Don Gagne's avatar
Don Gagne committed
899 900 901 902 903 904 905 906 907
            // Make sure there is a vehicle on the link
            QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles();
            for (int i=0; i<vehicles->count(); i++) {
                Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles->get(i));
                if (vehicle->containsLink(link)) {
                    found = true;
                    break;
                }
            }
908 909
        } else {
            link = NULL;
Don Gagne's avatar
Don Gagne committed
910 911 912 913 914 915 916
        }
    }

    if (_activeLinkCheckList.count() == 0) {
        _activeLinkCheckTimer.stop();
    }

917 918 919
    if (!found && link) {
        // See if we can get an NSH prompt on this link
        bool foundNSHPrompt = false;
920
        link->writeBytesSafe("\r", 1);
921 922 923 924 925 926 927 928 929
        QSignalSpy spy(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)));
        if (spy.wait(100)) {
            QList<QVariant> arguments = spy.takeFirst();
            if (arguments[1].value<QByteArray>().contains("nsh>")) {
                foundNSHPrompt = true;
            }
        }

        qgcApp()->showMessage(foundNSHPrompt ?
930 931
                                  tr("Please check to make sure you have an SD Card inserted in your Vehicle and try again.") :
                                  tr("Your Vehicle is not responding. If this continues, shutdown %1, restart the Vehicle letting it boot completely, then start %1.").arg(qgcApp()->applicationName()));
Don Gagne's avatar
Don Gagne committed
932 933
    }
}
Don Gagne's avatar
Don Gagne committed
934
#endif
935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976

bool LinkManager::containsLink(LinkInterface* link)
{
    for (int i=0; i<_sharedLinks.count(); i++) {
        if (_sharedLinks[i].data() == link) {
            return true;
        }
    }
    return false;
}

SharedLinkConfigurationPointer LinkManager::addConfiguration(LinkConfiguration* config)
{
    _qmlConfigurations.append(config);
    _sharedConfigurations.append(SharedLinkConfigurationPointer(config));
    return _sharedConfigurations.last();
}

void LinkManager::_removeConfiguration(LinkConfiguration* config)
{
    _qmlConfigurations.removeOne(config);

    for (int i=0; i<_sharedConfigurations.count(); i++) {
        if (_sharedConfigurations[i].data() == config) {
            _sharedConfigurations.removeAt(i);
            return;
        }
    }

    qWarning() << "LinkManager::_removeConfiguration called with unknown config";
}

QList<LinkInterface*> LinkManager::links(void)
{
    QList<LinkInterface*> rawLinks;

    for (int i=0; i<_sharedLinks.count(); i++) {
        rawLinks.append(_sharedLinks[i].data());
    }

    return rawLinks;
}
977 978 979 980 981 982 983 984 985 986 987

void LinkManager::startAutoConnectedLinks(void)
{
    SharedLinkConfigurationPointer conf;

    for(int i = 0; i < _sharedConfigurations.count(); i++) {
        conf = _sharedConfigurations[i];
        if (conf->isAutoConnect())
            createConnectedLink(conf);
    }
}
988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009

int LinkManager::_reserveMavlinkChannel(void)
{
    // Find a mavlink channel to use for this link, Channel 0 is reserved for internal use.
    for (int mavlinkChannel=1; mavlinkChannel<32; mavlinkChannel++) {
        if (!(_mavlinkChannelsUsedBitMask & 1 << mavlinkChannel)) {
            mavlink_reset_channel_status(mavlinkChannel);
            // Start the channel on Mav 1 protocol
            mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
            mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
            _mavlinkChannelsUsedBitMask |= 1 << mavlinkChannel;
            return mavlinkChannel;
        }
    }

    return 0;   // All channels reserved
}

void LinkManager::_freeMavlinkChannel(int channel)
{
    _mavlinkChannelsUsedBitMask &= ~(1 << channel);
}
1010

1011 1012
void LinkManager::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) {
    link->startMavlinkMessagesTimer(message.sysid);
1013
}