diff --git a/multisensor/devicepluginmultisensor.cpp b/multisensor/devicepluginmultisensor.cpp index e356dfd8..0a391cde 100644 --- a/multisensor/devicepluginmultisensor.cpp +++ b/multisensor/devicepluginmultisensor.cpp @@ -97,6 +97,10 @@ DeviceManager::DeviceSetupStatus DevicePluginMultiSensor::setupDevice(Device *de SensorTag *sensor = new SensorTag(device, bluetoothDevice, this); // Preconfigure with plugin configurations + sensor->setTemperatureSensorEnabled(configValue(MultiSensorTemperatureEnabledParamTypeId).toBool()); + sensor->setHumiditySensorEnabled(configValue(MultiSensorHumidityEnabledParamTypeId).toBool()); + sensor->setPressureSensorEnabled(configValue(MultiSensorPressureEnabledParamTypeId).toBool()); + sensor->setOpticalSensorEnabled(configValue(MultiSensorOpticalEnabledParamTypeId).toBool()); sensor->setAccelerometerEnabled(configValue(MultiSensorAccelerometerEnabledParamTypeId).toBool()); sensor->setGyroscopeEnabled(configValue(MultiSensorGyroscopeEnabledParamTypeId).toBool()); sensor->setMagnetometerEnabled(configValue(MultiSensorMagnetometerEnabledParamTypeId).toBool()); @@ -200,7 +204,15 @@ void DevicePluginMultiSensor::onPluginConfigurationChanged(const ParamTypeId &pa qCDebug(dcMultiSensor()) << "Plugin configuration changed" << paramTypeId.toString() << value; foreach (SensorTag *sensor, m_sensors.values()) { - if (paramTypeId == MultiSensorAccelerometerEnabledParamTypeId) { + if (paramTypeId == MultiSensorTemperatureEnabledParamTypeId) { + sensor->setTemperatureSensorEnabled(value.toBool()); + } else if (paramTypeId == MultiSensorHumidityEnabledParamTypeId) { + sensor->setHumiditySensorEnabled(value.toBool()); + } else if (paramTypeId == MultiSensorPressureEnabledParamTypeId) { + sensor->setPressureSensorEnabled(value.toBool()); + } else if (paramTypeId == MultiSensorOpticalEnabledParamTypeId) { + sensor->setOpticalSensorEnabled(value.toBool()); + } else if (paramTypeId == MultiSensorAccelerometerEnabledParamTypeId) { sensor->setAccelerometerEnabled(value.toBool()); } else if (paramTypeId == MultiSensorGyroscopeEnabledParamTypeId) { sensor->setGyroscopeEnabled(value.toBool()); diff --git a/multisensor/devicepluginmultisensor.json b/multisensor/devicepluginmultisensor.json index af761ba6..1a2e3689 100644 --- a/multisensor/devicepluginmultisensor.json +++ b/multisensor/devicepluginmultisensor.json @@ -3,6 +3,34 @@ "name": "MultiSensor", "id": "23bf0d8d-df7d-4bee-80ee-f015c5a7f52e", "paramTypes": [ + { + "id": "6f0e40a2-0f97-4c1e-8229-757e3c18b345", + "name": "temperatureEnabled", + "displayName": "Temperature sensor enabled", + "type": "bool", + "defaultValue": true + }, + { + "id": "29881172-626a-42d5-83b8-3e2e5ca533be", + "name": "humidityEnabled", + "displayName": "Humidity sensor enabled", + "type": "bool", + "defaultValue": true + }, + { + "id": "50e5e282-9707-4b31-bb3f-a6ca30a7e1ff", + "name": "pressureEnabled", + "displayName": "Pressure sensor enabled", + "type": "bool", + "defaultValue": true + }, + { + "id": "1460a6d6-9fb4-4385-b27b-ee4b7594e454", + "name": "opticalEnabled", + "displayName": "Optical sensor enabled", + "type": "bool", + "defaultValue": true + }, { "id": "5786c91a-d94d-461a-8d22-f978dd1438ab", "name": "accelerometerEnabled", diff --git a/multisensor/multisensor.plot b/multisensor/multisensor.plot new file mode 100644 index 00000000..3bcf24b4 --- /dev/null +++ b/multisensor/multisensor.plot @@ -0,0 +1,15 @@ +# Autocreated plot script for system disk usage +set term png small size 2000,1000 +set output "multisensor.png" +set title "Multisensor filter" +set xlabel "Time" +set ylabel "Value" +set xdata time +set timefmt "%s" +set xtics format "%H:%M:%S" +set yrange [0:*] +set grid + +plot "/tmp/multisensor.log" using 1:2 with lines title "Original value", \ + "/tmp/multisensor.log" using 1:3 with lines title "Filtered value" + diff --git a/multisensor/multisensor.pro b/multisensor/multisensor.pro index 83fe6c9b..86cba0f8 100644 --- a/multisensor/multisensor.pro +++ b/multisensor/multisensor.pro @@ -5,9 +5,13 @@ TARGET = $$qtLibraryTarget(nymea_devicepluginmultisensor) SOURCES += \ devicepluginmultisensor.cpp \ #sensortag-old.cpp \ - sensortag.cpp + sensortag.cpp \ + sensorfilter.cpp \ + sensordataprocessor.cpp HEADERS += \ devicepluginmultisensor.h \ #sensortag-old.h \ - sensortag.h + sensortag.h \ + sensorfilter.h \ + sensordataprocessor.h diff --git a/multisensor/sensordataprocessor.cpp b/multisensor/sensordataprocessor.cpp new file mode 100644 index 00000000..ca841882 --- /dev/null +++ b/multisensor/sensordataprocessor.cpp @@ -0,0 +1,307 @@ +#include "sensordataprocessor.h" +#include "extern-plugininfo.h" +#include "math.h" + +#include +#include +#include +#include + +SensorDataProcessor::SensorDataProcessor(Device *device, QObject *parent) : + QObject(parent), + m_device(device) +{ + // Create data filters + m_temperatureFilter = new SensorFilter(SensorFilter::TypeLowPass, this); + m_temperatureFilter->setLowPassAlpha(0.1); + m_temperatureFilter->setFilterWindowSize(30); + + m_objectTemperatureFilter = new SensorFilter(SensorFilter::TypeLowPass, this); + m_objectTemperatureFilter->setLowPassAlpha(0.4); + m_objectTemperatureFilter->setFilterWindowSize(20); + + m_humidityFilter = new SensorFilter(SensorFilter::TypeLowPass, this); + m_humidityFilter->setLowPassAlpha(0.1); + m_humidityFilter->setFilterWindowSize(30); + + m_pressureFilter = new SensorFilter(SensorFilter::TypeLowPass, this); + m_pressureFilter->setLowPassAlpha(0.1); + m_pressureFilter->setFilterWindowSize(30); + + m_opticalFilter = new SensorFilter(SensorFilter::TypeLowPass, this); + m_opticalFilter->setLowPassAlpha(0.01); + m_opticalFilter->setFilterWindowSize(10); + + m_accelerometerFilter = new SensorFilter(SensorFilter::TypeLowPass, this); + m_accelerometerFilter->setLowPassAlpha(0.6); + m_accelerometerFilter->setFilterWindowSize(40); + + // Check if the data should be logged + if (m_filterDebug) { + m_logFile = new QFile("/tmp/multisensor.log", this); + if (!m_logFile->open(QIODevice::Append | QIODevice::Text)) { + qCWarning(dcMultiSensor()) << "Could not open log file" << m_logFile->fileName(); + delete m_logFile; + m_logFile = nullptr; + } + } +} + +SensorDataProcessor::~SensorDataProcessor() +{ + if (m_logFile) { + m_logFile->close(); + } +} + +void SensorDataProcessor::setAccelerometerRange(int accelerometerRange) +{ + m_accelerometerRange = accelerometerRange; +} + +void SensorDataProcessor::setMovementSensitivity(int movementSensitivity) +{ + m_movementSensitivity = movementSensitivity; +} + +double SensorDataProcessor::roundValue(float value) +{ + int tmpValue = static_cast(value * 10); + return static_cast(tmpValue) / 10.0; +} + +bool SensorDataProcessor::testBitUint8(quint8 value, int bitPosition) +{ + return (((value)>> (bitPosition)) & 1); +} + +void SensorDataProcessor::processTemperatureData(const QByteArray &data) +{ + Q_ASSERT(data.count() == 4); + + quint16 rawObjectTemperature = 0; + quint16 rawAmbientTemperature = 0; + + QByteArray payload(data); + QDataStream stream(&payload, QIODevice::ReadOnly); + stream.setByteOrder(QDataStream::LittleEndian); + stream >> rawObjectTemperature >> rawAmbientTemperature ; + + double scaleFactor = 0.03125; + double objectTemperature = static_cast(rawObjectTemperature) / 4 * scaleFactor; + double ambientTemperature = static_cast(rawAmbientTemperature) / 4 * scaleFactor; + + //qCDebug(dcMultiSensor()) << "Temperature value" << data.toHex(); + //qCDebug(dcMultiSensor()) << "Object temperature" << roundValue(objectTemperature) << "°C"; + //qCDebug(dcMultiSensor()) << "Ambient temperature" << roundValue(ambientTemperature) << "°C"; + + double objectTemperatureFiltered = m_objectTemperatureFilter->filterValue(objectTemperature); + double ambientTemperatureFiltered = m_temperatureFilter->filterValue(ambientTemperature); + + if (m_objectTemperatureFilter->isReady()) { + m_device->setStateValue(sensortagObjectTemperatureStateTypeId, roundValue(objectTemperatureFiltered)); + } + + // Note: only change the state once the filter has collected enought data + if (m_temperatureFilter->isReady()) { + m_device->setStateValue(sensortagTemperatureStateTypeId, roundValue(ambientTemperatureFiltered)); + } + +} + +void SensorDataProcessor::processKeyData(const QByteArray &data) +{ + Q_ASSERT(data.count() == 1); + quint8 flags = static_cast(data.at(0)); + setLeftButtonPressed(testBitUint8(flags, 0)); + setRightButtonPressed(testBitUint8(flags, 1)); + setMagnetDetected(testBitUint8(flags, 2)); +} + +void SensorDataProcessor::processHumidityData(const QByteArray &data) +{ + Q_ASSERT(data.count() == 4); + quint16 rawHumidityTemperature = 0; + quint16 rawHumidity = 0; + + QByteArray payload(data); + QDataStream stream(&payload, QIODevice::ReadOnly); + 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; + double humidityFiltered = m_humidityFilter->filterValue(humidity); + + if (m_humidityFilter->isReady()) { + m_device->setStateValue(sensortagHumidityStateTypeId, roundValue(humidityFiltered)); + } +} + +void SensorDataProcessor::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; + rawPressure |= static_cast(pressureData.at(1)); + 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"; + + double pressureFiltered = m_pressureFilter->filterValue(rawPressure / 100.0); + if (m_pressureFilter->isReady()) { + m_device->setStateValue(sensortagPressureStateTypeId, roundValue(pressureFiltered)); + } +} + +void SensorDataProcessor::processOpticalData(const QByteArray &data) +{ + Q_ASSERT(data.count() == 2); + + quint16 rawOptical = 0; + QByteArray payload(data); + QDataStream stream(&payload, QIODevice::ReadOnly); + stream.setByteOrder(QDataStream::LittleEndian); + stream >> rawOptical; + + quint16 lumm = rawOptical & 0x0FFF; + quint16 lume = (rawOptical & 0xF000) >> 12; + + double lux = lumm * (0.01 * pow(2,lume)); + + //qCDebug(dcMultiSensor()) << "Lux:" << lux; + + double luxFiltered = m_opticalFilter->filterValue(lux); + if (m_opticalFilter->isReady()) { + m_device->setStateValue(sensortagLightIntensityStateTypeId, qRound(luxFiltered)); + } + + logSensorValue(lux, qRound(luxFiltered)); +} + +void SensorDataProcessor::processMovementData(const QByteArray &data) +{ + //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; + + qint16 accXRaw = 0; qint16 accYRaw = 0; qint16 accZRaw = 0; + stream >> accXRaw >> accYRaw >> accZRaw; + + qint16 magXRaw = 0; qint16 magYRaw = 0; qint16 magZRaw = 0; + stream >> magXRaw >> magYRaw >> magZRaw; + + + // 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); + + // Calculate acceleration [G], Range +- m_accelerometerRange + double accX = static_cast(accXRaw) / (32768 / static_cast(m_accelerometerRange)); + double accY = static_cast(accYRaw) / (32768 / static_cast(m_accelerometerRange)); + double accZ = static_cast(accZRaw) / (32768 / static_cast(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) + + double filteredVectorLength = m_accelerometerFilter->filterValue(accelerometerVector.length()); + + // Initialize the accelerometer value if no data known yet + if (m_lastAccelerometerVectorLenght == -99999) { + m_lastAccelerometerVectorLenght = filteredVectorLength; + return; + } + + double delta = qAbs(qAbs(m_lastAccelerometerVectorLenght) - qAbs(filteredVectorLength)); + bool motionDetected = (delta >= m_movementSensitivity); + m_device->setStateValue(sensortagMovingStateTypeId, motionDetected); + m_lastAccelerometerVectorLenght = filteredVectorLength; +} + +void SensorDataProcessor::reset() +{ + m_lastAccelerometerVectorLenght = -99999; + + // Reset data filters + m_temperatureFilter->reset(); + m_objectTemperatureFilter->reset(); + m_humidityFilter->reset(); + m_pressureFilter->reset(); + m_opticalFilter->reset(); + m_accelerometerFilter->reset(); +} + +void SensorDataProcessor::setLeftButtonPressed(bool pressed) +{ + if (m_leftButtonPressed == pressed) + return; + + qCDebug(dcMultiSensor()) << "Left button" << (pressed ? "pressed" : "released"); + m_leftButtonPressed = pressed; + m_device->setStateValue(sensortagLeftButtonPressedStateTypeId, m_leftButtonPressed); +} + +void SensorDataProcessor::setRightButtonPressed(bool pressed) +{ + if (m_rightButtonPressed == pressed) + return; + + qCDebug(dcMultiSensor()) << "Right button" << (pressed ? "pressed" : "released"); + m_rightButtonPressed = pressed; + m_device->setStateValue(sensortagRightButtonPressedStateTypeId, m_rightButtonPressed); +} + +void SensorDataProcessor::setMagnetDetected(bool detected) +{ + if (m_magnetDetected == detected) + return; + + qCDebug(dcMultiSensor()) << "Magnet detector" << (detected ? "active" : "inactive"); + m_magnetDetected = detected; + m_device->setStateValue(sensortagMagnetDetectedStateTypeId, m_magnetDetected); +} + +void SensorDataProcessor::logSensorValue(double originalValue, double filteredValue) +{ + if (!m_filterDebug || !m_logFile) + return; + + QString logLine = QString("%1 %2 %3\n").arg(QDateTime::currentDateTime().toTime_t()).arg(originalValue).arg(filteredValue); + + QTextStream logStream(m_logFile); + logStream.setCodec("UTF-8"); + logStream << logLine; +} diff --git a/multisensor/sensordataprocessor.h b/multisensor/sensordataprocessor.h new file mode 100644 index 00000000..aba7fe94 --- /dev/null +++ b/multisensor/sensordataprocessor.h @@ -0,0 +1,71 @@ +#ifndef SENSORDATAPROCESSOR_H +#define SENSORDATAPROCESSOR_H + +#include +#include + +#include "plugin/device.h" +#include "extern-plugininfo.h" + +#include "sensorfilter.h" + +class SensorDataProcessor : public QObject +{ + Q_OBJECT +public: + explicit SensorDataProcessor(Device *device, QObject *parent = nullptr); + ~SensorDataProcessor(); + + void setAccelerometerRange(int accelerometerRange); + void setMovementSensitivity(int movementSensitivity); + + static double roundValue(float value); + static bool testBitUint8(quint8 value, int bitPosition); + + void processTemperatureData(const QByteArray &data); + void processKeyData(const QByteArray &data); + void processHumidityData(const QByteArray &data); + void processPressureData(const QByteArray &data); + void processOpticalData(const QByteArray &data); + void processMovementData(const QByteArray &data); + + void reset(); + +private: + Device *m_device = nullptr; + double m_lastAccelerometerVectorLenght = -99999; + + int m_accelerometerRange = 16; + double m_movementSensitivity = 0.5; + + bool m_leftButtonPressed = false; + bool m_rightButtonPressed = false; + bool m_magnetDetected = false; + + // Log sensor data for debugging filters + // Note: set this to true for enable sensor filter logging + bool m_filterDebug = true; + QFile *m_logFile = nullptr; + + SensorFilter *m_temperatureFilter = nullptr; + SensorFilter *m_objectTemperatureFilter = nullptr; + SensorFilter *m_humidityFilter = nullptr; + SensorFilter *m_pressureFilter = nullptr; + SensorFilter *m_opticalFilter = nullptr; + SensorFilter *m_accelerometerFilter = nullptr; + + // Set methods + void setLeftButtonPressed(bool pressed); + void setRightButtonPressed(bool pressed); + void setMagnetDetected(bool detected); + + void logSensorValue(double originalValue, double filteredValue); + + +signals: + +private slots: + +}; + +#endif // SENSORDATAPROCESSOR_H diff --git a/multisensor/sensorfilter.cpp b/multisensor/sensorfilter.cpp new file mode 100644 index 00000000..2a69dd0d --- /dev/null +++ b/multisensor/sensorfilter.cpp @@ -0,0 +1,156 @@ +#include "sensorfilter.h" + +#include + +SensorFilter::SensorFilter(Type filterType, QObject *parent) : + QObject(parent), + m_filterType(filterType) +{ + +} + +double SensorFilter::filterValue(double value) +{ + double resultValue = value; + switch (m_filterType) { + case TypeLowPass: + resultValue = lowPassFilterValue(value); + break; + case TypeHighPass: + resultValue = highPassFilterValue(value); + break; + case TypeAverage: + resultValue = averageFilterValue(value); + break; + default: + break; + } + + return resultValue; +} + +bool SensorFilter::isReady() const +{ + // Note: filter is ready once 10% of window filled + return m_inputData.size() >= m_filterWindowSize * 0.1; +} + +void SensorFilter::reset() +{ + m_averageSum = 0; + m_inputData.clear(); +} + +SensorFilter::Type SensorFilter::filterType() const +{ + return m_filterType; +} + +QVector SensorFilter::inputData() const +{ + return m_inputData; +} + +QVector SensorFilter::outputData() const +{ + return m_outputData; +} + +uint SensorFilter::windowSize() const +{ + return m_filterWindowSize; +} + +void SensorFilter::setFilterWindowSize(uint windowSize) +{ + Q_ASSERT_X(windowSize > 0, "value out of range", "The filter window size must be bigger than 0"); + m_filterWindowSize = windowSize; +} + +double SensorFilter::lowPassAlpha() const +{ + return m_lowPassAlpha; +} + +void SensorFilter::setLowPassAlpha(double alpha) +{ + Q_ASSERT_X(alpha > 0 && alpha <= 1, "value out of range", "The alpha low pass filter value must be [ 0 < alpha <= 1 ]"); + m_lowPassAlpha = alpha; +} + +double SensorFilter::highPassAlpha() const +{ + return m_highPassAlpha; +} + +void SensorFilter::setHighPassAlpha(double alpha) +{ + Q_ASSERT_X(alpha > 0 && alpha <= 1, "value out of range", "The alpha high pass filter value must be [ 0 < alpha <= 1 ]"); + m_highPassAlpha = alpha; +} + +void SensorFilter::addInputValue(double value) +{ + m_inputData.append(value); + if (m_inputData.size() > m_filterWindowSize) { + m_inputData.removeFirst(); + } +} + +double SensorFilter::lowPassFilterValue(double value) +{ + addInputValue(value); + + // Check if we have enought data for filtering + if (m_inputData.size() < 2) { + return value; + } + + QVector outputData; + outputData.append(m_inputData.at(0)); + for (int i = 1; i < m_inputData.size(); i++) { + // y[i] := y[i-1] + α * (x[i] - y[i-1]) + outputData.append(outputData.at(i - 1) + m_lowPassAlpha * (m_inputData.at(i) - outputData.at(i - 1))); + } + + m_outputData = outputData; + + return m_outputData.last(); +} + +double SensorFilter::highPassFilterValue(double value) +{ + addInputValue(value); + + // Check if we have enought data for filtering + if (m_inputData.size() < 2) { + return value; + } + + QVector outputData; + outputData.append(m_inputData.at(0)); + for (int i = 1; i < m_inputData.size(); i++) { + // y[i] := α * y[i-1] + α * (x[i] - x[i-1]) + outputData.append(m_highPassAlpha * outputData.at(i - 1) + m_highPassAlpha * (m_inputData.at(i) - m_inputData.at(i - 1))); + } + + m_outputData = outputData; + return m_outputData.last(); +} + +double SensorFilter::averageFilterValue(double value) +{ + if (m_inputData.isEmpty()) { + addInputValue(value); + m_averageSum = value; + return value; + } + + if (m_inputData.size() >= m_filterWindowSize) { + m_averageSum -= m_inputData.takeFirst(); + } + + addInputValue(value); + m_averageSum += value; + return m_averageSum / m_inputData.size(); +} diff --git a/multisensor/sensorfilter.h b/multisensor/sensorfilter.h new file mode 100644 index 00000000..d137ed13 --- /dev/null +++ b/multisensor/sensorfilter.h @@ -0,0 +1,60 @@ +#ifndef SENSORFILTER_H +#define SENSORFILTER_H + +#include +#include + +class SensorFilter : public QObject +{ + Q_OBJECT +public: + enum Type { + TypeLowPass, + TypeHighPass, + TypeAverage + }; + Q_ENUM(Type) + + explicit SensorFilter(Type filterType, QObject *parent = nullptr); + + double filterValue(double value); + + bool isReady() const; + void reset(); + + Type filterType() const; + + QVector inputData() const; + QVector outputData() const; + + // Filter configuration + uint windowSize() const; + void setFilterWindowSize(uint windowSize = 20); + + double lowPassAlpha() const; + void setLowPassAlpha(double alpha = 0.2); + + double highPassAlpha() const; + void setHighPassAlpha(double alpha = 0.2); + +private: + Type m_filterType = TypeLowPass; + int m_filterWindowSize = 20; + double m_lowPassAlpha = 0.2; + double m_highPassAlpha = 0.2; + + double m_averageSum = 0; + + QVector m_inputData; + QVector m_outputData; + + void addInputValue(double value); + + // Filter methods + double lowPassFilterValue(double value); + double highPassFilterValue(double value); + double averageFilterValue(double value); + +}; + +#endif // SENSORFILTER_H diff --git a/multisensor/sensortag.cpp b/multisensor/sensortag.cpp index 2f8223d6..2854d2c0 100644 --- a/multisensor/sensortag.cpp +++ b/multisensor/sensortag.cpp @@ -31,7 +31,8 @@ SensorTag::SensorTag(Device *device, BluetoothLowEnergyDevice *bluetoothDevice, QObject *parent) : QObject(parent), m_device(device), - m_bluetoothDevice(bluetoothDevice) + m_bluetoothDevice(bluetoothDevice), + m_dataProcessor(new SensorDataProcessor(m_device, this)) { connect(m_bluetoothDevice, &BluetoothLowEnergyDevice::connectedChanged, this, &SensorTag::onConnectedChanged); connect(m_bluetoothDevice, &BluetoothLowEnergyDevice::servicesDiscoveryFinished, this, &SensorTag::onServiceDiscoveryFinished); @@ -47,6 +48,50 @@ BluetoothLowEnergyDevice *SensorTag::bluetoothDevice() return m_bluetoothDevice; } +void SensorTag::setTemperatureSensorEnabled(bool enabled) +{ + qCDebug(dcMultiSensor()) << "Temperature sensor" << (enabled ? "enabled" : "disabled"); + + if (m_temperatureEnabled == enabled) + return; + + m_temperatureEnabled = enabled; + setTemperatureSensorPower(m_temperatureEnabled); +} + +void SensorTag::setHumiditySensorEnabled(bool enabled) +{ + qCDebug(dcMultiSensor()) << "Humidity sensor" << (enabled ? "enabled" : "disabled"); + + if (m_humidityEnabled == enabled) + return; + + m_humidityEnabled = enabled; + setHumiditySensorPower(m_humidityEnabled); +} + +void SensorTag::setPressureSensorEnabled(bool enabled) +{ + qCDebug(dcMultiSensor()) << "Pressure sensor" << (enabled ? "enabled" : "disabled"); + + if (m_pressureEnabled == enabled) + return; + + m_pressureEnabled = enabled; + setPressureSensorPower(m_pressureEnabled); +} + +void SensorTag::setOpticalSensorEnabled(bool enabled) +{ + qCDebug(dcMultiSensor()) << "Optical sensor" << (enabled ? "enabled" : "disabled"); + + if (m_opticalEnabled == enabled) + return; + + m_opticalEnabled = enabled; + setOpticalSensorPower(m_opticalEnabled); +} + void SensorTag::setAccelerometerEnabled(bool enabled) { qCDebug(dcMultiSensor()) << "Accelerometer" << (enabled ? "enabled" : "disabled"); @@ -275,203 +320,42 @@ void SensorTag::configureIo() m_ioService->writeCharacteristic(m_ioDataCharacteristic, payload); } -void SensorTag::processTemperatureData(const QByteArray &data) +void SensorTag::setTemperatureSensorPower(bool power) { - Q_ASSERT(data.count() == 4); - - quint16 rawObjectTemperature = 0; - quint16 rawAmbientTemperature = 0; - - QByteArray payload(data); - QDataStream stream(&payload, QIODevice::ReadOnly); - stream.setByteOrder(QDataStream::LittleEndian); - stream >> rawObjectTemperature >> rawAmbientTemperature ; - - float scaleFactor = 0.03125; - float objectTemperature = static_cast(rawObjectTemperature) / 4 * scaleFactor; - float ambientTemperature = static_cast(rawAmbientTemperature) / 4 * scaleFactor; - - //qCDebug(dcMultiSensor()) << "Temperature value" << data.toHex(); - //qCDebug(dcMultiSensor()) << "Object temperature" << roundValue(objectTemperature) << "°C"; - //qCDebug(dcMultiSensor()) << "Ambient temperature" << roundValue(ambientTemperature) << "°C"; - - m_device->setStateValue(sensortagObjectTemperatureStateTypeId, roundValue(objectTemperature)); - m_device->setStateValue(sensortagTemperatureStateTypeId, roundValue(ambientTemperature)); -} - -void SensorTag::processKeyData(const QByteArray &data) -{ - Q_ASSERT(data.count() == 1); - quint8 flags = static_cast(data.at(0)); - setLeftButtonPressed(testBitUint8(flags, 0)); - setRightButtonPressed(testBitUint8(flags, 1)); - setMagnetDetected(testBitUint8(flags, 2)); -} - -void SensorTag::processHumidityData(const QByteArray &data) -{ - Q_ASSERT(data.count() == 4); - quint16 rawHumidityTemperature = 0; - quint16 rawHumidity = 0; - - QByteArray payload(data); - QDataStream stream(&payload, QIODevice::ReadOnly); - 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; - - //qCDebug(dcMultiSensor()) << "Humidity" << humidity << "%" << humidityTemperature << "°C"; - m_device->setStateValue(sensortagHumidityStateTypeId, roundValue(humidity)); -} - -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; - rawPressure |= static_cast(pressureData.at(1)); - 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)); -} - -void SensorTag::processOpticalData(const QByteArray &data) -{ - Q_ASSERT(data.count() == 2); - - quint16 rawOptical = 0; - QByteArray payload(data); - QDataStream stream(&payload, QIODevice::ReadOnly); - stream.setByteOrder(QDataStream::LittleEndian); - stream >> rawOptical; - - quint16 lumm = rawOptical & 0x0FFF; - quint16 lume = (rawOptical & 0xF000) >> 12; - - double lux = lumm * (0.01 * pow(2,lume)); - - //qCDebug(dcMultiSensor()) << "Lux:" << lux; - device()->setStateValue(sensortagLightIntensityStateTypeId, roundValue(lux)); -} - -void SensorTag::processMovementData(const QByteArray &data) -{ - //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; - - qint16 accXRaw = 0; qint16 accYRaw = 0; qint16 accZRaw = 0; - stream >> accXRaw >> accYRaw >> accZRaw; - - qint16 magXRaw = 0; qint16 magYRaw = 0; qint16 magZRaw = 0; - stream >> magXRaw >> magYRaw >> magZRaw; - - - // 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); - - // Calculate acceleration [G], Range +- m_accelerometerRange - double accX = static_cast(accXRaw) / (32768 / static_cast(m_accelerometerRange)); - double accY = static_cast(accYRaw) / (32768 / static_cast(m_accelerometerRange)); - double accZ = static_cast(accZRaw) / (32768 / static_cast(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) - - // Initialize the accelerometer value if no data known yet - if (m_lastAccelerometerVectorLenght == -99999) { - m_lastAccelerometerVectorLenght = accelerometerVector.length(); - return; - } - - - double delta = qAbs(qAbs(m_lastAccelerometerVectorLenght) - qAbs(accelerometerVector.length())); - bool motionDetected = (delta >= m_movementSensitivity); - //qCDebug(dcMultiSensor()) << accelerometerVector.length() << " | " << delta << m_movementSensitivity << (motionDetected ? "motion" : "-"); - m_device->setStateValue(sensortagMovingStateTypeId, motionDetected); - m_lastAccelerometerVectorLenght = accelerometerVector.length(); -} - -void SensorTag::setLeftButtonPressed(bool pressed) -{ - if (m_leftButtonPressed == pressed) + if (!m_temperatureService || !m_temperatureConfigurationCharacteristic.isValid()) return; - qCDebug(dcMultiSensor()) << "Left button" << (pressed ? "pressed" : "released"); - m_leftButtonPressed = pressed; - emit leftButtonPressedChainged(m_leftButtonPressed); - m_device->setStateValue(sensortagLeftButtonPressedStateTypeId, m_leftButtonPressed); + QByteArray payload = (power ? QByteArray::fromHex("01") : QByteArray::fromHex("00")); + m_temperatureService->writeCharacteristic(m_temperatureConfigurationCharacteristic, payload); } -void SensorTag::setRightButtonPressed(bool pressed) +void SensorTag::setHumiditySensorPower(bool power) { - if (m_rightButtonPressed == pressed) + if (!m_humidityService || !m_humidityConfigurationCharacteristic.isValid()) return; - qCDebug(dcMultiSensor()) << "Right button" << (pressed ? "pressed" : "released"); - m_rightButtonPressed = pressed; - emit rightButtonPressedChainged(m_rightButtonPressed); - m_device->setStateValue(sensortagRightButtonPressedStateTypeId, m_rightButtonPressed); + QByteArray payload = (power ? QByteArray::fromHex("01") : QByteArray::fromHex("00")); + m_humidityService->writeCharacteristic(m_humidityConfigurationCharacteristic, payload); } -void SensorTag::setMagnetDetected(bool detected) +void SensorTag::setPressureSensorPower(bool power) { - if (m_magnetDetected == detected) + if (!m_pressureService || !m_pressureConfigurationCharacteristic.isValid()) return; - qCDebug(dcMultiSensor()) << "Magnet detector" << (detected ? "active" : "inactive"); - m_magnetDetected = detected; - emit magnetDetectedChainged(m_magnetDetected); - m_device->setStateValue(sensortagMagnetDetectedStateTypeId, m_magnetDetected); + QByteArray payload = (power ? QByteArray::fromHex("01") : QByteArray::fromHex("00")); + m_pressureService->writeCharacteristic(m_pressureConfigurationCharacteristic, payload); } -bool SensorTag::testBitUint8(quint8 value, int bitPosition) +void SensorTag::setOpticalSensorPower(bool power) { - return (((value)>> (bitPosition)) & 1); + if (!m_opticalService || !m_opticalConfigurationCharacteristic.isValid()) + return; + + QByteArray payload = (power ? QByteArray::fromHex("01") : QByteArray::fromHex("00")); + m_opticalService->writeCharacteristic(m_opticalConfigurationCharacteristic, payload); } -double SensorTag::roundValue(float value) -{ - int tmpValue = static_cast(value * 10); - return static_cast(tmpValue) / 10.0; -} void SensorTag::onConnectedChanged(const bool &connected) { @@ -496,7 +380,7 @@ void SensorTag::onConnectedChanged(const bool &connected) m_movementService = nullptr; m_ioService = nullptr; - m_lastAccelerometerVectorLenght = -99999; + m_dataProcessor->reset(); } } @@ -667,6 +551,11 @@ void SensorTag::onServiceDiscoveryFinished() } } +void SensorTag::onBuzzerImpulseTimeout() +{ + setBuzzerPower(false); +} + void SensorTag::onTemperatureServiceStateChanged(const QLowEnergyService::ServiceState &state) { // Only continue if discovered @@ -713,17 +602,14 @@ void SensorTag::onTemperatureServiceStateChanged(const QLowEnergyService::Servic configurePeriod(m_temperatureService, m_temperaturePeriodCharacteristic, m_temperaturePeriod); - // Enable measuring - m_temperatureService->writeCharacteristic(m_temperatureConfigurationCharacteristic, QByteArray::fromHex("01")); + // Enable/disable measuring + setTemperatureSensorPower(m_temperatureEnabled); } void SensorTag::onTemperatureServiceCharacteristicChanged(const QLowEnergyCharacteristic &characteristic, const QByteArray &value) { if (characteristic == m_temperatureDataCharacteristic) { - processTemperatureData(value); - - // FIXME: Disable measuring - // m_temperatureService->writeCharacteristic(m_temperatureConfigCharacteristic, QByteArray::fromHex("00")); + m_dataProcessor->processTemperatureData(value); } } @@ -779,9 +665,7 @@ void SensorTag::onHumidityServiceStateChanged(const QLowEnergyService::ServiceSt void SensorTag::onHumidityServiceCharacteristicChanged(const QLowEnergyCharacteristic &characteristic, const QByteArray &value) { if (characteristic == m_humidityDataCharacteristic) { - processHumidityData(value); - // FIXME: Disable measuring - // m_humidityService->writeCharacteristic(m_humidityConfigCharacteristic, QByteArray::fromHex("00")); + m_dataProcessor->processHumidityData(value); } } @@ -837,9 +721,7 @@ void SensorTag::onPressureServiceStateChanged(const QLowEnergyService::ServiceSt void SensorTag::onPressureServiceCharacteristicChanged(const QLowEnergyCharacteristic &characteristic, const QByteArray &value) { if (characteristic == m_pressureDataCharacteristic) { - processPressureData(value); - // FIXME: Disable measuring - // m_pressureService->writeCharacteristic(m_pressureConfigCharacteristic, QByteArray::fromHex("00")); + m_dataProcessor->processPressureData(value); } } @@ -894,11 +776,8 @@ void SensorTag::onOpticalServiceStateChanged(const QLowEnergyService::ServiceSta void SensorTag::onOpticalServiceCharacteristicChanged(const QLowEnergyCharacteristic &characteristic, const QByteArray &value) { if (characteristic == m_opticalDataCharacteristic) { - processOpticalData(value); - // FIXME: Disable measuring - // m_opticalService->writeCharacteristic(m_pressureopticalCharacteristic, QByteArray::fromHex("00")); + m_dataProcessor->processOpticalData(value); } - } void SensorTag::onKeyServiceStateChanged(const QLowEnergyService::ServiceState &state) @@ -930,7 +809,7 @@ void SensorTag::onKeyServiceStateChanged(const QLowEnergyService::ServiceState & void SensorTag::onKeyServiceCharacteristicChanged(const QLowEnergyCharacteristic &characteristic, const QByteArray &value) { if (characteristic == m_keyDataCharacteristic) { - processKeyData(value); + m_dataProcessor->processKeyData(value); } } @@ -985,7 +864,7 @@ void SensorTag::onMovementServiceStateChanged(const QLowEnergyService::ServiceSt void SensorTag::onMovementServiceCharacteristicChanged(const QLowEnergyCharacteristic &characteristic, const QByteArray &value) { if (characteristic == m_movementDataCharacteristic) { - processMovementData(value); + m_dataProcessor->processMovementData(value); } } @@ -1031,8 +910,3 @@ void SensorTag::onIoServiceCharacteristicChanged(const QLowEnergyCharacteristic { qCDebug(dcMultiSensor()) << characteristic.uuid().toString() << value.toHex(); } - -void SensorTag::onBuzzerImpulseTimeout() -{ - setBuzzerPower(false); -} diff --git a/multisensor/sensortag.h b/multisensor/sensortag.h index eb233420..57037337 100644 --- a/multisensor/sensortag.h +++ b/multisensor/sensortag.h @@ -27,6 +27,8 @@ #include "plugin/device.h" #include "extern-plugininfo.h" +#include "sensordataprocessor.h" + #include "hardware/bluetoothlowenergy/bluetoothlowenergydevice.h" // http://processors.wiki.ti.com/index.php/CC2650_SensorTag_User's_Guide @@ -89,10 +91,15 @@ public: BluetoothLowEnergyDevice *bluetoothDevice(); // Configurations + void setTemperatureSensorEnabled(bool enabled); + void setHumiditySensorEnabled(bool enabled); + void setPressureSensorEnabled(bool enabled); + void setOpticalSensorEnabled(bool enabled); void setAccelerometerEnabled(bool enabled); - void setAccelerometerRange(const SensorAccelerometerRange &range); void setGyroscopeEnabled(bool enabled); void setMagnetometerEnabled(bool enabled); + + void setAccelerometerRange(const SensorAccelerometerRange &range); void setMeasurementPeriod(int period); void setMeasurementPeriodMovement(int period); void setMovementSensitivity(int percentage); @@ -152,49 +159,43 @@ private: SensorAccelerometerRange m_accelerometerRange = SensorAccelerometerRange16G; // States - bool m_leftButtonPressed = false; - bool m_rightButtonPressed = false; - bool m_magnetDetected = false; bool m_greenLedEnabled = false; bool m_redLedEnabled = false; bool m_buzzerEnabled = false; - double m_lastAccelerometerVectorLenght = -99999; // Plugin configs + bool m_temperatureEnabled = true; + bool m_humidityEnabled = true; + bool m_pressureEnabled = true; + bool m_opticalEnabled = true; bool m_accelerometerEnabled = true; bool m_gyroscopeEnabled = false; bool m_magnetometerEnabled = false; + SensorDataProcessor *m_dataProcessor = nullptr; + + // Configuration methods void configurePeriod(QLowEnergyService *serice, const QLowEnergyCharacteristic &characteristic, int measurementPeriod); void configureMovement(); void configureSensorMode(const SensorMode &mode); void configureIo(); - void processTemperatureData(const QByteArray &data); - void processKeyData(const QByteArray &data); - void processHumidityData(const QByteArray &data); - void processPressureData(const QByteArray &data); - void processOpticalData(const QByteArray &data); - void processMovementData(const QByteArray &data); - - // Set methods - void setLeftButtonPressed(bool pressed); - void setRightButtonPressed(bool pressed); - void setMagnetDetected(bool detected); - - // Helper - bool testBitUint8(quint8 value, int bitPosition); - double roundValue(float value); + void setTemperatureSensorPower(bool power); + void setHumiditySensorPower(bool power); + void setPressureSensorPower(bool power); + void setOpticalSensorPower(bool power); signals: - void leftButtonPressedChainged(bool pressed); - void rightButtonPressedChainged(bool pressed); - void magnetDetectedChainged(bool detected); + void leftButtonPressedChanged(bool pressed); + void rightButtonPressedChanged(bool pressed); + void magnetDetectedChanged(bool detected); private slots: void onConnectedChanged(const bool &connected); void onServiceDiscoveryFinished(); + void onBuzzerImpulseTimeout(); + // Temperature sensor service void onTemperatureServiceStateChanged(const QLowEnergyService::ServiceState &state); void onTemperatureServiceCharacteristicChanged(const QLowEnergyCharacteristic &characteristic, const QByteArray &value); @@ -222,8 +223,6 @@ private slots: // IO service void onIoServiceStateChanged(const QLowEnergyService::ServiceState &state); void onIoServiceCharacteristicChanged(const QLowEnergyCharacteristic &characteristic, const QByteArray &value); - - void onBuzzerImpulseTimeout(); }; #endif // SENSORTAG_H