Skip to content
Snippets Groups Projects
BluetoothLink.cc 10.8 KiB
Newer Older
  • Learn to ignore specific revisions
  • dogmaphobic's avatar
    dogmaphobic committed
    /*=====================================================================
    
    QGroundControl Open Source Ground Control Station
    
    (c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
    
    This file is part of the QGROUNDCONTROL project
    
        QGROUNDCONTROL is free software: you can redistribute it and/or modify
        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.
    
        QGROUNDCONTROL is distributed in the hope that it will be useful,
        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.
    
        You should have received a copy of the GNU General Public License
        along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
    
    ======================================================================*/
    
    /**
     * @file
     *   @brief Definition of Bluetooth connection for unmanned vehicles
     *   @author Gus Grubba <mavlink@grubba.com>
     *
     */
    
    #include <QtGlobal>
    #include <QTimer>
    #include <QList>
    #include <QDebug>
    #include <iostream>
    
    #include <QtBluetooth/QBluetoothDeviceDiscoveryAgent>
    #include <QtBluetooth/QBluetoothLocalDevice>
    #include <QtBluetooth/QBluetoothUuid>
    
    dogmaphobic's avatar
    dogmaphobic committed
    #include <QtBluetooth/QBluetoothSocket>
    
    dogmaphobic's avatar
    dogmaphobic committed
    
    #include "BluetoothLink.h"
    #include "QGC.h"
    
    BluetoothLink::BluetoothLink(BluetoothConfiguration* config)
        : _connectState(false)
        , _targetSocket(NULL)
    
    dogmaphobic's avatar
    dogmaphobic committed
    #ifdef __ios__
        , _discoveryAgent(NULL)
    #endif
        , _shutDown(false)
    
    dogmaphobic's avatar
    dogmaphobic committed
    {
        Q_ASSERT(config != NULL);
        _config = config;
        _config->setLink(this);
    
    dogmaphobic's avatar
    dogmaphobic committed
        //moveToThread(this);
    
    dogmaphobic's avatar
    dogmaphobic committed
    }
    
    BluetoothLink::~BluetoothLink()
    {
        // Disconnect link from configuration
        _config->setLink(NULL);
        _disconnect();
    
    dogmaphobic's avatar
    dogmaphobic committed
    #ifdef __ios__
        if(_discoveryAgent) {
            _shutDown = true;
            _discoveryAgent->stop();
            _discoveryAgent->deleteLater();
            _discoveryAgent = NULL;
        }
    #endif
    
    dogmaphobic's avatar
    dogmaphobic committed
    }
    
    void BluetoothLink::run()
    {
    }
    
    void BluetoothLink::_restartConnection()
    {
        if(this->isConnected())
        {
            _disconnect();
            _connect();
        }
    }
    
    QString BluetoothLink::getName() const
    {
        return _config->name();
    }
    
    
    void BluetoothLink::_writeBytes(const QByteArray bytes)
    
    dogmaphobic's avatar
    dogmaphobic committed
    {
        if(_targetSocket)
        {
            if(_targetSocket->isWritable())
            {
    
                if(_targetSocket->write(bytes) > 0) {
    
                    _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch());
    
    dogmaphobic's avatar
    dogmaphobic committed
                }
                else
                    qWarning() << "Bluetooth write error";
            }
            else
                qWarning() << "Bluetooth not writable error";
        }
    }
    
    void BluetoothLink::readBytes()
    {
        while (_targetSocket->bytesAvailable() > 0)
        {
            QByteArray datagram;
            datagram.resize(_targetSocket->bytesAvailable());
            _targetSocket->read(datagram.data(), datagram.size());
            emit bytesReceived(this, datagram);
            _logInputDataRate(datagram.length(), QDateTime::currentMSecsSinceEpoch());
        }
    }
    
    void BluetoothLink::_disconnect(void)
    {
    
    dogmaphobic's avatar
    dogmaphobic committed
    #ifdef __ios__
        if(_discoveryAgent) {
            _shutDown = true;
            _discoveryAgent->stop();
            _discoveryAgent->deleteLater();
            _discoveryAgent = NULL;
    
    dogmaphobic's avatar
    dogmaphobic committed
        }
    
    dogmaphobic's avatar
    dogmaphobic committed
    #endif
    
    dogmaphobic's avatar
    dogmaphobic committed
        if(_targetSocket)
        {
    
    dogmaphobic's avatar
    dogmaphobic committed
            delete _targetSocket;
    
    dogmaphobic's avatar
    dogmaphobic committed
            _targetSocket = NULL;
            emit disconnected();
        }
        _connectState = false;
    }
    
    bool BluetoothLink::_connect(void)
    {
    
    dogmaphobic's avatar
    dogmaphobic committed
        _hardwareConnect();
    
    dogmaphobic's avatar
    dogmaphobic committed
        return true;
    }
    
    bool BluetoothLink::_hardwareConnect()
    
    dogmaphobic's avatar
    dogmaphobic committed
    {
    #ifdef __ios__
        if(_discoveryAgent) {
            _shutDown = true;
            _discoveryAgent->stop();
            _discoveryAgent->deleteLater();
            _discoveryAgent = NULL;
        }
        _discoveryAgent = new QBluetoothServiceDiscoveryAgent(this);
    
    Tomaz Canabrava's avatar
    Tomaz Canabrava committed
        QObject::connect(_discoveryAgent, &QBluetoothServiceDiscoveryAgent::serviceDiscovered, this, &BluetoothLink::serviceDiscovered);
        QObject::connect(_discoveryAgent, &QBluetoothServiceDiscoveryAgent::finished, this, &BluetoothLink::discoveryFinished);
        QObject::connect(_discoveryAgent, &QBluetoothServiceDiscoveryAgent::canceled, this, &BluetoothLink::discoveryFinished);
    
        QObject::connect(_discoveryAgent, static_cast<void (QBluetoothServiceDiscoveryAgent::*)(QBluetoothSocket::SocketError)>(&QBluetoothServiceDiscoveryAgent::error),
                this, &BluetoothLink::discoveryError);
    
    dogmaphobic's avatar
    dogmaphobic committed
        _shutDown = false;
        _discoveryAgent->start();
    #else
        _createSocket();
        _targetSocket->connectToService(QBluetoothAddress(_config->device().address), QBluetoothUuid(QBluetoothUuid::Rfcomm));
    #endif
        return true;
    }
    
    void BluetoothLink::_createSocket()
    
    dogmaphobic's avatar
    dogmaphobic committed
    {
        if(_targetSocket)
        {
            delete _targetSocket;
            _targetSocket = NULL;
        }
        _targetSocket = new QBluetoothSocket(QBluetoothServiceInfo::RfcommProtocol, this);
    
        QObject::connect(_targetSocket, &QBluetoothSocket::connected, this, &BluetoothLink::deviceConnected);
    
        QObject::connect(_targetSocket, &QBluetoothSocket::readyRead, this, &BluetoothLink::readBytes);
        QObject::connect(_targetSocket, &QBluetoothSocket::disconnected, this, &BluetoothLink::deviceDisconnected);
    
    Tomaz Canabrava's avatar
    Tomaz Canabrava committed
    
        QObject::connect(_targetSocket, static_cast<void (QBluetoothSocket::*)(QBluetoothSocket::SocketError)>(&QBluetoothSocket::error),
                this, &BluetoothLink::deviceError);
    
    dogmaphobic's avatar
    dogmaphobic committed
    #ifdef __ios__
    void BluetoothLink::discoveryError(QBluetoothServiceDiscoveryAgent::Error error)
    {
        qDebug() << "Discovery error:" << error;
        qDebug() << _discoveryAgent->errorString();
    }
    #endif
    
    #ifdef __ios__
    void BluetoothLink::serviceDiscovered(const QBluetoothServiceInfo& info)
    {
        if(!info.device().name().isEmpty() && !_targetSocket)
        {
            if(_config->device().uuid == info.device().deviceUuid() && _config->device().name == info.device().name())
            {
                _createSocket();
                _targetSocket->connectToService(info);
            }
        }
    }
    #endif
    
    #ifdef __ios__
    void BluetoothLink::discoveryFinished()
    {
        if(_discoveryAgent && !_shutDown)
        {
            _shutDown = true;
            _discoveryAgent->deleteLater();
            _discoveryAgent = NULL;
            if(!_targetSocket)
            {
                _connectState = false;
                emit communicationError("Could not locate Bluetooth device:", _config->device().name);
            }
        }
    }
    #endif
    
    
    dogmaphobic's avatar
    dogmaphobic committed
    void BluetoothLink::deviceConnected()
    {
        _connectState = true;
        emit connected();
    }
    
    void BluetoothLink::deviceDisconnected()
    {
        _connectState = false;
        qWarning() << "Bluetooth disconnected";
    }
    
    void BluetoothLink::deviceError(QBluetoothSocket::SocketError error)
    {
        _connectState = false;
        qWarning() << "Bluetooth error" << error;
        emit communicationError("Bluetooth Link Error", _targetSocket->errorString());
    }
    
    bool BluetoothLink::isConnected() const
    {
        return _connectState;
    }
    
    qint64 BluetoothLink::getConnectionSpeed() const
    {
        return 1000000; // 1 Mbit
    }
    
    qint64 BluetoothLink::getCurrentInDataRate() const
    {
        return 0;
    }
    
    qint64 BluetoothLink::getCurrentOutDataRate() const
    {
        return 0;
    }
    
    //--------------------------------------------------------------------------
    //-- BluetoothConfiguration
    
    BluetoothConfiguration::BluetoothConfiguration(const QString& name)
        : LinkConfiguration(name)
        , _deviceDiscover(NULL)
    {
    
    }
    
    BluetoothConfiguration::BluetoothConfiguration(BluetoothConfiguration* source)
        : LinkConfiguration(source)
        , _deviceDiscover(NULL)
    {
    
        _device = source->device();
    
    dogmaphobic's avatar
    dogmaphobic committed
    }
    
    BluetoothConfiguration::~BluetoothConfiguration()
    {
        if(_deviceDiscover)
        {
            _deviceDiscover->stop();
            delete _deviceDiscover;
        }
    }
    
    void BluetoothConfiguration::copyFrom(LinkConfiguration *source)
    {
        LinkConfiguration::copyFrom(source);
        BluetoothConfiguration* usource = dynamic_cast<BluetoothConfiguration*>(source);
        Q_ASSERT(usource != NULL);
    
        _device = usource->device();
    
    dogmaphobic's avatar
    dogmaphobic committed
    }
    
    void BluetoothConfiguration::saveSettings(QSettings& settings, const QString& root)
    {
        settings.beginGroup(root);
    
    dogmaphobic's avatar
    dogmaphobic committed
        settings.setValue("deviceName", _device.name);
    #ifdef __ios__
        settings.setValue("uuid", _device.uuid.toString());
    #else
        settings.setValue("address",_device.address);
    #endif
    
    dogmaphobic's avatar
    dogmaphobic committed
        settings.endGroup();
    }
    
    void BluetoothConfiguration::loadSettings(QSettings& settings, const QString& root)
    {
        settings.beginGroup(root);
    
    dogmaphobic's avatar
    dogmaphobic committed
        _device.name    = settings.value("deviceName", _device.name).toString();
    #ifdef __ios__
        QString suuid   = settings.value("uuid", _device.uuid.toString()).toString();
        _device.uuid    = QUuid(suuid);
    #else
    
        _device.address = settings.value("address", _device.address).toString();
    
    dogmaphobic's avatar
    dogmaphobic committed
    #endif
    
    dogmaphobic's avatar
    dogmaphobic committed
        settings.endGroup();
    }
    
    void BluetoothConfiguration::updateSettings()
    {
        if(_link) {
            BluetoothLink* ulink = dynamic_cast<BluetoothLink*>(_link);
            if(ulink) {
                ulink->_restartConnection();
            }
        }
    }
    
    void BluetoothConfiguration::stopScan()
    {
        if(_deviceDiscover)
        {
    
            _deviceDiscover->stop();
            _deviceDiscover->deleteLater();
            _deviceDiscover = NULL;
            emit scanningChanged();
    
    dogmaphobic's avatar
    dogmaphobic committed
        }
    }
    
    void BluetoothConfiguration::startScan()
    {
        if(!_deviceDiscover)
        {
            _deviceDiscover = new QBluetoothDeviceDiscoveryAgent(this);
    
            connect(_deviceDiscover, &QBluetoothDeviceDiscoveryAgent::deviceDiscovered,  this, &BluetoothConfiguration::deviceDiscovered);
            connect(_deviceDiscover, &QBluetoothDeviceDiscoveryAgent::finished,          this, &BluetoothConfiguration::doneScanning);
    
    dogmaphobic's avatar
    dogmaphobic committed
            emit scanningChanged();
        }
        else
        {
            _deviceDiscover->stop();
        }
    
        _nameList.clear();
    
    dogmaphobic's avatar
    dogmaphobic committed
        _deviceList.clear();
    
        emit nameListChanged();
    
    dogmaphobic's avatar
    dogmaphobic committed
        _deviceDiscover->setInquiryType(QBluetoothDeviceDiscoveryAgent::GeneralUnlimitedInquiry);
        _deviceDiscover->start();
    }
    
    void BluetoothConfiguration::deviceDiscovered(QBluetoothDeviceInfo info)
    {
    
        //print_device_info(info);
        if(!info.name().isEmpty() && info.isValid())
        {
            BluetoothData data;
            data.name    = info.name();
    
    dogmaphobic's avatar
    dogmaphobic committed
    #ifdef __ios__
            data.uuid    = info.deviceUuid();
    #else
    
            data.address = info.address().toString();
    
    dogmaphobic's avatar
    dogmaphobic committed
    #endif
    
            if(!_deviceList.contains(data))
            {
                _deviceList += data;
                _nameList   += data.name;
                emit nameListChanged();
                return;
            }
        }
    
    dogmaphobic's avatar
    dogmaphobic committed
    }
    
    void BluetoothConfiguration::doneScanning()
    {
        if(_deviceDiscover)
        {
            _deviceDiscover->deleteLater();
            _deviceDiscover = NULL;
            emit scanningChanged();
        }
    }
    
    
    void BluetoothConfiguration::setDevName(const QString &name)
    
    dogmaphobic's avatar
    dogmaphobic committed
    {
    
        foreach(const BluetoothData& data, _deviceList)
    
    dogmaphobic's avatar
    dogmaphobic committed
        {
    
            if(data.name == name)
            {
                _device = data;
                emit devNameChanged();
    
    dogmaphobic's avatar
    dogmaphobic committed
    #ifndef __ios__
    
                emit addressChanged();
    
    dogmaphobic's avatar
    dogmaphobic committed
    #endif
    
    dogmaphobic's avatar
    dogmaphobic committed
    QString BluetoothConfiguration::address()
    {
    #ifdef __ios__
        return QString("");
    #else
        return _device.address;
    #endif
    }