LinkManager.cc

/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/

#include <QList>
#include <QApplication>
#include <QDebug>
#include <QSignalSpy>

#ifndef NO_SERIAL_LINK
#include "QGCSerialPortInfo.h"
#endif

#include "LinkManager.h"
#include "QGCApplication.h"
#include "UDPLink.h"
#include "TCPLink.h"
#ifdef QGC_ENABLE_BLUETOOTH
#include "BluetoothLink.h"
#endif

#ifndef __mobile__
    #include "GPSManager.h"
#endif

QGC_LOGGING_CATEGORY(LinkManagerLog, "LinkManagerLog")
QGC_LOGGING_CATEGORY(LinkManagerVerboseLog, "LinkManagerVerboseLog")

const char* LinkManager::_settingsGroup =            "LinkManager";
const char* LinkManager::_autoconnectUDPKey =        "AutoconnectUDP";
const char* LinkManager::_autoconnectPixhawkKey =    "AutoconnectPixhawk";
const char* LinkManager::_autoconnect3DRRadioKey =   "Autoconnect3DRRadio";
const char* LinkManager::_autoconnectPX4FlowKey =    "AutoconnectPX4Flow";
const char* LinkManager::_autoconnectRTKGPSKey =     "AutoconnectRTKGPS";
const char* LinkManager::_autoconnectLibrePilotKey = "AutoconnectLibrePilot";
const char* LinkManager::_defaultUPDLinkName =       "UDP Link (AutoConnect)";

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

LinkManager::LinkManager(QGCApplication* app)
    : QGCTool(app)
    , _configUpdateSuspended(false)
    , _configurationsLoaded(false)
    , _connectionsSuspended(false)
    , _mavlinkChannelsUsedBitMask(1)    // We never use channel 0 to avoid sequence numbering problems
    , _mavlinkProtocol(NULL)
    , _autoconnectUDP(true)
    , _autoconnectPixhawk(true)
    , _autoconnect3DRRadio(true)
    , _autoconnectPX4Flow(true)
    , _autoconnectRTKGPS(true)
    , _autoconnectLibrePilot(true)
{
    qmlRegisterUncreatableType<LinkManager>         ("QGroundControl", 1, 0, "LinkManager",         "Reference only");
    qmlRegisterUncreatableType<LinkConfiguration>   ("QGroundControl", 1, 0, "LinkConfiguration",   "Reference only");
    qmlRegisterUncreatableType<LinkInterface>       ("QGroundControl", 1, 0, "LinkInterface",       "Reference only");

    QSettings settings;

    settings.beginGroup(_settingsGroup);
    _autoconnectUDP =        settings.value(_autoconnectUDPKey, true).toBool();
    _autoconnectPixhawk =    settings.value(_autoconnectPixhawkKey, true).toBool();
    _autoconnect3DRRadio =   settings.value(_autoconnect3DRRadioKey, true).toBool();
    _autoconnectPX4Flow =    settings.value(_autoconnectPX4FlowKey, true).toBool();
    _autoconnectRTKGPS =     settings.value(_autoconnectRTKGPSKey, true).toBool();
    _autoconnectLibrePilot = settings.value(_autoconnectLibrePilotKey, true).toBool();

#ifndef NO_SERIAL_LINK
    _activeLinkCheckTimer.setInterval(_activeLinkCheckTimeoutMSecs);
    _activeLinkCheckTimer.setSingleShot(false);
    connect(&_activeLinkCheckTimer, &QTimer::timeout, this, &LinkManager::_activeLinkCheck);
#endif
}

LinkManager::~LinkManager()
{

}

void LinkManager::setToolbox(QGCToolbox *toolbox)
{
   QGCTool::setToolbox(toolbox);

   _mavlinkProtocol = _toolbox->mavlinkProtocol();

    connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateAutoConnectLinks);
    _portListTimer.start(_autoconnectUpdateTimerMSecs); // timeout must be long enough to get past bootloader on second pass

}

LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& config)
{
    if (!config) {
        qWarning() << "LinkManager::createConnectedLink called with NULL config";
        return NULL;
    }

    LinkInterface* pLink = NULL;
    switch(config->type()) {
#ifndef NO_SERIAL_LINK
        case LinkConfiguration::TypeSerial:
        {
            SerialConfiguration* serialConfig = dynamic_cast<SerialConfiguration*>(config.data());
            if (serialConfig) {
                pLink = new SerialLink(config);
                if (serialConfig->usbDirect()) {
                    _activeLinkCheckList.append((SerialLink*)pLink);
                    if (!_activeLinkCheckTimer.isActive()) {
                        _activeLinkCheckTimer.start();
                    }
                }
            }
        }
        break;
#endif
        case LinkConfiguration::TypeUdp:
            pLink = new UDPLink(config);
            break;
        case LinkConfiguration::TypeTcp:
            pLink = new TCPLink(config);
            break;
#ifdef QGC_ENABLE_BLUETOOTH
        case LinkConfiguration::TypeBluetooth:
            pLink = new BluetoothLink(config);
            break;
#endif
#ifndef __mobile__
        case LinkConfiguration::TypeLogReplay:
            pLink = new LogReplayLink(config);
            break;
#endif
#ifdef QT_DEBUG
        case LinkConfiguration::TypeMock:
            pLink = new MockLink(config);
            break;
#endif
        case LinkConfiguration::TypeLast:
        default:
            break;
    }

    if (pLink) {
        _addLink(pLink);
        connectLink(pLink);
    }

    return pLink;
}

LinkInterface* LinkManager::createConnectedLink(const QString& name)
{
    Q_ASSERT(name.isEmpty() == false);
    for(int i = 0; i < _sharedConfigurations.count(); i++) {
        SharedLinkConfigurationPointer& conf = _sharedConfigurations[i];
        if (conf->name() == name)
            return createConnectedLink(conf);
    }
    return NULL;
}

void LinkManager::_addLink(LinkInterface* link)
{
    if (thread() != QThread::currentThread()) {
        qWarning() << "_deleteLink called from incorrect thread";
        return;
    }

    if (!link) {
        return;
    }

    if (!containsLink(link)) {
        bool channelSet = false;

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