BLEProthesisInterfaceQtWorker.cpp

Go to the documentation of this file.

#include "BLEProthesisInterfaceQtWorker.h"
 
#include <QRegularExpression>
 
#define UARTSERVICEUUID "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
#define RXUUID "6e400002-b5a3-f393-e0a9-e50e24dcca9e"
#define TXUUID "6e400003-b5a3-f393-e0a9-e50e24dcca9e"
 
BLEProthesisInterfaceQtWorker::BLEProthesisInterfaceQtWorker(const QString& mac,
                                                             BLEProthesisInterface& owner) :
    _owner{&owner}, _mac{mac}
{
    _timer = startTimer(std::chrono::milliseconds{10});
}
 
BLEProthesisInterfaceQtWorker::~BLEProthesisInterfaceQtWorker()
{
    cleanup();
}
 
void
BLEProthesisInterfaceQtWorker::kill()
{
    _killed = true;
}
 
void
BLEProthesisInterfaceQtWorker::sendCommand(const std::string& cmd)
{
    std::lock_guard guard(_cmdMutex);
    _cmd += QString::fromStdString(cmd);
}
 
void
BLEProthesisInterfaceQtWorker::cleanup()
{
    //stop services etc
    if (_timer != -1)
    {
        killTimer(_timer);
        _timer = -1;
    }
    //disconnect
    if (_control)
    {
        _control->disconnectFromDevice();
    }
 
    //delete
    if (_service)
    {
        delete _service;
        _service = nullptr;
    }
    if (_control)
    {
        delete _control;
        _control = nullptr;
    }
    if (_deviceDiscoveryAgent)
    {
        delete _deviceDiscoveryAgent;
        _deviceDiscoveryAgent = nullptr;
    }
}
 
void
BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
{
    if (_killed)
    {
        qDebug() << '[' << _mac << ']' << " Stopping NOW!";
        cleanup();
        quit();
        return;
    }
    //discovering
    if (_owner->_state == State::Created)
    {
        _deviceDiscoveryAgent = new QBluetoothDeviceDiscoveryAgent;
        connect(_deviceDiscoveryAgent,
                SIGNAL(deviceDiscovered(const QBluetoothDeviceInfo&)),
                this,
                SLOT(deviceDiscovered(const QBluetoothDeviceInfo&)));
        connect(_deviceDiscoveryAgent,
                SIGNAL(error(QBluetoothDeviceDiscoveryAgent::Error)),
                this,
                SLOT(deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error)));
        connect(_deviceDiscoveryAgent, SIGNAL(finished()), this, SLOT(deviceDiscoverFinished()));
        connect(_deviceDiscoveryAgent, SIGNAL(canceled()), this, SLOT(deviceDiscoverFinished()));
        qDebug() << '[' << _mac << ']' << " State DiscoveringDevices";
        _owner->_state = State::DiscoveringDevices;
        _deviceDiscoveryAgent->start();
    }
    else if (_owner->_state == State::DiscoveringDevicesDone)
    {
        if (!_deviceDiscovered)
        {
            qDebug() << '[' << _mac << ']' << " Device discovering failed!";
            kill();
            return;
        }
        qDebug() << '[' << _mac << ']' << " State Disconnected";
        _owner->_state = State::Disconnected;
    }
    else if (_owner->_state == State::Disconnected)
    {
        if (_service)
        {
            delete _service;
            _service = nullptr;
        }
        if (_control)
        {
            delete _control;
            _control = nullptr;
        }
        _serviceDiscovered = false;
        _control = new QLowEnergyController(_currentDevice);
        _control->setRemoteAddressType(QLowEnergyController::RandomAddress);
 
        connect(_control,
                SIGNAL(serviceDiscovered(QBluetoothUuid)),
                this,
                SLOT(serviceDiscovered(QBluetoothUuid)));
        connect(_control, SIGNAL(discoveryFinished()), this, SLOT(serviceScanDone()));
        connect(_control,
                SIGNAL(error(QLowEnergyController::Error)),
                this,
                SLOT(controllerError(QLowEnergyController::Error)));
        connect(_control, SIGNAL(connected()), this, SLOT(deviceConnected()));
        connect(_control, SIGNAL(disconnected()), this, SLOT(deviceDisconnected()));
        _control->connectToDevice();
        qDebug() << '[' << _mac << ']' << " State Connecting";
        _owner->_state = State::Connecting;
    }
    else if (_owner->_state == State::ConnectingDone)
    {
        _control->discoverServices();
        qDebug() << '[' << _mac << ']' << " State DiscoveringServices";
        _owner->_state = State::DiscoveringServices;
    }
    else if (_owner->_state == State::DiscoveringServicesDone)
    {
        if (!_serviceDiscovered)
        {
            qDebug() << '[' << _mac << ']' << " Service discovering failed!";
            kill();
            return;
        }
        _service = _control->createServiceObject(QBluetoothUuid(QUuid(UARTSERVICEUUID)));
 
        connect(_service,
                SIGNAL(stateChanged(QLowEnergyService::ServiceState)),
                this,
                SLOT(serviceStateChanged(QLowEnergyService::ServiceState)));
        connect(_service,
                SIGNAL(characteristicChanged(QLowEnergyCharacteristic, QByteArray)),
                this,
                SLOT(readData(QLowEnergyCharacteristic, QByteArray)));
        connect(_service,
                SIGNAL(descriptorWritten(QLowEnergyDescriptor, QByteArray)),
                this,
                SLOT(receiveDeviceDisconnec(QLowEnergyDescriptor, QByteArray)));
 
        _service->discoverDetails();
 
        qDebug() << '[' << _mac << ']' << " State ConnectingService";
        _owner->_state = State::ConnectingService;
    }
    else if (_owner->_state == State::Running) //send data
    {
        std::lock_guard g(_cmdMutex);
        if (_service && _cmd.size() != 0)
        {
            const QLowEnergyCharacteristic RxChar =
                _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
            QByteArray data;
            data.append(_cmd);
            _service->writeCharacteristic(RxChar, data, QLowEnergyService::WriteWithoutResponse);
            if (_owner->_verboseSend)
            {
                qDebug() << '[' << _mac << ']' << " send: " << _cmd;
            }
            _cmd.clear();
        }
    }
}
 
