diff options
Diffstat (limited to 'src/plugins/sensors/sensortag/sensortagbase.cpp')
| -rw-r--r-- | src/plugins/sensors/sensortag/sensortagbase.cpp | 600 |
1 files changed, 600 insertions, 0 deletions
diff --git a/src/plugins/sensors/sensortag/sensortagbase.cpp b/src/plugins/sensors/sensortag/sensortagbase.cpp new file mode 100644 index 00000000..322a86d7 --- /dev/null +++ b/src/plugins/sensors/sensortag/sensortagbase.cpp @@ -0,0 +1,600 @@ +/**************************************************************************** +** +** Copyright (C) 2017 Lorn Potter +** Copyright (C) 2016 Canonical, Ltd. +** Contact: https://www.qt.io/licensing/ +** +** This file is part of the QtSensors module of the Qt Toolkit. +** +** $QT_BEGIN_LICENSE:LGPL$ +** Commercial License Usage +** Licensees holding valid commercial Qt licenses may use this file in +** accordance with the commercial license agreement provided with the +** Software or, alternatively, in accordance with the terms contained in +** a written agreement between you and The Qt Company. For licensing terms +** and conditions see https://www.qt.io/terms-conditions. For further +** information use the contact form at https://www.qt.io/contact-us. +** +** GNU Lesser General Public License Usage +** Alternatively, this file may be used under the terms of the GNU Lesser +** General Public License version 3 as published by the Free Software +** Foundation and appearing in the file LICENSE.LGPL3 included in the +** packaging of this file. Please review the following information to +** ensure the GNU Lesser General Public License version 3 requirements +** will be met: https://www.gnu.org/licenses/lgpl-3.0.html. +** +** GNU General Public License Usage +** Alternatively, this file may be used under the terms of the GNU +** General Public License version 2.0 or (at your option) the GNU General +** Public license version 3 or any later version approved by the KDE Free +** Qt Foundation. The licenses are as published by the Free Software +** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3 +** included in the packaging of this file. Please review the following +** information to ensure the GNU General Public License requirements will +** be met: https://www.gnu.org/licenses/gpl-2.0.html and +** https://www.gnu.org/licenses/gpl-3.0.html. +** +** $QT_END_LICENSE$ +** +****************************************************************************/ + +#include "sensortagbase.h" + +#include <QLowEnergyCharacteristic> +#include <QtMath> +#include <QTimer> +#include <QDeadlineTimer> + +Q_GLOBAL_STATIC(SensorTagBasePrivate, sensortagBasePrivate) + +SensorTagBasePrivate::SensorTagBasePrivate(QObject *parent) + : QObject(parent) +{ + QTimer::singleShot(50, this, &SensorTagBasePrivate::deviceSearch); +} + +void SensorTagBasePrivate::deviceSearch() +{ + m_deviceDiscoveryAgent = new QBluetoothDeviceDiscoveryAgent(this); + + connect(m_deviceDiscoveryAgent, &QBluetoothDeviceDiscoveryAgent::deviceDiscovered, + this, &SensorTagBasePrivate::deviceFound); + connect(m_deviceDiscoveryAgent, QOverload<QBluetoothDeviceDiscoveryAgent::Error>::of(&QBluetoothDeviceDiscoveryAgent::error), + this, &SensorTagBasePrivate::deviceScanError); + connect(m_deviceDiscoveryAgent, &QBluetoothDeviceDiscoveryAgent::finished, + this, &SensorTagBasePrivate::scanFinished); + + QTimer::singleShot(20000, this, &SensorTagBasePrivate::deviceSearchTimeout); //make sure to timeout + m_deviceDiscoveryAgent->start(QBluetoothDeviceDiscoveryAgent::LowEnergyMethod); +} + +void SensorTagBasePrivate::deviceSearchTimeout() +{ + if (m_deviceDiscoveryAgent->isActive() && m_control == nullptr) { + m_deviceDiscoveryAgent->stop(); + qWarning("No Sensor Tag devices found"); + } +} + +void SensorTagBasePrivate::deviceFound(const QBluetoothDeviceInfo &device) +{ + if (device.coreConfigurations() & QBluetoothDeviceInfo::LowEnergyCoreConfiguration) { + const QString idString = QString::fromLatin1(qgetenv("QT_SENSORTAG_ID")); + + const QBluetoothAddress watchForAddress(idString); + //mac uses deviceUuid + const QUuid watchForId(idString); + + bool ok; + + if ((!watchForAddress.isNull() && watchForAddress == device.address()) || + (!watchForId.isNull() && watchForId == device.deviceUuid())) { + ok = true; + } + if (ok || device.name().contains("SensorTag")) { + + m_deviceDiscoveryAgent->stop(); + + m_control = new QLowEnergyController(device.address(), this); + + connect(m_control, &QLowEnergyController::discoveryFinished, + this, &SensorTagBasePrivate::serviceDiscoveryFinished); + connect(m_control, &QLowEnergyController::serviceDiscovered, + this, &SensorTagBasePrivate::serviceDiscovered); + connect(m_control, QOverload<QLowEnergyController::Error>::of(&QLowEnergyController::error), + this, &SensorTagBasePrivate::controllerError); + connect(m_control, &QLowEnergyController::connected, + this, &SensorTagBasePrivate::sensortagDeviceConnected); + connect(m_control, &QLowEnergyController::disconnected, + this, &SensorTagBasePrivate::deviceDisconnected); + + m_control->connectToDevice(); + } + } +} + +void SensorTagBasePrivate::serviceDiscoveryFinished() +{ + discoveryDone = true; +} + +void SensorTagBasePrivate::scanFinished() +{ + if (m_control == nullptr) + qWarning("No Sensor Tag devices found"); +} + +void SensorTagBasePrivate::deviceScanError(QBluetoothDeviceDiscoveryAgent::Error error) +{ + switch (error) { + case QBluetoothDeviceDiscoveryAgent::PoweredOffError: + qWarning("The Bluetooth adaptor is powered off, power it on before doing discovery."); + break; + case QBluetoothDeviceDiscoveryAgent::InputOutputError: + qWarning("Writing or reading from the device resulted in an error."); + break; + default: + qWarning("An unknown error has occurred."); + break; + }; +} + +void SensorTagBasePrivate::serviceError(QLowEnergyService::ServiceError e) +{ + switch (e) { + case QLowEnergyService::DescriptorWriteError: + qWarning("Cannot obtain SensorTag notifications"); + break; + default: + case QLowEnergyService::CharacteristicWriteError: + qWarning() << "SensorTag service error:" << e; + break; + }; +} + +void SensorTagBasePrivate::controllerError(QLowEnergyController::Error error) +{ + qWarning("Cannot connect to remote device."); + qWarning() << "Controller Error:" << error; +} + +void SensorTagBasePrivate::sensortagDeviceConnected() +{ + m_control->discoverServices(); +} + +void SensorTagBasePrivate::deviceDisconnected() +{ + if (q_ptr && q_ptr->sensor()->isActive()) + q_ptr->sensorStopped(); +} + +void SensorTagBasePrivate::serviceDiscovered(const QBluetoothUuid &gatt) +{ + if (enabledServiceUuids.contains(gatt)) { + + if (gatt == TI_SENSORTAG_LIGHT_SERVICE) { + lightService = m_control->createServiceObject(gatt, this); + doConnections(lightService); + } else if (gatt == TI_SENSORTAG_TEMPERATURE_SERVICE) { + temperatureService = m_control->createServiceObject(gatt, this); + doConnections(temperatureService); + } else if (gatt == TI_SENSORTAG_BAROMETER_SERVICE) { + barometerService = m_control->createServiceObject(gatt, this); + doConnections(barometerService); + } else if (gatt == TI_SENSORTAG_HUMIDTIY_SERVICE) { + humidityService = m_control->createServiceObject(gatt, this); + doConnections(humidityService); + } else if (gatt == TI_SENSORTAG_INFO_SERVICE) { + infoService = m_control->createServiceObject(gatt, this); + doConnections(infoService); + } else if (gatt == TI_SENSORTAG_ACCELEROMETER_SERVICE) { + acceleratorService = m_control->createServiceObject(gatt, this); + doConnections(acceleratorService); + } else if (gatt == TI_SENSORTAG_GYROSCOPE_SERVICE) { + gyroscopeService = m_control->createServiceObject(gatt, this); + doConnections(gyroscopeService); + } else if (gatt == TI_SENSORTAG_MAGNETOMETER_SERVICE) { + magnetometerService = m_control->createServiceObject(gatt, this); + doConnections(magnetometerService); + } else if (movementService == nullptr) { + if (gatt == TI_SENSORTAG_MOVEMENT_SERVICE) { + movementService = m_control->createServiceObject(gatt, this); + doConnections(movementService); + } + } + } +} + +void SensorTagBasePrivate::doConnections(QLowEnergyService *service) +{ + if (service) { + connect(service, &QLowEnergyService::stateChanged, + this, &SensorTagBasePrivate::serviceStateChanged); + + connect(service, &QLowEnergyService::characteristicChanged, + this, &SensorTagBasePrivate::updateCharacteristic); + + connect(service,SIGNAL(error(QLowEnergyService::ServiceError)), + this,SLOT(serviceError(QLowEnergyService::ServiceError))); + + if (service->state() == QLowEnergyService::DiscoveryRequired) { + service->discoverDetails(); + } else if (!enabledServiceUuids.isEmpty() + && enabledServiceUuids.contains(service->serviceUuid())) { + enableService(service->serviceUuid()); + } + } +} + +void SensorTagBasePrivate::serviceStateChanged(QLowEnergyService::ServiceState newState) +{ + if (newState != QLowEnergyService::ServiceDiscovered) + return; + + QLowEnergyService *m_service = qobject_cast<QLowEnergyService *>(sender()); + + if (!m_service) + return; + + if (!enabledServiceUuids.isEmpty() + && enabledServiceUuids.contains(m_service->serviceUuid())) { + enableService(m_service->serviceUuid()); + } +} + +void SensorTagBasePrivate::enableLight(bool on) +{ + if (!lightService && discoveryDone) + serviceDiscovered(TI_SENSORTAG_LIGHT_SERVICE); + if (!lightService) + return; + + const QLowEnergyCharacteristic hrChar = lightService->characteristic(TI_SENSORTAG_LIGHT_CONTROL); + lightService->writeCharacteristic(hrChar, on ? enableSensorCharacteristic : disableSensorCharacteristic); + + const QLowEnergyCharacteristic hrChar2 = lightService->characteristic(TI_SENSORTAG_LIGHT_DATA); + + if (hrChar2.descriptors().count() > 0) { + const QLowEnergyDescriptor m_notificationDesc = hrChar2.descriptors().at(0); + lightService->writeDescriptor(m_notificationDesc, + on ? enableNotificationsCharacteristic : disableNotificationsCharacteristic); + } +} + +void SensorTagBasePrivate::enableTemp(bool on) +{ + if (!temperatureService && discoveryDone) + serviceDiscovered(TI_SENSORTAG_TEMPERATURE_SERVICE); + + if (!temperatureService) + return; + + const QLowEnergyCharacteristic hrChar = temperatureService->characteristic(TI_SENSORTAG_IR_TEMPERATURE_CONTROL); + temperatureService->writeCharacteristic(hrChar,on ? enableSensorCharacteristic : disableSensorCharacteristic); + + const QLowEnergyCharacteristic hrChar2 = temperatureService->characteristic(TI_SENSORTAG_IR_TEMPERATURE_DATA); + + if (hrChar2.descriptors().count() > 0) { + const QLowEnergyDescriptor m_notificationDesc = hrChar2.descriptors().at(0); + temperatureService->writeDescriptor(m_notificationDesc, + on ? enableNotificationsCharacteristic : disableNotificationsCharacteristic); + } +} + +void SensorTagBasePrivate::enablePressure(bool on) +{ + if (!barometerService && discoveryDone) + serviceDiscovered(TI_SENSORTAG_BAROMETER_SERVICE); + if (!barometerService) + return; + + const QLowEnergyCharacteristic hrChar = barometerService->characteristic(TI_SENSORTAG_BAROMETER_CONTROL); + barometerService->writeCharacteristic(hrChar, on ? enableSensorCharacteristic : disableSensorCharacteristic); + + const QLowEnergyCharacteristic hrChar2 = barometerService->characteristic(TI_SENSORTAG_BAROMETER_DATA); + if (hrChar2.descriptors().count() > 0) { + const QLowEnergyDescriptor m_notificationDesc = hrChar2.descriptors().at(0); + + barometerService->writeDescriptor(m_notificationDesc, + on ? enableNotificationsCharacteristic : disableNotificationsCharacteristic); + } +} + +void SensorTagBasePrivate::enableHumidity(bool on) +{ + if (!humidityService && discoveryDone) + serviceDiscovered(TI_SENSORTAG_HUMIDTIY_SERVICE); + if (!humidityService) + return; + + const QLowEnergyCharacteristic hrChar = humidityService->characteristic(TI_SENSORTAG_HUMIDTIY_CONTROL); + humidityService->writeCharacteristic(hrChar, on ? enableSensorCharacteristic : disableSensorCharacteristic); + const QLowEnergyCharacteristic hrChar2 = humidityService->characteristic(TI_SENSORTAG_HUMIDTIY_DATA); + if (hrChar2.descriptors().count() > 0) { + const QLowEnergyDescriptor m_notificationDesc = hrChar2.descriptors().at(0); + humidityService->writeDescriptor(m_notificationDesc, + on ? enableNotificationsCharacteristic : disableNotificationsCharacteristic); + } +} + +void SensorTagBasePrivate::enableMovement(bool on) +{ + if (!movementService && discoveryDone) + serviceDiscovered(TI_SENSORTAG_MOVEMENT_SERVICE); + if (!movementService) + return; + + QByteArray controlCharacteristic; + + int movementControl = 0; + //movement service has different syntax here + if (on) { + if (gyroscopeEnabled) + movementControl += 7; + if (accelerometerEnabled) + movementControl += 56; + if (magnetometerEnabled) + movementControl += 64; + + controlCharacteristic = QByteArray::number(movementControl, 16); + controlCharacteristic.append("04"); + + } else { + controlCharacteristic = "00"; + } + + const QLowEnergyCharacteristic hrChar = movementService->characteristic(TI_SENSORTAG_MOVEMENT_CONTROL); + movementService->writeCharacteristic(hrChar, QByteArray::fromHex(controlCharacteristic)); + + const QLowEnergyCharacteristic hrChar2 = movementService->characteristic(TI_SENSORTAG_MOVEMENT_DATA); + if (hrChar2.descriptors().count() > 0) { + QLowEnergyDescriptor m_notificationDesc = hrChar2.descriptors().at(0); + + movementService->writeDescriptor(m_notificationDesc, + on ? enableNotificationsCharacteristic : disableNotificationsCharacteristic); + } +} + +void SensorTagBasePrivate::enableService(const QBluetoothUuid &uuid) +{ + if (uuid == TI_SENSORTAG_ACCELEROMETER_SERVICE + || uuid == TI_SENSORTAG_MAGNETOMETER_SERVICE + || uuid == TI_SENSORTAG_GYROSCOPE_SERVICE) { + + if ((uuid != TI_SENSORTAG_MOVEMENT_SERVICE) + && (accelerometerEnabled || magnetometerEnabled || gyroscopeEnabled)) + return; + + if (!enabledServiceUuids.contains(TI_SENSORTAG_MOVEMENT_SERVICE)) + enabledServiceUuids.append(TI_SENSORTAG_MOVEMENT_SERVICE); + + if (uuid == TI_SENSORTAG_ACCELEROMETER_SERVICE) + accelerometerEnabled = true; + else if (uuid == TI_SENSORTAG_MAGNETOMETER_SERVICE) + magnetometerEnabled = true; + else if (uuid == TI_SENSORTAG_GYROSCOPE_SERVICE) + gyroscopeEnabled = true; + + } else if (!enabledServiceUuids.contains(uuid)) + enabledServiceUuids.append(uuid); + + if (discoveryDone) { + + if (uuid == TI_SENSORTAG_LIGHT_SERVICE) + enableLight(true); + else if (uuid == TI_SENSORTAG_TEMPERATURE_SERVICE) + enableTemp(true); + else if (uuid == TI_SENSORTAG_BAROMETER_SERVICE) + enablePressure(true); + else if (uuid == TI_SENSORTAG_HUMIDTIY_SERVICE) + enableHumidity(true); + else if (uuid == TI_SENSORTAG_MOVEMENT_SERVICE) + enableMovement(true); + else if (uuid == TI_SENSORTAG_ACCELEROMETER_SERVICE) + enableMovement(true); + else if (uuid == TI_SENSORTAG_MAGNETOMETER_SERVICE) + enableMovement(true); + else if (uuid == TI_SENSORTAG_GYROSCOPE_SERVICE) + enableMovement(true); + } +} + +void SensorTagBasePrivate::disableService(const QBluetoothUuid &uuid) +{ + enabledServiceUuids.removeOne(uuid); + + if (uuid == TI_SENSORTAG_LIGHT_SERVICE) { + enableLight(false); + } else if (uuid == TI_SENSORTAG_TEMPERATURE_SERVICE) { + enableTemp(false); + } else if (uuid == TI_SENSORTAG_BAROMETER_SERVICE) { + enablePressure(false); + } else if (uuid == TI_SENSORTAG_HUMIDTIY_SERVICE) { + enableHumidity(false); + } else if (uuid == TI_SENSORTAG_MOVEMENT_SERVICE) { + enableMovement(false); + } else if (uuid == TI_SENSORTAG_ACCELEROMETER_SERVICE) { + enableMovement(false); + accelerometerEnabled = false; + } else if (uuid == TI_SENSORTAG_MAGNETOMETER_SERVICE) { + enableMovement(false); + magnetometerEnabled = false; + } else if (uuid == TI_SENSORTAG_GYROSCOPE_SERVICE) { + enableMovement(false); + gyroscopeEnabled = false; + } +} + +void SensorTagBasePrivate::updateCharacteristic(const QLowEnergyCharacteristic &c, + const QByteArray &value) +{ + if (c.uuid() == TI_SENSORTAG_LIGHT_DATA) { + convertLux(value); + } else if (c.uuid()== TI_SENSORTAG_IR_TEMPERATURE_DATA) { + convertTemperature(value); + } else if (c.uuid() == TI_SENSORTAG_BAROMETER_DATA) { + convertBarometer(value); + } else if (c.uuid()== TI_SENSORTAG_HUMIDTIY_DATA) { + convertHumidity(value); + } else if (c.uuid()== TI_SENSORTAG_BAROMETER_DATA) { + convertBarometer(value); + } else if ((c.uuid() == TI_SENSORTAG_ACCELEROMETER_DATA + || c.uuid() == TI_SENSORTAG_MOVEMENT_DATA) && accelerometerEnabled) { + convertAccelerometer(value); + } else if ((c.uuid() == TI_SENSORTAG_MAGNETOMETER_DATA + || c.uuid()== TI_SENSORTAG_MOVEMENT_DATA) && magnetometerEnabled) { + convertMagnetometer(value); + } else if ((c.uuid() == TI_SENSORTAG_GYROSCOPE_DATA + || c.uuid() == TI_SENSORTAG_MOVEMENT_DATA) && gyroscopeEnabled) { + convertGyroscope(value); + } +} + +void SensorTagBasePrivate::convertLux(const QByteArray &bytes) +{ + if (bytes.size() < 1) + return; + + quint16 dat = ((quint16)bytes[1] & 0xFF) << 8; + dat |= (quint16)(bytes[0] & 0xFF); + + qreal lux = dat * .01; + + emit luxDataAvailable(lux); +} + +void SensorTagBasePrivate::convertTemperature(const QByteArray &bytes) +{ + if (bytes.size() < 3) + return; + + qint16 objTemp = ((bytes[0] & 0xff) | ((bytes[1] << 8) & 0xff00)); + objTemp >>= 2; + qreal objectTemperature = objTemp * 0.03125; + // change to this if you want to use the ambient/die temp sensor + // qreal ambientTemperature = ambTemp / 128.0; + + emit tempDataAvailable(objectTemperature); +} + +void SensorTagBasePrivate::convertHumidity(const QByteArray &bytes) +{ + if (bytes.size() < 3) + return; + quint16 rawH = (bytes[2] & 0xff) | ((bytes[3] << 8) & 0xff00); + qreal rHumidity = (qreal)(rawH / 65535) * 100.0; + + emit humidityDataAvailable(rHumidity); +} + +void SensorTagBasePrivate::convertBarometer(const QByteArray &bytes) +{ + if (bytes.size() < 5) + return; + quint32 pressure = (bytes[3] & 0xff) | ((bytes[4] << 8) & 0xff00) | ((bytes[5] << 16) & 0xff0000); + + qreal mbars = (qreal)pressure / 100.0; + emit pressureDataAvailable(mbars); +} + +void SensorTagBasePrivate::convertAccelerometer(const QByteArray &bytes) +{ + if (bytes.size() < 3) + return; + + int range = 8; + + qint16 X = (qint16)((bytes[8]) + ((bytes[9] << 8))); + qint16 Y = (qint16)((bytes[6]) + ((bytes[7] << 8))); + qint16 Z = (qint16)((bytes[10]) + ((bytes[11] << 8))); + + accelReading.setX((qreal)(X * 1.0) / (32768 / range) * 9.80665); + accelReading.setY(-(qreal)(Y * 1.0) / (32768 / range) * 9.80665); + accelReading.setZ((qreal)(Z * 1.0) / (32768 / range) * 9.80665); + // TODO needs calibration + + emit accelDataAvailable(accelReading); +} + +void SensorTagBasePrivate::convertMagnetometer(const QByteArray &bytes) +{ + if (bytes.size() < 3) + return; + + qreal scale = 6.67100977199; // 32768 / 4912; + qint16 X = (qint16)((bytes[12]) + ((bytes[13] << 8))); + qint16 Y = (qint16)((bytes[14]) + ((bytes[15] << 8))); + qint16 Z = (qint16)((bytes[16]) + ((bytes[17] << 8))); + // TODO needs calibration + + magReading.setX((qreal)(X / scale)); + magReading.setY((qreal)(Y / scale)); + magReading.setZ((qreal)(Z / scale)); + + emit magDataAvailable(magReading); +} + +void SensorTagBasePrivate::convertGyroscope(const QByteArray &bytes) +{ + if (bytes.size() < 3) + return; + + qreal scale = 128.0; + qint16 X = (qint16)((bytes[2]) + ((bytes[3] << 8))); + qint16 Y = (qint16)((bytes[0]) + ((bytes[1] << 8))); + qint16 Z = (qint16)((bytes[4]) + ((bytes[5] << 8))); + + gyroReading.setX((qreal)(X / scale)); + gyroReading.setY((qreal)(Y / scale)); + gyroReading.setZ((qreal)(Z / scale)); + + emit gyroDataAvailable(gyroReading); +} + +SensorTagBasePrivate * SensorTagBasePrivate::instance() +{ + SensorTagBasePrivate *priv = sensortagBasePrivate(); + return priv; +} + +SensorTagBase::SensorTagBase(QSensor *sensor) + : QSensorBackend(sensor), + leService(nullptr), + serviceId(nullptr), + d_ptr(SensorTagBasePrivate::instance()) +{ + connect(d_ptr, &SensorTagBasePrivate::luxDataAvailable, + this, &SensorTagBase::luxDataAvailable); + connect(d_ptr, &SensorTagBasePrivate::tempDataAvailable, + this, &SensorTagBase::tempDataAvailable); + connect(d_ptr, &SensorTagBasePrivate::humidityDataAvailable, + this, &SensorTagBase::humidityDataAvailable); + connect(d_ptr, &SensorTagBasePrivate::pressureDataAvailable, + this, &SensorTagBase::pressureDataAvailable); + connect(d_ptr, &SensorTagBasePrivate::accelDataAvailable, + this, &SensorTagBase::accelDataAvailable); + connect(d_ptr, &SensorTagBasePrivate::gyroDataAvailable, + this, &SensorTagBase::gyroDataAvailable); + connect(d_ptr, &SensorTagBasePrivate::magDataAvailable, + this, &SensorTagBase::magDataAvailable); +} + +SensorTagBase::~SensorTagBase() +{ +} + +void SensorTagBase::start() +{ +} + +void SensorTagBase::stop() +{ +} + +quint64 SensorTagBase::produceTimestamp() +{ + return QDeadlineTimer::current().deadlineNSecs() / 1000; +} + + |