Skip to content

Commit

Permalink
Adds a userConfig parameter to disable FTMS control point write
Browse files Browse the repository at this point in the history 
Not completely filled out. This should be moved to a trainer evaluation on if they support control point write.
  • Loading branch information
@doudar
doudar committed Jan 20, 2024
1 parent c75f2e3 commit 98923d3
Show file tree
Hide file tree
Showing 2 changed files with 49 additions and 41 deletions.
4 changes: 4 additions & 0 deletions include/SmartSpin_parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,6 +122,7 @@ class userParameters {
bool shifterDir;
bool udpLogEnabled = false;
bool logComm = false;
bool FTMSControlPointWrite = false;
String ssid;
String password;
String connectedPowerMeter = CONNECTED_POWER_METER;
Expand All @@ -142,6 +143,9 @@ class userParameters {
void setStealthChop(bool sc) { stealthChop = sc; }
bool getStealthChop() { return stealthChop; }

void setFTMSControlPointWrite(bool cpw) { FTMSControlPointWrite = cpw; }
bool getFTMSControlPointWrite() { return FTMSControlPointWrite; }

void setInclineMultiplier(float im) { inclineMultiplier = im; }
float getInclineMultiplier() { return inclineMultiplier; }

Expand Down
86 changes: 45 additions & 41 deletions src/BLE_Client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,13 +29,13 @@ static MyAdvertisedDeviceCallback myAdvertisedDeviceCallbacks;

void SpinBLEClient::start() {
// Create the task for the BLE Client loop
xTaskCreatePinnedToCore(bleClientTask, /* Task function. */
"BLEClientTask", /* name of task. */
BLE_CLIENT_STACK, /* Stack size of task */
NULL, /* parameter of the task */
1, /* priority of the task */
&BLEClientTask, /* Task handle to keep track of created task */
1); /* pin task to core */
xTaskCreatePinnedToCore(bleClientTask, /* Task function. */
"BLEClientTask", /* name of task. */
BLE_CLIENT_STACK, /* Stack size of task */
NULL, /* parameter of the task */
1, /* priority of the task */
&BLEClientTask, /* Task handle to keep track of created task */
1); /* pin task to core */
}

static void onNotify(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t *pData, size_t length, bool isNotify) {
Expand Down Expand Up @@ -541,38 +541,40 @@ void SpinBLEClient::resetDevices(NimBLEClient *pClient) {

// Control a connected FTMS trainer. If no args are passed, treat it like an external stepper motor.
void SpinBLEClient::FTMSControlPointWrite(const uint8_t *pData, int length) {
NimBLEClient *pClient = nullptr;
uint8_t modData[7];
for (int i = 0; i < length; i++) {
modData[i] = pData[i];
}
for (int i = 0; i < NUM_BLE_DEVICES; i++) {
if (myBLEDevices[i].postConnected && (myBLEDevices[i].serviceUUID == FITNESSMACHINESERVICE_UUID)) {
if (NimBLEDevice::getClientByPeerAddress(myBLEDevices[i].peerAddress)->getService(FITNESSMACHINESERVICE_UUID)) {
pClient = NimBLEDevice::getClientByPeerAddress(myBLEDevices[i].peerAddress);
break;
if (userConfig.getFTMSControlPointWrite()) {
NimBLEClient *pClient = nullptr;
uint8_t modData[7];
for (int i = 0; i < length; i++) {
modData[i] = pData[i];
}
for (int i = 0; i < NUM_BLE_DEVICES; i++) {
if (myBLEDevices[i].postConnected && (myBLEDevices[i].serviceUUID == FITNESSMACHINESERVICE_UUID)) {
if (NimBLEDevice::getClientByPeerAddress(myBLEDevices[i].peerAddress)->getService(FITNESSMACHINESERVICE_UUID)) {
pClient = NimBLEDevice::getClientByPeerAddress(myBLEDevices[i].peerAddress);
break;
}
}
}
}
if (pClient) {
NimBLERemoteCharacteristic *writeCharacteristic = pClient->getService(FITNESSMACHINESERVICE_UUID)->getCharacteristic(FITNESSMACHINECONTROLPOINT_UUID);
int logBufLength = 0;
if (writeCharacteristic) {
const int kLogBufCapacity = length + 40;
char logBuf[kLogBufCapacity];
if (modData[0] == FitnessMachineControlPointProcedure::SetIndoorBikeSimulationParameters) { // use virtual Shifting
int incline = ss2k.targetPosition / userConfig.getInclineMultiplier();
modData[3] = (uint8_t)(incline & 0xff);
modData[4] = (uint8_t)(incline >> 8);
writeCharacteristic->writeValue(modData, length);
logBufLength = ss2k_log_hex_to_buffer(modData, length, logBuf, 0, kLogBufCapacity);
logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Shifted Sim Data: %d", rtConfig.getShifterPosition());
} else {
writeCharacteristic->writeValue(modData, length);
logBufLength = ss2k_log_hex_to_buffer(modData, length, logBuf, 0, kLogBufCapacity);
logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Shifted ERG Data: %d", rtConfig.getShifterPosition());
if (pClient) {
NimBLERemoteCharacteristic *writeCharacteristic = pClient->getService(FITNESSMACHINESERVICE_UUID)->getCharacteristic(FITNESSMACHINECONTROLPOINT_UUID);
int logBufLength = 0;
if (writeCharacteristic) {
const int kLogBufCapacity = length + 40;
char logBuf[kLogBufCapacity];
if (modData[0] == FitnessMachineControlPointProcedure::SetIndoorBikeSimulationParameters) { // use virtual Shifting
int incline = ss2k.targetPosition / userConfig.getInclineMultiplier();
modData[3] = (uint8_t)(incline & 0xff);
modData[4] = (uint8_t)(incline >> 8);
writeCharacteristic->writeValue(modData, length);
logBufLength = ss2k_log_hex_to_buffer(modData, length, logBuf, 0, kLogBufCapacity);
logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Shifted Sim Data: %d", rtConfig.getShifterPosition());
} else {
writeCharacteristic->writeValue(modData, length);
logBufLength = ss2k_log_hex_to_buffer(modData, length, logBuf, 0, kLogBufCapacity);
logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Shifted ERG Data: %d", rtConfig.getShifterPosition());
}
SS2K_LOG(BLE_CLIENT_LOG_TAG, "%s", logBuf);
}
SS2K_LOG(BLE_CLIENT_LOG_TAG, "%s", logBuf);
}
}
}
Expand All @@ -598,17 +600,19 @@ void SpinBLEClient::postConnect() {
rtConfig.setMaxResistance(MAX_ECHELON_RESISTANCE);
}

if (this->myBLEDevices[i].charUUID == FITNESSMACHINEINDOORBIKEDATA_UUID) {
if ((this->myBLEDevices[i].charUUID == FITNESSMACHINEINDOORBIKEDATA_UUID)) {
NimBLERemoteCharacteristic *writeCharacteristic = pClient->getService(FITNESSMACHINESERVICE_UUID)->getCharacteristic(FITNESSMACHINECONTROLPOINT_UUID);
if (writeCharacteristic == nullptr) {
SS2K_LOG(BLE_CLIENT_LOG_TAG, "Failed to find FTMS control characteristic UUID: %s", FITNESSMACHINECONTROLPOINT_UUID.toString().c_str());
return;
}
// Start Training
writeCharacteristic->writeValue(FitnessMachineControlPointProcedure::RequestControl, 1);
vTaskDelay(BLE_NOTIFY_DELAY / portTICK_PERIOD_MS);
writeCharacteristic->writeValue(FitnessMachineControlPointProcedure::StartOrResume, 1);
SS2K_LOG(BLE_CLIENT_LOG_TAG, "Activated FTMS Training.");
if (userConfig.getFTMSControlPointWrite()) {
writeCharacteristic->writeValue(FitnessMachineControlPointProcedure::RequestControl, 1);
vTaskDelay(BLE_NOTIFY_DELAY / portTICK_PERIOD_MS);
writeCharacteristic->writeValue(FitnessMachineControlPointProcedure::StartOrResume, 1);
SS2K_LOG(BLE_CLIENT_LOG_TAG, "Activated FTMS Training.");
}
BLEDevice::getServer()->updateConnParams(pClient->getConnId(), 120, 120, 2, 1000);
}
}
Expand Down

0 comments on commit 98923d3

Please sign in to comment.