From 66398c3a296a71522ffa63c3da20181cf17c5c77 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Simon=20St=C3=BCrz?= Date: Fri, 23 Feb 2018 17:52:24 +0100 Subject: [PATCH] Add motion sensor parsing --- multisensor/devicepluginmultisensor.cpp | 47 +++++++++- multisensor/devicepluginmultisensor.h | 2 + multisensor/devicepluginmultisensor.json | 28 ++++-- multisensor/sensortag.cpp | 113 +++++++++++++++++++++-- multisensor/sensortag.h | 5 + 5 files changed, 176 insertions(+), 19 deletions(-) diff --git a/multisensor/devicepluginmultisensor.cpp b/multisensor/devicepluginmultisensor.cpp index e1e9aadb..df77c2da 100644 --- a/multisensor/devicepluginmultisensor.cpp +++ b/multisensor/devicepluginmultisensor.cpp @@ -59,6 +59,8 @@ DevicePluginMultiSensor::~DevicePluginMultiSensor() void DevicePluginMultiSensor::init() { + connect(this, &DevicePluginMultiSensor::configValueChanged, this, &DevicePluginMultiSensor::onPluginConfigurationChanged); + m_reconnectTimer = hardwareManager()->pluginTimerManager()->registerTimer(10); connect(m_reconnectTimer, &PluginTimer::timeout, this, &DevicePluginMultiSensor::onPluginTimer); } @@ -94,15 +96,27 @@ DeviceManager::DeviceSetupStatus DevicePluginMultiSensor::setupDevice(Device *de BluetoothLowEnergyDevice *bluetoothDevice = hardwareManager()->bluetoothLowEnergyManager()->registerDevice(deviceInfo, QLowEnergyController::PublicAddress); SensorTag *sensor = new SensorTag(device, bluetoothDevice, this); + // Preconfigure with plugin configurations + sensor->setAccelerometerEnabled(configValue(MultiSensorAccelerometerEnabledParamTypeId).toBool()); + sensor->setGyroscopeEnabled(configValue(MultiSensorGyroscopeEnabledParamTypeId).toBool()); + sensor->setMagnetometerEnabled(configValue(MultiSensorMagnetometerEnabledParamTypeId).toBool()); + sensor->setMeasurementPeriod(configValue(MultiSensorMeasurementPeriodParamTypeId).toInt()); + sensor->setMeasurementPeriodMovement(configValue(MultiSensorMeasurementPeriodMovementParamTypeId).toInt()); m_sensors.insert(device, sensor); - sensor->bluetoothDevice()->connectDevice(); return DeviceManager::DeviceSetupStatusSuccess; } return DeviceManager::DeviceSetupStatusFailure; } +void DevicePluginMultiSensor::postSetupDevice(Device *device) +{ + // Try to connect right after setup + SensorTag *sensor = m_sensors.value(device); + sensor->bluetoothDevice()->connectDevice(); +} + void DevicePluginMultiSensor::deviceRemoved(Device *device) { @@ -161,3 +175,34 @@ void DevicePluginMultiSensor::onBluetoothDiscoveryFinished() reply->deleteLater(); emit devicesDiscovered(sensortagDeviceClassId, deviceDescriptors); } + +void DevicePluginMultiSensor::onPluginConfigurationChanged(const ParamTypeId ¶mTypeId, const QVariant &value) +{ + qCDebug(dcMultiSensor()) << "Plugin configuration changed" << paramTypeId.toString() << value; + + foreach (SensorTag *sensor, m_sensors.values()) { + if (paramTypeId == MultiSensorAccelerometerEnabledParamTypeId) { + sensor->setAccelerometerEnabled(value.toBool()); + } else if (paramTypeId == MultiSensorGyroscopeEnabledParamTypeId) { + sensor->setGyroscopeEnabled(value.toBool()); + } else if (paramTypeId == MultiSensorMagnetometerEnabledParamTypeId) { + sensor->setMagnetometerEnabled(value.toBool()); + } else if (paramTypeId == MultiSensorMeasurementPeriodParamTypeId) { + int valueInt = value.toInt(); + if (valueInt % 10 != 0) { + qCWarning(dcMultiSensor()) << "Measurement period of sensors" << valueInt << "must be a multiple of 10ms"; + // FIXME: force to valid value + return; + } + sensor->setMeasurementPeriod(valueInt); + } else if (paramTypeId == MultiSensorMeasurementPeriodParamTypeId) { + int valueInt = value.toInt(); + if (valueInt % 10 != 0) { + qCWarning(dcMultiSensor()) << "Measurement period of movement sensor" << valueInt << "must be a multiple of 10ms"; + // FIXME: force to valid value + return; + } + sensor->setMeasurementPeriodMovement(valueInt); + } + } +} diff --git a/multisensor/devicepluginmultisensor.h b/multisensor/devicepluginmultisensor.h index 22206770..91e055ca 100644 --- a/multisensor/devicepluginmultisensor.h +++ b/multisensor/devicepluginmultisensor.h @@ -46,6 +46,7 @@ public: void init() override; DeviceManager::DeviceError discoverDevices(const DeviceClassId &deviceClassId, const ParamList ¶ms) override; DeviceManager::DeviceSetupStatus setupDevice(Device *device) override; + void postSetupDevice(Device *device) override; void deviceRemoved(Device *device) override; private: @@ -57,6 +58,7 @@ private: private slots: void onPluginTimer(); void onBluetoothDiscoveryFinished(); + void onPluginConfigurationChanged(const ParamTypeId ¶mTypeId, const QVariant &value); }; #endif // DEVICEPLUGINMULTISENSOR_H diff --git a/multisensor/devicepluginmultisensor.json b/multisensor/devicepluginmultisensor.json index f71831a8..64f71a2d 100644 --- a/multisensor/devicepluginmultisensor.json +++ b/multisensor/devicepluginmultisensor.json @@ -24,15 +24,6 @@ "type": "bool", "defaultValue": false }, - { - "id": "5237c89c-c21d-4d78-ac99-8be661b40da7", - "name": "measurementPeriodMovement", - "displayName": "Measurement period movement sensor", - "type": "int", - "minValue": 10, - "maxValue": 2500, - "defaultValue": 2000 - }, { "id": "c5308565-5dc9-409e-ae99-577c212c7a92", "name": "measurementPeriod", @@ -40,7 +31,26 @@ "type": "int", "minValue": 10, "maxValue": 2500, + "defaultValue": 2000 + }, + { + "id": "5237c89c-c21d-4d78-ac99-8be661b40da7", + "name": "measurementPeriodMovement", + "displayName": "Measurement period movement sensor", + "type": "int", + "minValue": 10, + "maxValue": 2500, "defaultValue": 500 + }, + { + "id": "a3298d16-eea6-4474-9061-90466e92d476", + "name": "movementSensitivity", + "displayName": "Movement sensitivity", + "type": "int", + "unit": "Percentage", + "minValue": 0, + "maxValue": 100, + "defaultValue": 50 } ], "vendors": [ diff --git a/multisensor/sensortag.cpp b/multisensor/sensortag.cpp index 2b075e6c..11555462 100644 --- a/multisensor/sensortag.cpp +++ b/multisensor/sensortag.cpp @@ -24,6 +24,7 @@ #include "extern-plugininfo.h" #include "math.h" +#include #include SensorTag::SensorTag(Device *device, BluetoothLowEnergyDevice *bluetoothDevice, QObject *parent) : @@ -47,7 +48,72 @@ BluetoothLowEnergyDevice *SensorTag::bluetoothDevice() void SensorTag::setAccelerometerEnabled(bool enabled) { + qCDebug(dcMultiSensor()) << "Accelerometer" << (enabled ? "enabled" : "disabled"); + + if (m_accelerometerEnabled == enabled) + return; + m_accelerometerEnabled = enabled; + configureMovement(m_gyroscopeEnabled, m_accelerometerEnabled, m_magnetometerEnabled, true); +} + +void SensorTag::setGyroscopeEnabled(bool enabled) +{ + qCDebug(dcMultiSensor()) << "Gyroscope" << (enabled ? "enabled" : "disabled"); + + if (m_gyroscopeEnabled == enabled) + return; + + m_gyroscopeEnabled = enabled; + configureMovement(m_gyroscopeEnabled, m_accelerometerEnabled, m_magnetometerEnabled, true); +} + +void SensorTag::setMagnetometerEnabled(bool enabled) +{ + qCDebug(dcMultiSensor()) << "Magnetometer" << (enabled ? "enabled" : "disabled"); + + if (m_magnetometerEnabled == enabled) + return; + + m_magnetometerEnabled = enabled; + configureMovement(m_gyroscopeEnabled, m_accelerometerEnabled, m_magnetometerEnabled, true); +} + +void SensorTag::setMeasurementPeriod(int period) +{ + qCDebug(dcMultiSensor()) << "Set sensor measurement period to" << period << "ms"; + + m_temperaturePeriod = period; + if (m_temperatureService && m_temperaturePeriodCharacteristic.isValid()) + configurePeriod(m_temperatureService, m_temperaturePeriodCharacteristic, m_temperaturePeriod); + + m_humidityPeriod = period; + if (m_humidityService && m_humidityPeriodCharacteristic.isValid()) + configurePeriod(m_humidityService, m_humidityPeriodCharacteristic, m_humidityPeriod); + + m_pressurePeriod = period; + if (m_pressureService && m_pressurePeriodCharacteristic.isValid()) + configurePeriod(m_pressureService, m_pressurePeriodCharacteristic, m_pressurePeriod); + + m_opticalPeriod = period; + if (m_opticalService && m_opticalPeriodCharacteristic.isValid()) + configurePeriod(m_opticalService, m_opticalPeriodCharacteristic, m_opticalPeriod); + +} + +void SensorTag::setMeasurementPeriodMovement(int period) +{ + qCDebug(dcMultiSensor()) << "Set movement sensor measurement period to" << period << "ms"; + + m_movementPeriod = period; + if (m_movementService && m_movementPeriodCharacteristic.isValid()) + configurePeriod(m_movementService, m_movementPeriodCharacteristic, m_movementPeriod); + +} + +void SensorTag::setMovementSensitivity(int percentage) +{ + m_movementSensitivity = static_cast(percentage) / 100.0; } void SensorTag::configurePeriod(QLowEnergyService *serice, const QLowEnergyCharacteristic &characteristic, int measurementPeriod) @@ -87,12 +153,10 @@ void SensorTag::configureMovement(bool gyroscopeEnabled, bool accelerometerEnabl configuration |= (1 << 8); // enable } - // Accelerometer 2 Bit ( 0 = 2G, 1 = 4G, 2 = 8G, 3 = 16G) - // Default 0 - + // Accelerometer 2 Bit ( 0 = 2G, 1 = 4G, 2 = 8G, 3 = 16G) - Default 0 QByteArray data; QDataStream stream(&data, QIODevice::WriteOnly); - stream.setByteOrder(QDataStream::BigEndian); + stream.setByteOrder(QDataStream::LittleEndian); stream << configuration; qCDebug(dcMultiSensor()) << "Configure movement sensor" << data.toHex(); @@ -143,6 +207,7 @@ void SensorTag::processHumidityData(const QByteArray &data) stream.setByteOrder(QDataStream::LittleEndian); stream >> rawHumidityTemperature >> rawHumidity ; + // Note: we don't need the temperature measurement from the humidity sensor //double humidityTemperature = (rawHumidityTemperature / 65536.0 * 165.0) - 40; double humidity = rawHumidity / 65536.0 * 100.0; @@ -154,6 +219,13 @@ void SensorTag::processPressureData(const QByteArray &data) { Q_ASSERT(data.count() == 6); + QByteArray temperatureData(data.left(3)); + quint32 rawTemperature = static_cast(temperatureData.at(2)); + rawTemperature <<= 8; + rawTemperature |= static_cast(temperatureData.at(1)); + rawTemperature <<= 8; + rawTemperature |= static_cast(temperatureData.at(0)); + QByteArray pressureData(data.right(3)); quint32 rawPressure = static_cast(pressureData.at(2)); rawPressure <<= 8; @@ -161,6 +233,8 @@ void SensorTag::processPressureData(const QByteArray &data) rawPressure <<= 8; rawPressure |= static_cast(pressureData.at(0)); + // Note: we don't need the temperature measurement from the barometic pressure sensor + //qCDebug(dcMultiSensor()) << "Pressure temperature:" << roundValue(rawTemperature / 100.0) << "°C"; //qCDebug(dcMultiSensor()) << "Pressure:" << roundValue(rawPressure / 100.0) << "mBar"; m_device->setStateValue(sensortagPressureStateTypeId, roundValue(rawPressure / 100.0)); } @@ -186,10 +260,11 @@ void SensorTag::processOpticalData(const QByteArray &data) void SensorTag::processMovementData(const QByteArray &data) { - qCDebug(dcMultiSensor()) << "Movement value" << data.toHex(); + qCDebug(dcMultiSensor()) << "--> Movement value" << data.toHex(); QByteArray payload(data); QDataStream stream(&payload, QIODevice::ReadOnly); + stream.setByteOrder(QDataStream::LittleEndian); qint16 gyroXRaw = 0; qint16 gyroYRaw = 0; qint16 gyroZRaw = 0; stream >> gyroXRaw >> gyroYRaw >> gyroZRaw; @@ -201,12 +276,33 @@ void SensorTag::processMovementData(const QByteArray &data) stream >> magXRaw >> magYRaw >> magZRaw; - // Calculate rotation, unit deg/s, range -250, +250 + // Calculate rotation [deg/s], Range +- 250 double gyroX = static_cast(gyroXRaw) / (65536 / 500); double gyroY = static_cast(gyroYRaw) / (65536 / 500); double gyroZ = static_cast(gyroZRaw) / (65536 / 500); - qCDebug(dcMultiSensor()) << "Gyroscope x:" << gyroX << " y:" << gyroY << " z:" << gyroZ; + double accX = static_cast(accXRaw) / (32768 / m_accelerometerRange); + double accY = static_cast(accYRaw) / (32768 / m_accelerometerRange); + double accZ = static_cast(accZRaw) / (32768 / m_accelerometerRange); + + // Calculate magnetism [uT], Range +- 4900 + double magX = static_cast(magXRaw); + double magY = static_cast(magYRaw); + double magZ = static_cast(magZRaw); + + + qCDebug(dcMultiSensor()) << "Accelerometer x:" << accX << " y:" << accY << " z:" << accZ; + //qCDebug(dcMultiSensor()) << "Gyroscope x:" << gyroX << " y:" << gyroY << " z:" << gyroZ; + //qCDebug(dcMultiSensor()) << "Magnetometer x:" << magX << " y:" << magY << " z:" << magZ; + + QVector3D accelerometerVector(accX, accY, accZ); + QVector3D gyroscopeVector(gyroX, gyroY, gyroZ); + QVector3D magnetometerVector(magX, magY, magZ); + + Q_UNUSED(gyroscopeVector) + Q_UNUSED(magnetometerVector) + + qCDebug(dcMultiSensor()) << accelerometerVector.length(); } void SensorTag::setLeftButtonPressed(bool pressed) @@ -318,7 +414,6 @@ void SensorTag::onServiceDiscoveryFinished() return; } - // IR Temperature if (!m_temperatureService) { m_temperatureService = m_bluetoothDevice->controller()->createServiceObject(temperatureServiceUuid, this); @@ -731,7 +826,7 @@ void SensorTag::onMovementServiceStateChanged(const QLowEnergyService::ServiceSt // Set measurement period configurePeriod(m_movementService, m_movementPeriodCharacteristic, m_movementPeriod); - configureMovement(); + configureMovement(m_gyroscopeEnabled, m_accelerometerEnabled, m_magnetometerEnabled, true); // Enable measuring m_movementService->writeCharacteristic(m_movementConfigurationCharacteristic, QByteArray::fromHex("01")); diff --git a/multisensor/sensortag.h b/multisensor/sensortag.h index ad98702b..29c17fae 100644 --- a/multisensor/sensortag.h +++ b/multisensor/sensortag.h @@ -77,6 +77,8 @@ public: void setGyroscopeEnabled(bool enabled); void setMagnetometerEnabled(bool enabled); void setMeasurementPeriod(int period); + void setMeasurementPeriodMovement(int period); + void setMovementSensitivity(int percentage); private: Device *m_device; @@ -119,6 +121,9 @@ private: int m_pressurePeriod = 2500; int m_opticalPeriod = 2500; int m_movementPeriod = 500; + double m_movementSensitivity = 50; + // Note: possible value: 2G, 4G, 8G, 16G, default 2G + int m_accelerometerRange = 2; bool m_leftButtonPressed = false; bool m_rightButtonPressed = false;