Skip to content

Commit

Permalink
HRM data now updates correctly from multiple sensors
Browse files Browse the repository at this point in the history
  • Loading branch information
@doudar
doudar committed Feb 19, 2025
1 parent 7cdc41d commit 01a75ae
Show file tree
Hide file tree
Showing 6 changed files with 90 additions and 44 deletions.
3 changes: 1 addition & 2 deletions include/BLE_Fitness_Machine_Service.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
extern BLE_Fitness_Machine_Service fitnessMachineService;
7 changes: 4 additions & 3 deletions include/BLE_Heart_Service.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
NimBLEService *pHeartService;
NimBLECharacteristic *heartRateMeasurementCharacteristic;
};
2 changes: 1 addition & 1 deletion include/settings.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
76 changes: 44 additions & 32 deletions src/BLE_Fitness_Machine_Service.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@

#include "BLE_Fitness_Machine_Service.h"
#include <Constants.h>
#include <vector>

BLE_Fitness_Machine_Service::BLE_Fitness_Machine_Service()
: pFitnessMachineService(nullptr),
Expand All @@ -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);
Expand All @@ -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<uint8_t>(ftmsIBDFlags & 0xFF); // LSB, mask with 0xFF to get the lower 8 bits
ftmsIndoorBikeData[1] = static_cast<uint8_t>((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));
Expand All @@ -62,45 +57,62 @@ void BLE_Fitness_Machine_Service::setupService(NimBLEServer *pServer, MyCharacte
}

void BLE_Fitness_Machine_Service::update() {
std::vector<uint8_t> ftmsIndoorBikeData;

this->processFTMSWrite();
/*if (!spinBLEServer.clientSubscribed.IndoorBikeData) {
return;
}*/
float cadRaw = rtConfig->cad.getValue();
int cad = static_cast<int>(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;
} else {
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<uint8_t>(ftmsIBDFlags & 0xFF));
ftmsIndoorBikeData.push_back(static_cast<uint8_t>((ftmsIBDFlags >> 8) & 0xFF));

// Add speed
ftmsIndoorBikeData.push_back(static_cast<uint8_t>(speedFtmsUnit & 0xff));
ftmsIndoorBikeData.push_back(static_cast<uint8_t>(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<uint8_t>(static_cast<int>(rtConfig->cad.getValue() * 2) & 0xff));
ftmsIndoorBikeData.push_back(static_cast<uint8_t>(static_cast<int>(rtConfig->cad.getValue() * 2) >> 8));

ftmsIndoorBikeData[6] = (uint8_t)(res & 0xff);
ftmsIndoorBikeData[7] = (uint8_t)(res >> 8);
// Add resistance
ftmsIndoorBikeData.push_back(static_cast<uint8_t>(rtConfig->resistance.getValue() & 0xff));
ftmsIndoorBikeData.push_back(static_cast<uint8_t>(rtConfig->resistance.getValue() >> 8));

ftmsIndoorBikeData[8] = (uint8_t)(watts & 0xff);
ftmsIndoorBikeData[9] = (uint8_t)(watts >> 8);
// Add power
ftmsIndoorBikeData.push_back(static_cast<uint8_t>(rtConfig->watts.getValue() & 0xff));
ftmsIndoorBikeData.push_back(static_cast<uint8_t>(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<uint8_t>(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.
Expand All @@ -111,9 +123,9 @@ void BLE_Fitness_Machine_Service::processFTMSWrite() {
if (rxValue == "") {
return;
}
std::vector<uint8_t> returnValue = {FitnessMachineControlPointProcedure::ResponseCode, (uint8_t)rxValue[0], FitnessMachineControlPointResultCode::OpCodeNotSupported};
BLECharacteristic *pCharacteristic = NimBLEDevice::getServer()->getServiceByUUID(FITNESSMACHINESERVICE_UUID)->getCharacteristic(FITNESSMACHINECONTROLPOINT_UUID);
std::vector<uint8_t> ftmsStatus = {FitnessMachineStatus::ReservedForFutureUse};
std::vector<uint8_t> returnValue = {FitnessMachineControlPointProcedure::ResponseCode, (uint8_t)rxValue[0], FitnessMachineControlPointResultCode::OpCodeNotSupported};
BLECharacteristic *pCharacteristic = NimBLEDevice::getServer()->getServiceByUUID(FITNESSMACHINESERVICE_UUID)->getCharacteristic(FITNESSMACHINECONTROLPOINT_UUID);
std::vector<uint8_t> ftmsStatus = {FitnessMachineStatus::ReservedForFutureUse};
std::vector<uint8_t> ftmsTrainingStatus = {0x00, FitnessMachineTrainingStatus::Other};

if (rxValue.length() >= 1) {
Expand Down
27 changes: 24 additions & 3 deletions src/BLE_Heart_Service.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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);
}
}
19 changes: 16 additions & 3 deletions src/SensorCollector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down

0 comments on commit 01a75ae

Please sign in to comment.