diff --git a/include/BLE_Common.h b/include/BLE_Common.h index 5a7c5c45..47595632 100644 --- a/include/BLE_Common.h +++ b/include/BLE_Common.h @@ -69,7 +69,7 @@ void updateHeartRateMeasurementChar(); class MyServerCallbacks : public BLEServerCallbacks { - void onConnect(BLEServer *, ble_gap_conn_desc* desc); + void onConnect(BLEServer *, ble_gap_conn_desc *desc); void onDisconnect(BLEServer *); }; @@ -80,244 +80,124 @@ class MyCallbacks : public BLECharacteristicCallbacks //*****************************Client***************************** -//Keeping the task outside the class so we don't need a mask. +//Keeping the task outside the class so we don't need a mask. //We're only going to run one anyway. -void bleClientTask(void *pvParameters); +void bleClientTask(void *pvParameters); //UUID's the client has methods for //BLEUUID serviceUUIDs[4] = {FITNESSMACHINESERVICE_UUID, CYCLINGPOWERSERVICE_UUID, HEARTSERVICE_UUID, FLYWHEEL_UART_SERVICE_UUID}; //BLEUUID charUUIDs[4] = {FITNESSMACHINEINDOORBIKEDATA_UUID, CYCLINGPOWERMEASUREMENT_UUID, HEARTCHARACTERISTIC_UUID, FLYWHEEL_UART_TX_UUID}; -enum userSelect : uint8_t { - HR = 1, - PM = 2, - CSC = 3, - CT = 4 - }; -class myAdvertisedBLEDevice{ - public: //eventually these shoul be made private - BLEAdvertisedDevice *advertisedDevice = nullptr; - NimBLEAddress peerAddress; - int connectedClientID = BLE_HS_CONN_HANDLE_NONE; - BLEUUID serviceUUID = (uint16_t)0x0000; - BLEUUID charUUID = (uint16_t)0x0000; - bool userSelectedHR = false; - bool userSelectedPM = false; - bool userSelectedCSC = false; - bool userSelectedCT = false; - bool doConnect = false; +enum userSelect : uint8_t +{ + HR = 1, + PM = 2, + CSC = 3, + CT = 4 +}; - void set(BLEAdvertisedDevice *device, int id = BLE_HS_CONN_HANDLE_NONE, BLEUUID inserviceUUID = (uint16_t)0x0000, BLEUUID incharUUID = (uint16_t)0x0000){ - advertisedDevice = device; +class myAdvertisedBLEDevice +{ +public: //eventually these shoul be made private + BLEAdvertisedDevice *advertisedDevice = nullptr; + NimBLEAddress peerAddress; + int connectedClientID = BLE_HS_CONN_HANDLE_NONE; + BLEUUID serviceUUID = (uint16_t)0x0000; + BLEUUID charUUID = (uint16_t)0x0000; + bool userSelectedHR = false; + bool userSelectedPM = false; + bool userSelectedCSC = false; + bool userSelectedCT = false; + bool doConnect = false; + + void set(BLEAdvertisedDevice *device, int id = BLE_HS_CONN_HANDLE_NONE, BLEUUID inserviceUUID = (uint16_t)0x0000, BLEUUID incharUUID = (uint16_t)0x0000) + { + advertisedDevice = device; peerAddress = device->getAddress(); - connectedClientID = id; + connectedClientID = id; serviceUUID = BLEUUID(inserviceUUID); - charUUID = BLEUUID(incharUUID); + charUUID = BLEUUID(incharUUID); } - void reset() { - advertisedDevice = nullptr; - //NimBLEAddress peerAddress; - connectedClientID = BLE_HS_CONN_HANDLE_NONE; - serviceUUID = (uint16_t)0x0000; - charUUID = (uint16_t)0x0000; - userSelectedHR = false; - userSelectedPM = false; - userSelectedCSC = false; - userSelectedCT = false; - doConnect = false; + void reset() + { + advertisedDevice = nullptr; + //NimBLEAddress peerAddress; + connectedClientID = BLE_HS_CONN_HANDLE_NONE; + serviceUUID = (uint16_t)0x0000; + charUUID = (uint16_t)0x0000; + userSelectedHR = false; + userSelectedPM = false; + userSelectedCSC = false; + userSelectedCT = false; + doConnect = false; } - //userSelectHR = 1,userSelectPM = 2,userSelectCSC = 3,userSelectCT = 4 - void setSelected (userSelect flags) - { - switch (flags) + //userSelectHR = 1,userSelectPM = 2,userSelectCSC = 3,userSelectCT = 4 + void setSelected(userSelect flags) { - case 1: - userSelectedHR = true; - break; - case 2: - userSelectedPM = true; - break; - case 3: - userSelectedCSC = true; - break; - case 4: - userSelectedCT = true; - break; + switch (flags) + { + case 1: + userSelectedHR = true; + break; + case 2: + userSelectedPM = true; + break; + case 3: + userSelectedCSC = true; + break; + case 4: + userSelectedCT = true; + break; + } } - -} - - - }; +class SpinBLEClient +{ +public: //Not all of these need to be public. This should be cleaned up later. + boolean connectedPM = false; + boolean connectedHR = false; + boolean connectedCD = false; + boolean doScan = false; + bool intentionalDisconnect = false; + int noReadingIn = 0; + int cscCumulativeCrankRev = 0; + int cscLastCrankEvtTime = 0; -class SpinBLEClient{ - - public: //Not all of these need to be public. This should be cleaned up later. - boolean doConnectPM = false; - boolean doConnectHR = false; - boolean connectedPM = false; - boolean connectedHR = false; - boolean doScan = false; - bool intentionalDisconnect = false; - float crankRev[2] = {0, 0}; - float crankEventTime[2] = {0, 0}; - int noReadingIn = 0; - int cscCumulativeCrankRev = 0; - int cscLastCrankEvtTime = 0; - - BLERemoteCharacteristic *pRemoteCharacteristic = nullptr; + BLERemoteCharacteristic *pRemoteCharacteristic = nullptr; //BLEDevices myBLEDevices; myAdvertisedBLEDevice myBLEDevices[NUM_BLE_DEVICES]; void start(); - void serverScan(bool connectRequest); + void serverScan(bool connectRequest); bool connectToServer(); void scanProcess(); void disconnect(); - //Check for duplicate services of BLEClient and remove the previoulsy connected one. + //Check for duplicate services of BLEClient and remove the previoulsy connected one. void removeDuplicates(BLEClient *pClient); private: - class MyAdvertisedDeviceCallback : public NimBLEAdvertisedDeviceCallbacks { - public: - void onResult(NimBLEAdvertisedDevice *); + public: + void onResult(NimBLEAdvertisedDevice *); }; class MyClientCallback : public NimBLEClientCallbacks { - public: - void onConnect(BLEClient *); - void onDisconnect(BLEClient *); - uint32_t onPassKeyRequest(); - bool onConfirmPIN(uint32_t); - void onAuthenticationComplete(ble_gap_conn_desc); - }; -}; - -extern SpinBLEClient spinBLEClient; - -class SensorData { -public: - SensorData(String id, uint8_t *data, size_t length) : id(id), data(data), length(length) {}; - - String getId(); - virtual bool hasHeartRate() = 0; - virtual bool hasCadence() = 0; - virtual bool hasPower() = 0; - virtual int getHeartRate() = 0; - virtual float getCadence() = 0; - virtual int getPower() = 0; - -protected: - String id; - uint8_t *data; - size_t length; -}; - -class SensorDataFactory { -public: - static std::unique_ptr getSensorData(BLERemoteCharacteristic *characteristic, uint8_t *data, size_t length); - -private: - SensorDataFactory() {}; -}; - -class NullData : public SensorData { -public: - NullData(uint8_t *data, size_t length) : SensorData("Null", data, length) {}; - - - virtual bool hasHeartRate(); - virtual bool hasCadence(); - virtual bool hasPower(); - virtual int getHeartRate(); - virtual float getCadence(); - virtual int getPower(); -}; - -class HeartRateData : public SensorData { -public: - HeartRateData(uint8_t *data, size_t length) : SensorData("HRM", data, length) {}; - - - virtual bool hasHeartRate(); - virtual bool hasCadence(); - virtual bool hasPower(); - virtual int getHeartRate(); - virtual float getCadence(); - virtual int getPower(); -}; - -class FlywheelData : public SensorData { -public: - FlywheelData(uint8_t *data, size_t length) : SensorData("FLYW", data, length) {}; - - - virtual bool hasHeartRate(); - virtual bool hasCadence(); - virtual bool hasPower(); - virtual int getHeartRate(); - virtual float getCadence(); - virtual int getPower(); -}; - -class FitnessMachineIndoorBikeData : public SensorData { -public: - FitnessMachineIndoorBikeData(uint8_t *data, size_t length); - - ~FitnessMachineIndoorBikeData(); - - enum Types : uint8_t { - InstantaneousSpeed = 0, - AverageSpeed = 1, - InstantaneousCadence = 2, - AverageCadence = 3, - TotalDistance = 4, - ResistanceLevel = 5, - InstantaneousPower = 6, - AveragePower = 7, - TotalEnergy = 8, - EnergyPerHour = 9, - EnergyPerMinute = 10, - HeartRate = 11, - MetabolicEquivalent = 12, - ElapsedTime = 13, - RemainingTime = 14 + public: + void onConnect(BLEClient *); + void onDisconnect(BLEClient *); + uint32_t onPassKeyRequest(); + bool onConfirmPIN(uint32_t); + void onAuthenticationComplete(ble_gap_conn_desc); }; - - static constexpr uint8_t FieldCount = Types::RemainingTime + 1; - - virtual bool hasHeartRate(); - virtual bool hasCadence(); - virtual bool hasPower(); - virtual int getHeartRate(); - virtual float getCadence(); - virtual int getPower(); - -private: - int flags; - double_t *values; - - static uint8_t const flagBitIndices[]; - static uint8_t const flagEnabledValues[]; - static size_t const byteSizes[]; - static uint8_t const signedFlags[]; - static double_t const resolutions[]; - static int convert(int value, size_t length, uint8_t isSigned); }; -/******************CPS***************************/ -void BLE_CPSDecode(BLERemoteCharacteristic *pBLERemoteCharacteristic); - -/******************FTMS**************************/ -void BLE_FTMSDecode(BLERemoteCharacteristic *pBLERemoteCharacteristic); \ No newline at end of file +extern SpinBLEClient spinBLEClient; \ No newline at end of file diff --git a/include/SmartSpin_parameters.h b/include/SmartSpin_parameters.h index 7439741e..5e0cadac 100644 --- a/include/SmartSpin_parameters.h +++ b/include/SmartSpin_parameters.h @@ -17,6 +17,7 @@ class userParameters int simulatedWatts; int simulatedHr; int simulatedCad; + float simulatedSpeed; String deviceName; int shiftStep; int stepperPower; @@ -38,6 +39,7 @@ class userParameters int getSimulatedWatts() {return simulatedWatts;} int getSimulatedHr() {return simulatedHr;} float getSimulatedCad() {return simulatedCad;} + float getSimulatedSpeed() {return simulatedSpeed;} const char* getDeviceName() {return deviceName.c_str();} int getShiftStep() {return shiftStep;} int getStepperPower() {return stepperPower;} @@ -59,6 +61,7 @@ class userParameters void setSimulatedWatts(int w) {simulatedWatts = w;} void setSimulatedHr(int hr) {simulatedHr = hr;} void setSimulatedCad(float cad) {simulatedCad = cad;} + void setSimulatedSpeed(float spd) {simulatedSpeed = spd;} void setDeviceName(String dvcn) {deviceName = dvcn;} void setShiftStep(int ss) {shiftStep = ss;} void setStepperPower(int sp) {stepperPower = sp;} diff --git a/include/sensors/CyclePowerData.h b/include/sensors/CyclePowerData.h new file mode 100644 index 00000000..60734906 --- /dev/null +++ b/include/sensors/CyclePowerData.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include "SensorData.h" + +class CyclePowerData : public SensorData +{ +public: + CyclePowerData() : SensorData("CPS"){}; + + bool hasHeartRate(); + bool hasCadence(); + bool hasPower(); + bool hasSpeed(); + int getHeartRate(); + float getCadence(); + int getPower(); + float getSpeed(); + void decode(uint8_t *data, size_t length); + +private: + int power; + float cadence; + float crankRev = 0; + float lastCrankRev = 0; + float lastCrankEventTime = 0; + float crankEventTime = 0; + uint8_t missedReadingCount = 0; +}; \ No newline at end of file diff --git a/include/sensors/FitnessMachineIndoorBikeData.h b/include/sensors/FitnessMachineIndoorBikeData.h new file mode 100644 index 00000000..97415679 --- /dev/null +++ b/include/sensors/FitnessMachineIndoorBikeData.h @@ -0,0 +1,53 @@ +#pragma once + +#include +#include "SensorData.h" + +class FitnessMachineIndoorBikeData : public SensorData +{ +public: + FitnessMachineIndoorBikeData() : SensorData("FTMS"){}; + + bool hasHeartRate(); + bool hasCadence(); + bool hasPower(); + bool hasSpeed(); + int getHeartRate(); + float getCadence(); + int getPower(); + float getSpeed(); + void decode(uint8_t *data, size_t length); + + enum Types : uint8_t + { + InstantaneousSpeed = 0, + AverageSpeed = 1, + InstantaneousCadence = 2, + AverageCadence = 3, + TotalDistance = 4, + ResistanceLevel = 5, + InstantaneousPower = 6, + AveragePower = 7, + TotalEnergy = 8, + EnergyPerHour = 9, + EnergyPerMinute = 10, + HeartRate = 11, + MetabolicEquivalent = 12, + ElapsedTime = 13, + RemainingTime = 14 + }; + + static constexpr uint8_t FieldCount = Types::RemainingTime + 1; + +private: + double_t values[FieldCount]; + + // See: https://github.com/oesmith/gatt-xml/blob/master/org.bluetooth.characteristic.indoor_bike_data.xml + static uint8_t const flagBitIndices[]; + static uint8_t const flagEnabledValues[]; + static size_t const byteSizes[]; + static uint8_t const signedFlags[]; + static double_t const resolutions[]; + + static int convert(int value, size_t length, uint8_t isSigned); +}; diff --git a/include/sensors/FlywheelData.h b/include/sensors/FlywheelData.h new file mode 100644 index 00000000..3289563b --- /dev/null +++ b/include/sensors/FlywheelData.h @@ -0,0 +1,24 @@ +#pragma once + +#include "SensorData.h" + +class FlywheelData : public SensorData +{ +public: + FlywheelData() : SensorData("FLYW"){}; + + bool hasHeartRate(); + bool hasCadence(); + bool hasPower(); + bool hasSpeed(); + int getHeartRate(); + float getCadence(); + int getPower(); + float getSpeed(); + void decode(uint8_t *data, size_t length); + +private: + bool hasData = false; + float cadence; + int power; +}; \ No newline at end of file diff --git a/include/sensors/HeartRateData.h b/include/sensors/HeartRateData.h new file mode 100644 index 00000000..ec7c4c91 --- /dev/null +++ b/include/sensors/HeartRateData.h @@ -0,0 +1,22 @@ +#pragma once + +#include "SensorData.h" + +class HeartRateData : public SensorData +{ +public: + HeartRateData() : SensorData("HRM"){}; + + bool hasHeartRate(); + bool hasCadence(); + bool hasPower(); + bool hasSpeed(); + int getHeartRate(); + float getCadence(); + int getPower(); + float getSpeed(); + void decode(uint8_t *data, size_t length); + +private: + int heartrate; +}; \ No newline at end of file diff --git a/include/sensors/SensorData.h b/include/sensors/SensorData.h new file mode 100644 index 00000000..87492646 --- /dev/null +++ b/include/sensors/SensorData.h @@ -0,0 +1,23 @@ +#pragma once + +#include + +class SensorData +{ +public: + SensorData(String id) : id(id){}; + + String getId(); + virtual bool hasHeartRate() = 0; + virtual bool hasCadence() = 0; + virtual bool hasPower() = 0; + virtual bool hasSpeed() = 0; + virtual int getHeartRate() = 0; + virtual float getCadence() = 0; + virtual int getPower() = 0; + virtual float getSpeed() = 0; + virtual void decode(uint8_t *data, size_t length) = 0; + +private: + String id; +}; \ No newline at end of file diff --git a/include/sensors/SensorDataFactory.h b/include/sensors/SensorDataFactory.h new file mode 100644 index 00000000..8ae14912 --- /dev/null +++ b/include/sensors/SensorDataFactory.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include "SensorData.h" +#include "CyclePowerData.h" +#include "FlywheelData.h" +#include "FitnessMachineIndoorBikeData.h" +#include "HeartRateData.h" + +class SensorDataFactory +{ +public: + SensorDataFactory(){}; + + std::shared_ptr getSensorData(BLERemoteCharacteristic *characteristic, uint8_t *data, size_t length); + +private: + class KnownDevice + { + public: + KnownDevice(NimBLEUUID uuid, std::shared_ptr sensorData) : uuid(uuid), sensorData(sensorData){}; + NimBLEUUID getUUID(); + std::shared_ptr decode(uint8_t *data, size_t length); + + private: + NimBLEUUID uuid; + std::shared_ptr sensorData; + }; + + class NullData : public SensorData + { + public: + NullData() : SensorData("Null"){}; + + virtual bool hasHeartRate(); + virtual bool hasCadence(); + virtual bool hasPower(); + virtual bool hasSpeed(); + virtual int getHeartRate(); + virtual float getCadence(); + virtual int getPower(); + virtual float getSpeed(); + virtual void decode(uint8_t *data, size_t length); + }; + + std::vector knownDevices; + static std::shared_ptr NULL_SENSOR_DATA; +}; \ No newline at end of file diff --git a/src/BLE_CPS.cpp b/src/BLE_CPS.cpp deleted file mode 100644 index ef34b731..00000000 --- a/src/BLE_CPS.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// SmartSpin2K code -// This software registers an ESP32 as a BLE FTMS device which then uses a stepper motor to turn the resistance knob on a regular spin bike. -// BLE code based on examples from https://github.com/nkolban -// Copyright 2020 Anthony Doud -// This work is licensed under the GNU General Public License v2 -// Prototype hardware build from plans in the SmartSpin2k repository are licensed under Cern Open Hardware Licence version 2 Permissive - -#include "Main.h" -#include "BLE_Common.h" -#include - -//https://github.com/oesmith/gatt-xml/blob/master/org.bluetooth.characteristic.cycling_power_measurement.xml -// -// IPWR, PPB, PPR, AT, ATS, WRD, CRD, EFM, ETM, EA, TDS, BDS, AE, OCI -//int8_t const CyclingPowerMeasurement::flagBitIndices[FieldCount] = { -1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; -//uint8_t const CyclingPowerMeasurement::flagEnabledValues[FieldCount] = { 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; -//size_t const CyclingPowerMeasurement::byteSizes[FieldCount] = { 2, 1, 0, 2, 0, 6, 5, 4, 4, 6, 2, 2, 2, 0}; -//uint8_t const CyclingPowerMeasurement::signedFlags[FieldCount] = { 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0}; -//double_t const CyclingPowerMeasurement::resolutions[FieldCount] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; -//double_t const CyclingPowerMeasurement::resolutions[FieldCount] = { 1, 1.0, .5, 1/32, 1.0, 1/2048, 1/1024, 1.0, 1/32, 1.0, 1.0, 1.0, 1.0, 1.0}; - -/*Calculate Cadence and power from Cycling Power Measurement*/ -//Takes a NimBLERemoteCharacteristic with CYCLINGPOWERMEASUREMENT_UUID and extracts the data. -void BLE_CPSDecode(BLERemoteCharacteristic *pBLERemoteCharacteristic) -{ - if (pBLERemoteCharacteristic->getUUID() == CYCLINGPOWERMEASUREMENT_UUID) //double checking this was - { - std::string pData = pBLERemoteCharacteristic->getValue(); - - byte flags = pData[0]; - int cPos = 2; //lowest position cadence could ever be - //Instanious power is always present. Do that first. - //first calculate which fields are present. Power is always 2 & 3, cadence can move depending on the flags. - userConfig.setSimulatedWatts(bytes_to_u16(pData[cPos + 1], pData[cPos])); - cPos += 2; - if (userConfig.getDoublePower()) - { - userConfig.setSimulatedWatts(userConfig.getSimulatedWatts() * 2); - } - - if (bitRead(flags, 0)) - { - //pedal balance field present - cPos++; - } - if (bitRead(flags, 1)) - { - //pedal power balance reference - //no field associated with this. - } - if (bitRead(flags, 2)) - { - //accumulated torque field present - cPos += 2; - } - if (bitRead(flags, 3)) - { - //accumulated torque field source - //no field associated with this. - } - if (bitRead(flags, 4)) - { - //Wheel Revolution field PAIR Data present. 32-bits for wheel revs, 16 bits for wheel event time. - //Why is that so hard to find in the specs? - cPos += 6; - } - if (bitRead(flags, 5)) - { - //Crank Revolution data present, lets process it. - spinBLEClient.crankRev[1] = spinBLEClient.crankRev[0]; - spinBLEClient.crankRev[0] = bytes_to_int(pData[cPos + 1], pData[cPos]); - spinBLEClient.crankEventTime[1] = spinBLEClient.crankEventTime[0]; - spinBLEClient.crankEventTime[0] = bytes_to_int(pData[cPos + 3], pData[cPos + 2]); - if ((spinBLEClient.crankRev[0] > spinBLEClient.crankRev[1]) && (spinBLEClient.crankEventTime[0] - spinBLEClient.crankEventTime[1] != 0)) - { - int tCAD = (((abs(spinBLEClient.crankRev[0] - spinBLEClient.crankRev[1]) * 1024) / abs(spinBLEClient.crankEventTime[0] - spinBLEClient.crankEventTime[1])) * 60); - if (tCAD > 1) - { - if (tCAD > 200) //Cadence Error - { - tCAD = 0; - } - userConfig.setSimulatedCad(tCAD); - spinBLEClient.noReadingIn = 0; - } - else - { - spinBLEClient.noReadingIn++; - } - } - else //the crank rev probably didn't update - { - if (spinBLEClient.noReadingIn > 2) //Require three consecutive readings before setting 0 cadence - { - userConfig.setSimulatedCad(0); - } - spinBLEClient.noReadingIn++; - } - - debugDirector(" CAD: " + String(userConfig.getSimulatedCad()) + " PWR: " + String(userConfig.getSimulatedWatts()), false); - debugDirector(""); - } - } -} \ No newline at end of file diff --git a/src/BLE_Client.cpp b/src/BLE_Client.cpp index 62ca7556..306b7167 100644 --- a/src/BLE_Client.cpp +++ b/src/BLE_Client.cpp @@ -313,6 +313,7 @@ bool SpinBLEClient::connectToServer() void SpinBLEClient::MyClientCallback::onConnect(BLEClient *pclient) { + debugDirector("Connect Called"); auto addr = pclient->getPeerAddress(); for (size_t i = 0; i < NUM_BLE_DEVICES; i++) { @@ -338,7 +339,8 @@ void SpinBLEClient::MyClientCallback::onConnect(BLEClient *pclient) void SpinBLEClient::MyClientCallback::onDisconnect(BLEClient *pclient) { - + + debugDirector("Disconnect Called"); if (spinBLEClient.intentionalDisconnect) { debugDirector("Intentional Disconnect"); @@ -360,11 +362,14 @@ void SpinBLEClient::MyClientCallback::onDisconnect(BLEClient *pclient) spinBLEClient.myBLEDevices[i].doConnect = true; if ((spinBLEClient.myBLEDevices[i].charUUID == CYCLINGPOWERMEASUREMENT_UUID) || (spinBLEClient.myBLEDevices[i].charUUID == FITNESSMACHINEINDOORBIKEDATA_UUID) || (spinBLEClient.myBLEDevices[i].charUUID == FLYWHEEL_UART_RX_UUID)) { + + debugDirector("Deregistered PM on Disconnect"); spinBLEClient.connectedPM = false; break; } if ((spinBLEClient.myBLEDevices[i].charUUID == HEARTCHARACTERISTIC_UUID)) { + debugDirector("Deregistered HR on Disconnect"); spinBLEClient.connectedHR = false; break; } diff --git a/src/BLE_Common.cpp b/src/BLE_Common.cpp index 1001138e..7e69adc2 100644 --- a/src/BLE_Common.cpp +++ b/src/BLE_Common.cpp @@ -8,12 +8,15 @@ #include "Main.h" #include #include "BLE_Common.h" +#include "sensors/SensorData.h" +#include "sensors/SensorDataFactory.h" //#include int bleConnDesc = 1; bool _BLEClientConnected = false; bool updateConnParametersFlag = false; TaskHandle_t BLECommunicationTask; +SensorDataFactory sensorDataFactory; void BLECommunications(void *pvParameters) { @@ -33,32 +36,55 @@ void BLECommunications(void *pvParameters) if ((myAdvertisedDevice.serviceUUID != BLEUUID((uint16_t)0x0000)) && (pClient->isConnected())) //Client connected with a valid UUID registered { //Write the recieved data to the Debug Director - BLERemoteCharacteristic *pRemoteBLECharacteristic = pClient->getService(myAdvertisedDevice.serviceUUID)->getCharacteristic(myAdvertisedDevice.charUUID); //get the registered services - std::string pData = pRemoteBLECharacteristic->getValue(); //read the data - int length = pData.length(); - String debugOutput = ""; - for (int i = 0; i < length; i++) //loop and print data + + BLERemoteCharacteristic *pRemoteBLECharacteristic = pClient->getService(myAdvertisedDevice.serviceUUID)->getCharacteristic(myAdvertisedDevice.charUUID); + std::string data = pRemoteBLECharacteristic->getValue(); + uint8_t *pData = reinterpret_cast(&data[0]); + int length = data.length(); + + // 250 == Data(60), Spaces(Data/2), Arrow(4), SvrUUID(37), Sep(3), ChrUUID(37), Sep(3), + // Name(10), Prefix(2), HR(8), SEP(1), CD(10), SEP(1), PW(8), SEP(1), SP(7), Suffix(2), Nul(1) - 225 rounded up + char logBuf[250]; + char *logBufP = logBuf; + for (int i = 0; i < length; i++) + { - debugOutput += String(pData[i], HEX) + " "; + logBufP += sprintf(logBufP, "%02x ", pData[i]); } - debugDirector(debugOutput + "<-" + String(myAdvertisedDevice.serviceUUID.toString().c_str()) + " | " + String(myAdvertisedDevice.charUUID.toString().c_str()), true, true); + logBufP += sprintf(logBufP, "<- %s | %s", myAdvertisedDevice.serviceUUID.toString().c_str(), myAdvertisedDevice.charUUID.toString().c_str()); + + std::shared_ptr sensorData = sensorDataFactory.getSensorData(pRemoteBLECharacteristic, pData, length); - if (pRemoteBLECharacteristic->getUUID() == CYCLINGPOWERMEASUREMENT_UUID) + logBufP += sprintf(logBufP, " | %s:[", sensorData->getId().c_str()); + if (sensorData->hasHeartRate()) + { + int heartRate = sensorData->getHeartRate(); + userConfig.setSimulatedHr(heartRate); + spinBLEClient.connectedHR |= true; + logBufP += sprintf(logBufP, " HR(%d)", heartRate % 1000); + } + if (sensorData->hasCadence()) + { + float cadence = sensorData->getCadence(); + userConfig.setSimulatedCad(cadence); + spinBLEClient.connectedCD |= true; + logBufP += sprintf(logBufP, " CD(%.2f)", fmodf(cadence, 1000.0)); + } + if (sensorData->hasPower()) { - BLE_CPSDecode(pRemoteBLECharacteristic); - if (!spinBLEClient.connectedPM) - { - spinBLEClient.connectedPM = true; - } + int power = sensorData->getPower(); + userConfig.setSimulatedWatts(power); + spinBLEClient.connectedPM |= true; + logBufP += sprintf(logBufP, " PW(%d)", power % 10000); } - if ((pRemoteBLECharacteristic->getUUID() == FITNESSMACHINEINDOORBIKEDATA_UUID) || (pRemoteBLECharacteristic->getUUID() == FLYWHEEL_UART_SERVICE_UUID) || (pRemoteBLECharacteristic->getUUID() == HEARTCHARACTERISTIC_UUID)) + if (sensorData->hasSpeed()) { - BLE_FTMSDecode(pRemoteBLECharacteristic); - if ((!spinBLEClient.connectedPM) &&(pRemoteBLECharacteristic->getUUID()!= HEARTCHARACTERISTIC_UUID)) //PM flag for HR-->PWR - { - spinBLEClient.connectedPM = true; - } + float speed = sensorData->getSpeed(); + userConfig.setSimulatedSpeed(speed); + logBufP += sprintf(logBufP, " SD(%.2f)", fmodf(speed, 1000.0)); } + strcat(logBufP, " ]"); + debugDirector(String(logBuf), true, true); } } } diff --git a/src/BLE_FTMS.cpp b/src/BLE_FTMS.cpp deleted file mode 100644 index bc5775cc..00000000 --- a/src/BLE_FTMS.cpp +++ /dev/null @@ -1,176 +0,0 @@ - // SmartSpin2K code -// This software registers an ESP32 as a BLE FTMS device which then uses a stepper motor to turn the resistance knob on a regular spin bike. -// BLE code based on examples from https://github.com/nkolban -// Copyright 2020 Joel Baranick and Anthony Doud -// This work is licensed under the GNU General Public License v2 -// Prototype hardware build from plans in the SmartSpin2k repository are licensed under Cern Open Hardware Licence version 2 Permissive - - - #include "BLE_Common.h" - #include "Main.h" - -// See: https://github.com/oesmith/gatt-xml/blob/master/org.bluetooth.characteristic.indoor_bike_data.xml -uint8_t const FitnessMachineIndoorBikeData::flagBitIndices[FieldCount] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 8, 8, 9, 10, 11, 12 }; -uint8_t const FitnessMachineIndoorBikeData::flagEnabledValues[FieldCount] = { 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }; -size_t const FitnessMachineIndoorBikeData::byteSizes[FieldCount] = { 2, 2, 2, 2, 3, 2, 2, 2, 2, 2, 1, 1, 1, 2, 2 }; -uint8_t const FitnessMachineIndoorBikeData::signedFlags[FieldCount] = { 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0 }; -double_t const FitnessMachineIndoorBikeData::resolutions[FieldCount] = { 0.01, 0.01, 0.5, 0.5, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.1, 1.0, 1.0 }; - - - void BLE_FTMSDecode(NimBLERemoteCharacteristic *pBLERemoteCharacteristic) - { - std::string data = pBLERemoteCharacteristic->getValue(); - uint8_t *pData = reinterpret_cast(&data[0]); - std::unique_ptr sensorData = SensorDataFactory::getSensorData(pBLERemoteCharacteristic ,pData, data.length()); - debugDirector(" SensorData(" + sensorData->getId() + "):[", false); - if (sensorData->hasHeartRate()) - { - int heartRate = sensorData->getHeartRate(); - userConfig.setSimulatedHr(heartRate); - debugDirector(" HR(" + String(heartRate) + ")", false); - if(!spinBLEClient.connectedHR) - { - spinBLEClient.connectedHR = true; - debugDirector("Registered HRM in FTMS"); - } - } - if (sensorData->hasCadence()) - { - float cadence = sensorData->getCadence(); - userConfig.setSimulatedCad(cadence); - debugDirector(" CD(" + String(cadence) + ")", false); - } - if (sensorData->hasPower()) - { - int power = sensorData->getPower(); - userConfig.setSimulatedWatts(power); - debugDirector(" PW(" + String(power) + ")", false); - } - debugDirector(" ]"); - } - - std::unique_ptr SensorDataFactory::getSensorData(BLERemoteCharacteristic *characteristic, uint8_t *data, size_t length) { - - if (characteristic->getUUID() == HEARTCHARACTERISTIC_UUID) { - return std::unique_ptr(new HeartRateData( data, length)); - } - - if (characteristic->getUUID() == FLYWHEEL_UART_SERVICE_UUID) { - return std::unique_ptr(new FlywheelData( data, length)); - } - - if (characteristic->getUUID() == FITNESSMACHINEINDOORBIKEDATA_UUID) { - return std::unique_ptr(new FitnessMachineIndoorBikeData( data, length)); - } - - return std::unique_ptr(new NullData( data, length)); -} - -String SensorData::getId() { - return id; -} - -bool NullData::hasHeartRate() { return false; } -bool NullData::hasCadence() { return false; } -bool NullData::hasPower() { return false; } -int NullData::getHeartRate() { return INT_MIN; } -float NullData::getCadence() { return NAN; } -int NullData::getPower() { return INT_MIN; } - -bool HeartRateData::hasHeartRate() { return true; } -bool HeartRateData::hasCadence() { return false; } -bool HeartRateData::hasPower() { return false; } -int HeartRateData::getHeartRate() { return (int)data[1]; } -float HeartRateData::getCadence() { return NAN; } -int HeartRateData::getPower() { return INT_MIN; } - -bool FlywheelData::hasHeartRate() { return false; } -bool FlywheelData::hasCadence() { return data[0] == 0xFF; } -bool FlywheelData::hasPower() { return data[0] == 0xFF; } -int FlywheelData::getHeartRate() { return INT_MIN; } - -float FlywheelData::getCadence() { - if (!hasCadence()) { - return NAN; - } - return float(bytes_to_u16(data[4], data[3])); -} - -int FlywheelData::getPower() { - if (!hasPower()) { - return INT_MIN; - } - return data[12]; -} - -bool FitnessMachineIndoorBikeData::hasHeartRate() { - return values[Types::HeartRate] != NAN; -} - -bool FitnessMachineIndoorBikeData::hasCadence() { - return values[Types::InstantaneousCadence] != NAN; -} - -bool FitnessMachineIndoorBikeData::hasPower() { - return values[Types::InstantaneousPower] != NAN; -} - -int FitnessMachineIndoorBikeData::getHeartRate() { - double_t value = values[Types::HeartRate]; - if (value == NAN) { - return INT_MIN; - } - return int(value); -} - -float FitnessMachineIndoorBikeData::getCadence() { - double_t value = values[Types::InstantaneousCadence]; - if (value == NAN) { - return nanf(""); - } - return float(value); -} - -int FitnessMachineIndoorBikeData::getPower() { - double_t value = values[Types::InstantaneousPower]; - if (value == NAN) { - return INT_MIN; - } - return int(value); -} - -FitnessMachineIndoorBikeData::FitnessMachineIndoorBikeData(uint8_t *data, size_t length) : - SensorData("FTMS", data, length), flags(bytes_to_u16(data[1], data[0])) { - uint8_t dataIndex = 2; - values = new double_t[FieldCount]; - std::fill_n(values, FieldCount, NAN); - for (int typeIndex = Types::InstantaneousSpeed; typeIndex <= Types::RemainingTime; typeIndex++) { - if (bitRead(flags, flagBitIndices[typeIndex]) == flagEnabledValues[typeIndex]) { - uint8_t byteSize = byteSizes[typeIndex]; - if (byteSize > 0) { - int value = data[dataIndex]; - for (int dataOffset = 1; dataOffset < byteSize; dataOffset++) { - uint8_t dataByte = data[dataIndex + dataOffset]; - value |= (dataByte << (dataOffset * 8)); - } - dataIndex += byteSize; - value = convert(value, byteSize, signedFlags[typeIndex]); - double_t result = double_t(int((value * resolutions[typeIndex] * 10) + 0.5)) / 10.0; - values[typeIndex] = result; - } - } - } -} - -FitnessMachineIndoorBikeData::~FitnessMachineIndoorBikeData() { - delete []values; -} - -int FitnessMachineIndoorBikeData::convert(int value, size_t length, uint8_t isSigned) { - int mask = 255 * length; - int convertedValue = value & mask; - if (isSigned) { - convertedValue = convertedValue - (convertedValue >> (length * 8 - 1) << (length * 8)); - } - return convertedValue; -} \ No newline at end of file diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp index 8ab18f6d..39024f46 100644 --- a/src/BLE_Server.cpp +++ b/src/BLE_Server.cpp @@ -51,7 +51,7 @@ byte ftmsControlPoint[8] = {0, 0, 0, 0, 0, 0, 0, 0}; //0x08 we need to return a byte ftmsMachineStatus[8] = {0, 0, 0, 0, 0, 0, 0, 0}; uint8_t ftmsFeature[8] = {0x86, 0x50, 0x00, 0x00, 0x0C, 0xE0, 0x00, 0x00}; //101000010000110 1110000000001100 -uint8_t ftmsIndoorBikeData[14] = {0x54, 0x0A, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; //00000000100001010100 ISpeed, ICAD, TDistance, IPower, ETime +uint8_t ftmsIndoorBikeData[9] = {0x44, 0x02, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; //00000000100001010100 ISpeed, ICAD, TDistance, IPower, HR uint8_t ftmsResistanceLevelRange[6] = {0x00, 0x00, 0x3A, 0x98, 0xC5, 0x68}; //+-15000 not sure what units uint8_t ftmsPowerRange[6] = {0x00, 0x00, 0xA0, 0x0F, 0x01, 0x00}; //1-4000 watts @@ -215,24 +215,29 @@ void computeCSC() //What was SIG smoking when they came up with the Cycling Spee void updateIndoorBikeDataChar() { - int cad = userConfig.getSimulatedCad(); + float cadRaw = userConfig.getSimulatedCad(); + int cad = (int)(cadRaw * 2); + int watts = userConfig.getSimulatedWatts(); int hr = userConfig.getSimulatedHr(); - float gearRatio = 1; - int speed = ((cad * 2.75 * 2.08 * 60 * gearRatio) / 10); + + int speed = 0; + float speedRaw = userConfig.getSimulatedSpeed(); + if (speedRaw <= 0) { + float gearRatio = 1; + speed = ((cad * 2.75 * 2.08 * 60 * gearRatio) / 10); + } else { + speed = (int)(speedRaw * 100); + } ftmsIndoorBikeData[2] = (uint8_t)(speed & 0xff); ftmsIndoorBikeData[3] = (uint8_t)(speed >> 8); - ftmsIndoorBikeData[4] = (uint8_t)((cad * 2) & 0xff); - ftmsIndoorBikeData[5] = (uint8_t)((cad * 2) >> 8); // cadence value - ftmsIndoorBikeData[6] = 0; //distance < - ftmsIndoorBikeData[7] = 0; //distance <-- uint24 with 1m resolution - ftmsIndoorBikeData[8] = 0; //distance < - ftmsIndoorBikeData[9] = (uint8_t)((watts)&0xff); - ftmsIndoorBikeData[10] = (uint8_t)((watts) >> 8); // power value, constrained to avoid negative values, although the specification allows for a sint16 - ftmsIndoorBikeData[11] = (uint8_t)hr; - ftmsIndoorBikeData[12] = 0; // Elapsed Time uint16 in seconds - ftmsIndoorBikeData[13] = 0; // Elapsed Time - fitnessMachineIndoorBikeData->setValue(ftmsIndoorBikeData, 14); + ftmsIndoorBikeData[4] = (uint8_t)(cad & 0xff); + ftmsIndoorBikeData[5] = (uint8_t)(cad >> 8); // cadence value + ftmsIndoorBikeData[6] = (uint8_t)(watts & 0xff); + ftmsIndoorBikeData[7] = (uint8_t)(watts >> 8); // power value, constrained to avoid negative values, although the specification allows for a sint16 + ftmsIndoorBikeData[8] = (uint8_t)hr; + fitnessMachineIndoorBikeData->setValue(ftmsIndoorBikeData, 9); + fitnessMachineFeature->notify(); fitnessMachineIndoorBikeData->notify(); } //^^Using the New Way of setting Bytes. @@ -245,26 +250,36 @@ void updateCyclingPowerMesurementChar() cyclingPowerMeasurement[2] = remainder; cyclingPowerMeasurement[3] = quotient; cyclingPowerMeasurementCharacteristic->setValue(cyclingPowerMeasurement, 9); - debugDirector(""); - for (const auto &text : cyclingPowerMeasurement) - { // Range-for! - debugDirector(String(text, HEX) + " ", false); + + // Data(18), Sep(data/2), Static(13), Nul(1) == 41, rounded up + char logBuf[50]; + char *logBufP = logBuf; + for (const auto &it : cyclingPowerMeasurement) + { + logBufP += sprintf(logBufP, "%02x ", it); } + strcat(logBufP, "<-- CPMC sent"); + cyclingPowerMeasurementCharacteristic->notify(); - debugDirector("<-- CPMC sent ", false); - debugDirector(""); + debugDirector(String(logBuf), true); } void updateHeartRateMeasurementChar() { heartRateMeasurement[1] = userConfig.getSimulatedHr(); heartRateMeasurementCharacteristic->setValue(heartRateMeasurement, 5); - for (const auto &text : heartRateMeasurement) - { // Range-for! - debugDirector(String(text, HEX) + " ", false); + + // Data(10), Sep(data/2), Static(11), Nul(1) == 26, rounded up + char logBuf[35]; + char *logBufP = logBuf; + for (const auto &it : heartRateMeasurement) + { + logBufP += sprintf(logBufP, "%02x ", it); } - debugDirector("<-- HR sent ", false); + strcat(logBufP, "<-- HR sent"); + heartRateMeasurementCharacteristic->notify(); + debugDirector(String(logBuf), true); } //Creating Server Connection Callbacks diff --git a/src/sensors/CyclePowerData.cpp b/src/sensors/CyclePowerData.cpp new file mode 100644 index 00000000..484485a1 --- /dev/null +++ b/src/sensors/CyclePowerData.cpp @@ -0,0 +1,103 @@ +#include "BLE_Common.h" +#include "sensors/CyclePowerData.h" + +bool CyclePowerData::hasHeartRate() { return false; } + +bool CyclePowerData::hasCadence() { return this->cadence > 0 || this->missedReadingCount < 3; } + +bool CyclePowerData::hasPower() { return true; } + +bool CyclePowerData::hasSpeed() { return false; } + +int CyclePowerData::getHeartRate() { return INT_MIN; } + +float CyclePowerData::getCadence() +{ + if (!this->hasCadence()) + { + return NAN; + } + return this->cadence; +} + +int CyclePowerData::getPower() +{ + if (!this->hasPower()) + { + return INT_MIN; + } + return this->power; +} + +float CyclePowerData::getSpeed() { return NAN; } + +void CyclePowerData::decode(uint8_t *data, size_t length) +{ + byte flags = data[0]; + int cPos = 2; //lowest position cadence could ever be + //Instanious power is always present. Do that first. + //first calculate which fields are present. Power is always 2 & 3, cadence can move depending on the flags. + this->power = bytes_to_u16(data[cPos + 1], data[cPos]); + cPos += 2; + + if (bitRead(flags, 0)) + { + //pedal balance field present + cPos++; + } + if (bitRead(flags, 1)) + { + //pedal power balance reference + //no field associated with this. + } + if (bitRead(flags, 2)) + { + //accumulated torque field present + cPos += 2; + } + if (bitRead(flags, 3)) + { + //accumulated torque field source + //no field associated with this. + } + if (bitRead(flags, 4)) + { + //Wheel Revolution field PAIR Data present. 32-bits for wheel revs, 16 bits for wheel event time. + //Why is that so hard to find in the specs? + cPos += 6; + } + if (bitRead(flags, 5)) + { + //Crank Revolution data present, lets process it. + this->lastCrankRev = this->crankRev; + this->crankRev = bytes_to_int(data[cPos + 1], data[cPos]); + this->lastCrankEventTime = this->crankEventTime; + this->crankEventTime = bytes_to_int(data[cPos + 3], data[cPos + 2]); + if (this->crankRev > this->lastCrankRev && this->crankEventTime - this->lastCrankEventTime != 0) + { + int cadence = abs(this->crankRev - this->lastCrankRev * 1024) / abs(this->crankEventTime - this->lastCrankEventTime) * 60; + if (cadence > 1) + { + if (cadence > 200) //Cadence Error + { + cadence = 0; + } + + this->cadence = cadence; + this->missedReadingCount = 0; + } + else + { + this->missedReadingCount++; + } + } + else //the crank rev probably didn't update + { + if (this->missedReadingCount > 2) //Require three consecutive readings before setting 0 cadence + { + this->cadence = 0; + } + this->missedReadingCount++; + } + } +} \ No newline at end of file diff --git a/src/sensors/FitnessMachineIndoorBikeData.cpp b/src/sensors/FitnessMachineIndoorBikeData.cpp new file mode 100644 index 00000000..2324aac6 --- /dev/null +++ b/src/sensors/FitnessMachineIndoorBikeData.cpp @@ -0,0 +1,108 @@ +#include "BLE_Common.h" +#include "sensors/FitnessMachineIndoorBikeData.h" + +// See: https://github.com/oesmith/gatt-xml/blob/master/org.bluetooth.characteristic.indoor_bike_data.xml +uint8_t const FitnessMachineIndoorBikeData::flagBitIndices[FieldCount] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 8, 8, 9, 10, 11, 12}; +uint8_t const FitnessMachineIndoorBikeData::flagEnabledValues[FieldCount] = {0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; +size_t const FitnessMachineIndoorBikeData::byteSizes[FieldCount] = {2, 2, 2, 2, 3, 2, 2, 2, 2, 2, 1, 1, 1, 2, 2}; +uint8_t const FitnessMachineIndoorBikeData::signedFlags[FieldCount] = {0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0}; +double_t const FitnessMachineIndoorBikeData::resolutions[FieldCount] = {0.01, 0.01, 0.5, 0.5, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.1, 1.0, 1.0}; + +bool FitnessMachineIndoorBikeData::hasHeartRate() +{ + return values[Types::HeartRate] != NAN; +} + +bool FitnessMachineIndoorBikeData::hasCadence() +{ + return values[Types::InstantaneousCadence] != NAN; +} + +bool FitnessMachineIndoorBikeData::hasPower() +{ + return values[Types::InstantaneousPower] != NAN; +} + +bool FitnessMachineIndoorBikeData::hasSpeed() +{ + return values[Types::InstantaneousSpeed] != NAN; +} + +int FitnessMachineIndoorBikeData::getHeartRate() +{ + double_t value = values[Types::HeartRate]; + if (value == NAN) + { + return INT_MIN; + } + return int(value); +} + +float FitnessMachineIndoorBikeData::getCadence() +{ + double_t value = values[Types::InstantaneousCadence]; + if (value == NAN) + { + return nanf(""); + } + return float(value); +} + +int FitnessMachineIndoorBikeData::getPower() +{ + double_t value = values[Types::InstantaneousPower]; + if (value == NAN) + { + return INT_MIN; + } + return int(value); +} + +float FitnessMachineIndoorBikeData::getSpeed() +{ + double_t value = values[Types::InstantaneousSpeed]; + if (value == NAN) + { + return nanf(""); + } + return float(value); +} + +void FitnessMachineIndoorBikeData::decode(uint8_t *data, size_t length) +{ + int flags = bytes_to_u16(data[1], data[0]); + uint8_t dataIndex = 2; + for (int typeIndex = Types::InstantaneousSpeed; typeIndex <= Types::RemainingTime; typeIndex++) + { + if (bitRead(flags, flagBitIndices[typeIndex]) == flagEnabledValues[typeIndex]) + { + uint8_t byteSize = byteSizes[typeIndex]; + if (byteSize > 0) + { + int value = data[dataIndex]; + for (int dataOffset = 1; dataOffset < byteSize; dataOffset++) + { + uint8_t dataByte = data[dataIndex + dataOffset]; + value |= (dataByte << (dataOffset * 8)); + } + dataIndex += byteSize; + value = convert(value, byteSize, signedFlags[typeIndex]); + double_t result = double_t(int((value * resolutions[typeIndex] * 10) + 0.5)) / 10.0; + values[typeIndex] = result; + continue; + } + } + values[typeIndex] = NAN; + } +} + +int FitnessMachineIndoorBikeData::convert(int value, size_t length, uint8_t isSigned) +{ + int mask = (1u << (8 * length)) - (1u); + int convertedValue = value & mask; + if (isSigned) + { + convertedValue = convertedValue - (convertedValue >> (length * 8 - 1) << (length * 8)); + } + return convertedValue; +} diff --git a/src/sensors/FlywheelData.cpp b/src/sensors/FlywheelData.cpp new file mode 100644 index 00000000..79a2f554 --- /dev/null +++ b/src/sensors/FlywheelData.cpp @@ -0,0 +1,34 @@ +#include "BLE_Common.h" +#include "sensors/FlywheelData.h" + +bool FlywheelData::hasHeartRate() { return false; } + +bool FlywheelData::hasCadence() { return this->hasData; } + +bool FlywheelData::hasPower() { return this->hasData; } + +bool FlywheelData::hasSpeed() { return false; } + +int FlywheelData::getHeartRate() { return INT_MIN; } + +float FlywheelData::getCadence() { return this->cadence; }; + +int FlywheelData::getPower() { return this->power; }; + +float FlywheelData::getSpeed() { return NAN; } + +void FlywheelData::decode(uint8_t *data, size_t length) +{ + if (data[0] == 0xFF) + { + cadence = float(bytes_to_u16(data[4], data[3])); + power = data[12]; + hasData = true; + } + else + { + cadence = NAN; + power = INT_MIN; + hasData = false; + } +} \ No newline at end of file diff --git a/src/sensors/HeartRateData.cpp b/src/sensors/HeartRateData.cpp new file mode 100644 index 00000000..604ff6e2 --- /dev/null +++ b/src/sensors/HeartRateData.cpp @@ -0,0 +1,22 @@ +#include "sensors/HeartRateData.h" + +bool HeartRateData::hasHeartRate() { return true; } + +bool HeartRateData::hasCadence() { return false; } + +bool HeartRateData::hasPower() { return false; } + +bool HeartRateData::hasSpeed() { return false; } + +int HeartRateData::getHeartRate() { return this->heartrate; } + +float HeartRateData::getCadence() { return NAN; } + +int HeartRateData::getPower() { return INT_MIN; } + +float HeartRateData::getSpeed() { return NAN; } + +void HeartRateData::decode(uint8_t *data, size_t length) +{ + this->heartrate = (int)data[1]; +} \ No newline at end of file diff --git a/src/sensors/SensorData.cpp b/src/sensors/SensorData.cpp new file mode 100644 index 00000000..34d51192 --- /dev/null +++ b/src/sensors/SensorData.cpp @@ -0,0 +1,3 @@ +#include "sensors/SensorData.h" + +String SensorData::getId() { return this->id; } \ No newline at end of file diff --git a/src/sensors/SensorDataFactory.cpp b/src/sensors/SensorDataFactory.cpp new file mode 100644 index 00000000..68ea30f3 --- /dev/null +++ b/src/sensors/SensorDataFactory.cpp @@ -0,0 +1,76 @@ +#include "BLE_Common.h" +#include "sensors/SensorDataFactory.h" +#include "sensors/SensorData.h" +#include "sensors/CyclePowerData.h" +#include "sensors/FlywheelData.h" +#include "sensors/FitnessMachineIndoorBikeData.h" +#include "sensors/HeartRateData.h" + +std::shared_ptr SensorDataFactory::getSensorData(BLERemoteCharacteristic *characteristic, uint8_t *data, size_t length) +{ + auto uuid = characteristic->getUUID(); + for (auto &it : SensorDataFactory::knownDevices) + { + if (it->getUUID() == uuid) + { + return it->decode(data, length); + } + } + + std::shared_ptr sensorData = NULL_SENSOR_DATA; + if (uuid == CYCLINGPOWERMEASUREMENT_UUID) + { + sensorData = std::shared_ptr(new CyclePowerData()); + } + else if (uuid == HEARTCHARACTERISTIC_UUID) + { + sensorData = std::shared_ptr(new HeartRateData()); + } + else if (uuid == FITNESSMACHINEINDOORBIKEDATA_UUID) + { + sensorData = std::shared_ptr(new FitnessMachineIndoorBikeData()); + } + else if (uuid == FLYWHEEL_UART_SERVICE_UUID) + { + sensorData = std::shared_ptr(new FlywheelData()); + } + else + { + return NULL_SENSOR_DATA; + } + + KnownDevice *knownDevice = new KnownDevice(uuid, sensorData); + SensorDataFactory::knownDevices.push_back(knownDevice); + return knownDevice->decode(data, length); +} + +NimBLEUUID SensorDataFactory::KnownDevice::getUUID() +{ + return this->uuid; +} + +std::shared_ptr SensorDataFactory::KnownDevice::decode(uint8_t *data, size_t length) +{ + this->sensorData->decode(data, length); + return this->sensorData; +} + +bool SensorDataFactory::NullData::hasHeartRate() { return false; } + +bool SensorDataFactory::NullData::hasCadence() { return false; } + +bool SensorDataFactory::NullData::hasPower() { return false; } + +bool SensorDataFactory::NullData::hasSpeed() { return false; } + +int SensorDataFactory::NullData::getHeartRate() { return INT_MIN; } + +float SensorDataFactory::NullData::getCadence() { return NAN; } + +int SensorDataFactory::NullData::getPower() { return INT_MIN; } + +float SensorDataFactory::NullData::getSpeed() { return NAN; }; + +void SensorDataFactory::NullData::decode(uint8_t *data, size_t length) {} + +std::shared_ptr SensorDataFactory::NULL_SENSOR_DATA = std::shared_ptr(new NullData()); \ No newline at end of file diff --git a/test/FitnessMachineIndoorBikeData/test_FitnessMachineIndoorBikeData.cpp b/test/FitnessMachineIndoorBikeData/test_FitnessMachineIndoorBikeData.cpp index 5a6b74f5..cf003ef6 100644 --- a/test/FitnessMachineIndoorBikeData/test_FitnessMachineIndoorBikeData.cpp +++ b/test/FitnessMachineIndoorBikeData/test_FitnessMachineIndoorBikeData.cpp @@ -4,19 +4,22 @@ static uint8_t data[9] = { 0x44, 0x02, 0xf2, 0x08, 0xb0, 0x00, 0x40, 0x00, 0x00 }; void test_parses_heartrate(void) { - FitnessMachineIndoorBikeData sensor = FitnessMachineIndoorBikeData(data, 9); + FitnessMachineIndoorBikeData sensor = FitnessMachineIndoorBikeData(); + sensor.decode(data, 9); TEST_ASSERT_TRUE(sensor.hasHeartRate()); TEST_ASSERT_EQUAL(0, sensor.getHeartRate()); } void test_parses_cadence(void) { - FitnessMachineIndoorBikeData sensor = FitnessMachineIndoorBikeData(data, 9); + FitnessMachineIndoorBikeData sensor = FitnessMachineIndoorBikeData(); + sensor.decode(data, 9); TEST_ASSERT_TRUE(sensor.hasCadence()); TEST_ASSERT_EQUAL(88, sensor.getCadence()); } void test_parses_power(void) { - FitnessMachineIndoorBikeData sensor = FitnessMachineIndoorBikeData(data, 9); + FitnessMachineIndoorBikeData sensor = FitnessMachineIndoorBikeData(); + sensor.decode(data, 9); TEST_ASSERT_TRUE(sensor.hasPower()); TEST_ASSERT_EQUAL(64, sensor.getPower()); }