From 01a75ae9cfa136fdeb5ce0e14c3d740f6d167b2a Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Wed, 19 Feb 2025 14:39:30 -0800 Subject: [PATCH] HRM data now updates correctly from multiple sensors --- include/BLE_Fitness_Machine_Service.h | 3 +- include/BLE_Heart_Service.h | 7 +-- include/settings.h | 2 +- src/BLE_Fitness_Machine_Service.cpp | 76 ++++++++++++++++----------- src/BLE_Heart_Service.cpp | 27 ++++++++-- src/SensorCollector.cpp | 19 +++++-- 6 files changed, 90 insertions(+), 44 deletions(-) diff --git a/include/BLE_Fitness_Machine_Service.h b/include/BLE_Fitness_Machine_Service.h index 386ff7e9..8fa14814 100644 --- a/include/BLE_Fitness_Machine_Service.h +++ b/include/BLE_Fitness_Machine_Service.h @@ -27,8 +27,7 @@ class BLE_Fitness_Machine_Service { BLECharacteristic *fitnessMachinePowerRange; BLECharacteristic *fitnessMachineInclinationRange; BLECharacteristic *fitnessMachineTrainingStatus; - uint8_t ftmsIndoorBikeData[11] = {0}; void processFTMSWrite(); }; -extern BLE_Fitness_Machine_Service fitnessMachineService; \ No newline at end of file +extern BLE_Fitness_Machine_Service fitnessMachineService; diff --git a/include/BLE_Heart_Service.h b/include/BLE_Heart_Service.h index 49755e7e..c6f21186 100644 --- a/include/BLE_Heart_Service.h +++ b/include/BLE_Heart_Service.h @@ -15,8 +15,9 @@ class BLE_Heart_Service { BLE_Heart_Service(); void setupService(NimBLEServer *pServer, MyCharacteristicCallbacks *chrCallbacks); void update(); + void deinit(); private: -BLEService *pHeartService; -BLECharacteristic *heartRateMeasurementCharacteristic; -}; \ No newline at end of file + NimBLEService *pHeartService; + NimBLECharacteristic *heartRateMeasurementCharacteristic; +}; diff --git a/include/settings.h b/include/settings.h index b688dc4b..04fed84f 100644 --- a/include/settings.h +++ b/include/settings.h @@ -351,7 +351,7 @@ const char* const DEFAULT_PASSWORD = "password"; // #define DEBUG_TORQUETABLE // Uncomment to enable BLE_TX_RX Logging -// #define DEBUG_BLE_TX_RX +#define DEBUG_BLE_TX_RX // UNcomment to enable Custom Characteristic Logging // #define CUSTOM_CHAR_DEBUG diff --git a/src/BLE_Fitness_Machine_Service.cpp b/src/BLE_Fitness_Machine_Service.cpp index 93f16316..5925ae90 100644 --- a/src/BLE_Fitness_Machine_Service.cpp +++ b/src/BLE_Fitness_Machine_Service.cpp @@ -7,6 +7,7 @@ #include "BLE_Fitness_Machine_Service.h" #include +#include BLE_Fitness_Machine_Service::BLE_Fitness_Machine_Service() : pFitnessMachineService(nullptr), @@ -32,10 +33,6 @@ void BLE_Fitness_Machine_Service::setupService(NimBLEServer *pServer, MyCharacte FitnessMachineTargetFlags::Types::ResistanceTargetSettingSupported | FitnessMachineTargetFlags::Types::IndoorBikeSimulationParametersSupported | FitnessMachineTargetFlags::Types::SpinDownControlSupported}; - // Fitness Machine Indoor Bike Data Flags Setup - FitnessMachineIndoorBikeDataFlags::Types ftmsIBDFlags = FitnessMachineIndoorBikeDataFlags::InstantaneousCadencePresent | - FitnessMachineIndoorBikeDataFlags::ResistanceLevelPresent | FitnessMachineIndoorBikeDataFlags::InstantaneousPowerPresent | - FitnessMachineIndoorBikeDataFlags::HeartRatePresent; // Fitness Machine service setup pFitnessMachineService = spinBLEServer.pServer->createService(FITNESSMACHINESERVICE_UUID); @@ -49,9 +46,7 @@ void BLE_Fitness_Machine_Service::setupService(NimBLEServer *pServer, MyCharacte fitnessMachineTrainingStatus = pFitnessMachineService->createCharacteristic(FITNESSMACHINETRAININGSTATUS_UUID, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); fitnessMachineFeature->setValue(ftmsFeature.bytes, sizeof(ftmsFeature)); - ftmsIndoorBikeData[0] = static_cast(ftmsIBDFlags & 0xFF); // LSB, mask with 0xFF to get the lower 8 bits - ftmsIndoorBikeData[1] = static_cast((ftmsIBDFlags >> 8) & 0xFF); // MSB, shift right by 8 bits and mask with 0xFF - fitnessMachineIndoorBikeData->setValue(ftmsIndoorBikeData, sizeof(ftmsIndoorBikeData)); + fitnessMachineResistanceLevelRange->setValue(ftmsResistanceLevelRange, sizeof(ftmsResistanceLevelRange)); fitnessMachinePowerRange->setValue(ftmsPowerRange, sizeof(ftmsPowerRange)); fitnessMachineInclinationRange->setValue(ftmsInclinationRange, sizeof(ftmsInclinationRange)); @@ -62,15 +57,11 @@ void BLE_Fitness_Machine_Service::setupService(NimBLEServer *pServer, MyCharacte } void BLE_Fitness_Machine_Service::update() { + std::vector ftmsIndoorBikeData; + this->processFTMSWrite(); - /*if (!spinBLEServer.clientSubscribed.IndoorBikeData) { - return; - }*/ - float cadRaw = rtConfig->cad.getValue(); - int cad = static_cast(cadRaw * 2); - int watts = rtConfig->watts.getValue(); - int hr = rtConfig->hr.getValue(); - int res = rtConfig->resistance.getValue(); + + // Calculate Speed for FTMS int speedFtmsUnit = 0; if (rtConfig->getSimulatedSpeed() > 5) { speedFtmsUnit = rtConfig->getSimulatedSpeed() * 100; @@ -78,29 +69,50 @@ void BLE_Fitness_Machine_Service::update() { speedFtmsUnit = spinBLEServer.calculateSpeed() * 100; } - ftmsIndoorBikeData[2] = (uint8_t)(speedFtmsUnit & 0xff); - ftmsIndoorBikeData[3] = (uint8_t)(speedFtmsUnit >> 8); + // Rebuild ftmsIndoorBikeData vector with current values + ftmsIndoorBikeData.clear(); + + // Fitness Machine Indoor Bike Data Flags Setup + FitnessMachineIndoorBikeDataFlags::Types ftmsIBDFlags = FitnessMachineIndoorBikeDataFlags::InstantaneousCadencePresent | + FitnessMachineIndoorBikeDataFlags::ResistanceLevelPresent | FitnessMachineIndoorBikeDataFlags::InstantaneousPowerPresent; + + if (String(userConfig->getConnectedHeartMonitor()) != "none") { + ftmsIBDFlags = ftmsIBDFlags | FitnessMachineIndoorBikeDataFlags::HeartRatePresent; + } + + // Add flags + ftmsIndoorBikeData.push_back(static_cast(ftmsIBDFlags & 0xFF)); + ftmsIndoorBikeData.push_back(static_cast((ftmsIBDFlags >> 8) & 0xFF)); + + // Add speed + ftmsIndoorBikeData.push_back(static_cast(speedFtmsUnit & 0xff)); + ftmsIndoorBikeData.push_back(static_cast(speedFtmsUnit >> 8)); - ftmsIndoorBikeData[4] = (uint8_t)(cad & 0xff); - ftmsIndoorBikeData[5] = (uint8_t)(cad >> 8); + // Add cadence. FTMS expects cadence in 0.5 RPM units + ftmsIndoorBikeData.push_back(static_cast(static_cast(rtConfig->cad.getValue() * 2) & 0xff)); + ftmsIndoorBikeData.push_back(static_cast(static_cast(rtConfig->cad.getValue() * 2) >> 8)); - ftmsIndoorBikeData[6] = (uint8_t)(res & 0xff); - ftmsIndoorBikeData[7] = (uint8_t)(res >> 8); + // Add resistance + ftmsIndoorBikeData.push_back(static_cast(rtConfig->resistance.getValue() & 0xff)); + ftmsIndoorBikeData.push_back(static_cast(rtConfig->resistance.getValue() >> 8)); - ftmsIndoorBikeData[8] = (uint8_t)(watts & 0xff); - ftmsIndoorBikeData[9] = (uint8_t)(watts >> 8); + // Add power + ftmsIndoorBikeData.push_back(static_cast(rtConfig->watts.getValue() & 0xff)); + ftmsIndoorBikeData.push_back(static_cast(rtConfig->watts.getValue() >> 8)); - ftmsIndoorBikeData[10] = (uint8_t)hr; + // Add heart rate if HRM is connected + if (String(userConfig->getConnectedHeartMonitor()) != "none") { + ftmsIndoorBikeData.push_back(static_cast(rtConfig->hr.getValue())); + } - fitnessMachineIndoorBikeData->setValue(ftmsIndoorBikeData, sizeof(ftmsIndoorBikeData)); - fitnessMachineIndoorBikeData->notify(); + fitnessMachineIndoorBikeData->notify(ftmsIndoorBikeData.data(), ftmsIndoorBikeData.size()); const int kLogBufCapacity = 200; // Data(30), Sep(data/2), Arrow(3), CharId(37), Sep(3), CharId(37), Sep(3), Name(10), Prefix(2), HR(7), SEP(1), CD(10), SEP(1), PW(8), // SEP(1), SD(7), Suffix(2), Nul(1), rounded up char logBuf[kLogBufCapacity]; - const size_t ftmsIndoorBikeDataLength = sizeof(ftmsIndoorBikeData) / sizeof(ftmsIndoorBikeData[0]); - logCharacteristic(logBuf, kLogBufCapacity, ftmsIndoorBikeData, ftmsIndoorBikeDataLength, FITNESSMACHINESERVICE_UUID, fitnessMachineIndoorBikeData->getUUID(), - "FTMS(IBD)[ HR(%d) CD(%.2f) PW(%d) SD(%.2f) ]", hr % 1000, fmodf(cadRaw, 1000.0), watts % 10000, fmodf((float)speedFtmsUnit / 100.0, 1000.0)); + logCharacteristic(logBuf, kLogBufCapacity, ftmsIndoorBikeData.data(), ftmsIndoorBikeData.size(), FITNESSMACHINESERVICE_UUID, fitnessMachineIndoorBikeData->getUUID(), + "FTMS(IBD)[ HR(%d) CD(%.2f) PW(%d) SD(%.2f) ]", rtConfig->hr.getValue() % 1000, fmodf(rtConfig->cad.getValue(), 1000.0), rtConfig->watts.getValue() % 10000, + fmodf((float)speedFtmsUnit / 100.0, 1000.0)); } // The things that happen when we receive a FitnessMachineControlPointProcedure from a Client. @@ -111,9 +123,9 @@ void BLE_Fitness_Machine_Service::processFTMSWrite() { if (rxValue == "") { return; } - std::vector returnValue = {FitnessMachineControlPointProcedure::ResponseCode, (uint8_t)rxValue[0], FitnessMachineControlPointResultCode::OpCodeNotSupported}; - BLECharacteristic *pCharacteristic = NimBLEDevice::getServer()->getServiceByUUID(FITNESSMACHINESERVICE_UUID)->getCharacteristic(FITNESSMACHINECONTROLPOINT_UUID); - std::vector ftmsStatus = {FitnessMachineStatus::ReservedForFutureUse}; + std::vector returnValue = {FitnessMachineControlPointProcedure::ResponseCode, (uint8_t)rxValue[0], FitnessMachineControlPointResultCode::OpCodeNotSupported}; + BLECharacteristic *pCharacteristic = NimBLEDevice::getServer()->getServiceByUUID(FITNESSMACHINESERVICE_UUID)->getCharacteristic(FITNESSMACHINECONTROLPOINT_UUID); + std::vector ftmsStatus = {FitnessMachineStatus::ReservedForFutureUse}; std::vector ftmsTrainingStatus = {0x00, FitnessMachineTrainingStatus::Other}; if (rxValue.length() >= 1) { diff --git a/src/BLE_Heart_Service.cpp b/src/BLE_Heart_Service.cpp index 0cf1afbc..d159707f 100644 --- a/src/BLE_Heart_Service.cpp +++ b/src/BLE_Heart_Service.cpp @@ -18,13 +18,34 @@ void BLE_Heart_Service::setupService(NimBLEServer *pServer, MyCharacteristicCall heartRateMeasurementCharacteristic->setValue(heartRateMeasurement, 2); heartRateMeasurementCharacteristic->setCallbacks(chrCallbacks); pHeartService->start(); - //spinBLEServer.pServer->getAdvertising()->addServiceUUID(pHeartService->getUUID()); +} + +void BLE_Heart_Service::deinit() { + if (pHeartService != nullptr) { + if (heartRateMeasurementCharacteristic != nullptr) { + pHeartService->removeCharacteristic(heartRateMeasurementCharacteristic); + heartRateMeasurementCharacteristic = nullptr; + } + spinBLEServer.pServer->removeService(pHeartService); + pHeartService = nullptr; + spinBLEServer.pServer->getAdvertising()->stop(); + SS2K_LOG(BLE_COMMON_LOG_TAG, "Heart Rate Service De-initialized"); + } } void BLE_Heart_Service::update() { - /*if (!spinBLEServer.clientSubscribed.Heartrate) { + /* Check if heart rate monitor is connected + if (String(userConfig->getConnectedHeartMonitor()) == "none") { + deinit(); return; + } + + if (!pHeartService) { + // Re-initialize if needed, most likely after changing HRM from none to a specific device + MyCharacteristicCallbacks* chrCallbacks; + setupService(spinBLEServer.pServer, chrCallbacks); }*/ + byte heartRateMeasurement[2] = {0x00, (byte)rtConfig->hr.getValue()}; heartRateMeasurementCharacteristic->setValue(heartRateMeasurement, 2); heartRateMeasurementCharacteristic->notify(); @@ -34,4 +55,4 @@ void BLE_Heart_Service::update() { const size_t heartRateMeasurementLength = sizeof(heartRateMeasurement) / sizeof(heartRateMeasurement[0]); logCharacteristic(logBuf, kLogBufCapacity, heartRateMeasurement, heartRateMeasurementLength, HEARTSERVICE_UUID, heartRateMeasurementCharacteristic->getUUID(), "HRS(HRM)[ HR(%d) ]", rtConfig->hr.getValue() % 1000); -} \ No newline at end of file +} diff --git a/src/SensorCollector.cpp b/src/SensorCollector.cpp index 1efbe7db..54d1771e 100644 --- a/src/SensorCollector.cpp +++ b/src/SensorCollector.cpp @@ -27,9 +27,22 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, NimBLEAddress ad logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " | %s[", sensorData->getId().c_str()); if (sensorData->hasHeartRate() && !rtConfig->hr.getSimulate()) { int heartRate = sensorData->getHeartRate(); - rtConfig->hr.setValue(heartRate); - spinBLEClient.connectedHRM = true; - logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " HR(%d)", heartRate % 1000); + static int zeroCount = 0; + zeroCount++; + if (heartRate > 0) { + rtConfig->hr.setValue(heartRate); + logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " HR(%d)", heartRate % 1000); + spinBLEClient.connectedHRM = true; + zeroCount = 0; + }else{ + //require 10 readings in a row before setting the HR to 0 + if (zeroCount > 10){ + rtConfig->hr.setValue(0); + logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " HR IGNORED", 0); + spinBLEClient.connectedHRM = false; + zeroCount = 0; + } + } } if (sensorData->hasCadence() && !rtConfig->cad.getSimulate()) {