void
BLEProthesisInterfaceQtWorker::deviceDiscovered(const QBluetoothDeviceInfo& device)
{
    if (device.address().toString() == _mac)
    {
        qDebug() << '[' << _mac << ']' << " Discovered target device "
                 << device.address().toString();
        _currentDevice = device;
        _deviceDiscovered = true;
        qDebug() << '[' << _mac << ']' << " State DiscoveringDevicesDone";
        _owner->_state = State::DiscoveringDevicesDone;
        _deviceDiscoveryAgent->stop();
    }
    else
    {
        qDebug() << '[' << _mac << ']' << " Discovered device " << device.address().toString();
    }
}
 
void
BLEProthesisInterfaceQtWorker::deviceDiscoverFinished()
{
    qDebug() << '[' << _mac << ']' << " Discovering of services done.";
    _owner->_state = State::DiscoveringDevicesDone;
}
 
void
BLEProthesisInterfaceQtWorker::deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error error)
{
    if (error == QBluetoothDeviceDiscoveryAgent::PoweredOffError)
    {
        qDebug() << '[' << _mac << ']'
                 << "The Bluetooth adaptor is powered off, power it on before doing discovery.";
    }
    else if (error == QBluetoothDeviceDiscoveryAgent::InputOutputError)
    {
        qDebug() << '[' << _mac << ']'
                 << "Writing or reading from the device resulted in an error.";
    }
    else
    {
        qDebug() << '[' << _mac << ']' << "An unknown error has occurred.";
    }
    kill();
}
 
void
BLEProthesisInterfaceQtWorker::serviceDiscovered(const QBluetoothUuid& gatt)
{
    qDebug() << '[' << _mac << ']' << " Discovered service " << gatt.toString();
    if (gatt == QBluetoothUuid(QUuid(UARTSERVICEUUID)))
    {
        qDebug() << '[' << _mac << ']' << "Discovered UART service " << gatt.toString();
        _serviceDiscovered = true;
        _owner->_state = State::DiscoveringServicesDone;
    }
}
 
void
BLEProthesisInterfaceQtWorker::serviceDiscoverFinished()
{
    qDebug() << '[' << _mac << ']' << " State DiscoveringServicesDone";
    _owner->_state = State::DiscoveringServicesDone;
}
 
void
BLEProthesisInterfaceQtWorker::controllerError(QLowEnergyController::Error error)
{
    qDebug() << '[' << _mac << ']' << " Cannot connect to remote device.";
    qWarning() << '[' << _mac << ']' << " Controller Error:" << error;
    kill();
}
 
void
BLEProthesisInterfaceQtWorker::receiveDeviceDisconnec(const QLowEnergyDescriptor& d,
                                                      const QByteArray& value)
{
    if (d.isValid() && d == _notificationDescTx && value == QByteArray("0000"))
    {
        qDebug() << '[' << _mac << ']' << "Device requests disconnect.";
        //disabled notifications -> assume disconnect intent
        _control->disconnectFromDevice();
        if (_service)
        {
            delete _service;
            _service = nullptr;
        }
    }
}
 
void
BLEProthesisInterfaceQtWorker::serviceStateChanged(QLowEnergyService::ServiceState s)
{
    // A descriptoc can only be written if the service is in the ServiceDiscovered state
    qDebug() << '[' << _mac << ']' << " serviceStateChanged -> " << s;
    switch (s)
    {
        case QLowEnergyService::ServiceDiscovered:
        {
 
            //looking for the TX characteristic
            const QLowEnergyCharacteristic TxChar =
                _service->characteristic(QBluetoothUuid(QUuid(TXUUID)));
            if (!TxChar.isValid())
            {
                qDebug() << '[' << _mac << ']' << " Tx characteristic not found";
                break;
            }
 
            //looking for the RX characteristic
            const QLowEnergyCharacteristic RxChar =
                _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
            if (!RxChar.isValid())
            {
                qDebug() << '[' << _mac << ']' << " Rx characteristic not found";
                break;
            }
 
            // Bluetooth LE spec Where a characteristic can be notified, a Client Characteristic Configuration descriptor
            // shall be included in that characteristic as required by the Bluetooth Core Specification
            // Tx notify is enabled
            _notificationDescTx =
                TxChar.descriptor(QBluetoothUuid::ClientCharacteristicConfiguration);
            if (_notificationDescTx.isValid())
            {
                // enable notification
                _service->writeDescriptor(_notificationDescTx, QByteArray::fromHex("0100"));
                qDebug() << '[' << _mac << ']' << " State Running";
                _owner->_state = State::Running;
            }
        }
        break;
        default:
            break;
    }
}
 
void
BLEProthesisInterfaceQtWorker::deviceConnected()
{
    qDebug() << '[' << _mac << ']' << " State ConnectingDone";
    _owner->_state = State::ConnectingDone;
}
 
void
BLEProthesisInterfaceQtWorker::deviceDisconnected()
{
    qDebug() << '[' << _mac << ']' << " State Disconnected";
    _owner->_state = State::Disconnected;
}
 
template <>
void
BLEProthesisInterfaceQtWorker::consumeData<
    BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm>()
{
    if (!_valueAkk.contains('\n'))
    {
        if (_owner->_verboseReceive)
        {
            qDebug() << '[' << _mac << ']'
                     << " data does not contain \\n -> no new sensor values\n Buffer:\n"
                     << _valueAkk;
        }
        return;
    }
    auto listPacks = _valueAkk.split('\n');
 
    if (_owner->_verboseReceive)
    {
        qDebug() << '[' << _mac << ']' << " parsing " << listPacks.at(listPacks.size() - 2);
    }
 
    auto listVals = listPacks.at(listPacks.size() - 2).split(' ');
    _owner->_thumbPos = listVals.at(0).toLong();
    _owner->_thumbPWM = listVals.at(1).toLong();
    _owner->_fingerPos = listVals.at(2).toLong();
    _owner->_fingerPWM = listVals.at(3).toLong();
 
    _valueAkk = listPacks.back();
}
 
template <>
void
BLEProthesisInterfaceQtWorker::consumeData<BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm>()
{
    if (!_valueAkk.contains('\n'))
    {
        if (_owner->_verboseReceive)
        {
            qDebug() << '[' << _mac << ']'
                     << " data does not contain \\n -> no new sensor values\n Buffer:\n"
                     << _valueAkk;
        }
        return;
    }
    auto listPacks = _valueAkk.split('\n');
 
    static const QRegularExpression m2(
        R"(^M2:[ \t]+Pos.:[ \t]+(-?[1-9][0-9]*|0)[ \t]+PWM:[ \t]+(-?[1-9][0-9]*|0)[ \t\n\r]+$)");
    static const QRegularExpression m3(
        R"(^M3:[ \t]+Pos.:[ \t]+(-?[1-9][0-9]*|0)[ \t]+PWM:[ \t]+(-?[1-9][0-9]*|0)[ \t\n\r]+$)");
 
    for (int i = 0; i < listPacks.size() - 1; ++i)
    {
        if (listPacks.at(i).size() == 0)
        {
            continue;
        }
        if (_owner->_verboseReceive)
        {
            qDebug() << '[' << _mac << ']' << " parsing " << listPacks.at(i);
        }
        if (const auto matchM2 = m2.match(listPacks.at(i)); matchM2.hasMatch())
        {
            _owner->_thumbPos = matchM2.captured(1).toLong();
            _owner->_thumbPWM = matchM2.captured(2).toLong();
            if (_owner->_verboseReceive)
            {
                qDebug() << '[' << _mac << ']' << " updated M2";
            }
        }
        else if (const auto matchM3 = m3.match(listPacks.at(i)); matchM3.hasMatch())
        {
            _owner->_fingerPos = matchM3.captured(1).toLong();
            _owner->_fingerPWM = matchM3.captured(2).toLong();
            if (_owner->_verboseReceive)
            {
                qDebug() << '[' << _mac << ']' << " updated M3";
            }
        }
        else
        {
            qWarning() << "unknown format for data: " << listPacks.at(i).toLocal8Bit()
                       << "\nSkipping";
        }
    }
    _valueAkk = listPacks.back();
}
 
void
BLEProthesisInterfaceQtWorker::readData(const QLowEnergyCharacteristic& c, const QByteArray& value)
{
    // ignore any other characteristic change
    if (c.uuid() != QBluetoothUuid(QUuid(TXUUID)))
    {
        return;
    }
    if (_owner->_verboseReceive)
    {
        qDebug() << '[' << _mac << ']' << " received : " << value;
    }
 
    _valueAkk.append(value.data());
 
    switch (_owner->_protocol)
    {
        case BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm:
            consumeData<BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm>();
            break;
        case BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm:
            consumeData<BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm>();
            break;
    }
}