FTMS Passthrough

CHANGELOG.md

@@ -12,6 +12,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Added power scaler for new board.
 - Added Main Index link to develop.html.
 - Added feature to automatically reconnect BLE devices if both are specified.
+- Added ftms passthrough. FTMS messages from the client app are now passed to a connected FTMS device.
 
 ### Changed
 - PowerTable values are now adjusted to 90 RPM cad on input.
@@ -21,6 +22,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Moved serial checking to own function. 
 - Reduced verbosity of ERG logging.
 - Fixed instance of BLE PM dropdown not being saved correctly. 
+- Moved post connect handling to the ble communication loop. (improves startup stability)
 
 ### Hardware
 - removed duplicate directory in direct mount folder

include/BLE_Common.h

@@ -45,7 +45,7 @@
 #define BLE_connectedPowerMeter   0x15
 #define BLE_connectedHeartMonitor 0x16
 #define BLE_shifterPosition       0x17
-#define BLE_saveToLittleFS          0x18
+#define BLE_saveToLittleFS        0x18
 #define BLE_targetPosition        0x19
 #define BLE_externalControl       0x1A
 #define BLE_syncMode              0x1B
@@ -158,6 +158,7 @@ class SpinBLEAdvertisedDevice {
   bool userSelectedCSC  = false;
   bool userSelectedCT   = false;
   bool doConnect        = false;
+  bool postConnected    = false;
 
   void set(BLEAdvertisedDevice *device, int id = BLE_HS_CONN_HANDLE_NONE, BLEUUID inserviceUUID = (uint16_t)0x0000, BLEUUID incharUUID = (uint16_t)0x0000) {
     advertisedDevice  = device;
@@ -179,6 +180,7 @@ class SpinBLEAdvertisedDevice {
     userSelectedCSC   = false;  // Cycling Speed/Cadence
     userSelectedCT    = false;  // Controllable Trainer
     doConnect         = false;  // Initiate connection flag
+    postConnected     = false;  // Has Cost Connect Been Run?
     if (dataBufferQueue != nullptr) {
       Serial.println("Resetting queue");
       xQueueReset(dataBufferQueue);
@@ -220,7 +222,8 @@ class SpinBLEClient {
   // Reset devices in myBLEDevices[]. Bool All (true) or only connected ones
   // (false)
   void resetDevices();
-  void postConnect(NimBLEClient *pClient);
+  void postConnect();
+  void FTMSControlPointWrite(const uint8_t *pData, int length);
 };
 class MyAdvertisedDeviceCallback : public NimBLEAdvertisedDeviceCallbacks {
  public:

src/BLE_Client.cpp

@@ -111,14 +111,14 @@ bool SpinBLEClient::connectToServer() {
       serviceUUID = FLYWHEEL_UART_SERVICE_UUID;
       charUUID    = FLYWHEEL_UART_TX_UUID;
       SS2K_LOG(BLE_CLIENT_LOG_TAG, "trying to connect to Flywheel Bike");
-    } else if (myDevice->isAdvertisingService(CYCLINGPOWERSERVICE_UUID)) {
-      serviceUUID = CYCLINGPOWERSERVICE_UUID;
-      charUUID    = CYCLINGPOWERMEASUREMENT_UUID;
-      SS2K_LOG(BLE_CLIENT_LOG_TAG, "trying to connect to PM");
     } else if (myDevice->isAdvertisingService(FITNESSMACHINESERVICE_UUID)) {
       serviceUUID = FITNESSMACHINESERVICE_UUID;
       charUUID    = FITNESSMACHINEINDOORBIKEDATA_UUID;
       SS2K_LOG(BLE_CLIENT_LOG_TAG, "trying to connect to Fitness machine service");
+    } else if (myDevice->isAdvertisingService(CYCLINGPOWERSERVICE_UUID)) {
+      serviceUUID = CYCLINGPOWERSERVICE_UUID;
+      charUUID    = CYCLINGPOWERMEASUREMENT_UUID;
+      SS2K_LOG(BLE_CLIENT_LOG_TAG, "trying to connect to PM");
     } else if (myDevice->isAdvertisingService(ECHELON_DEVICE_UUID)) {
       serviceUUID = ECHELON_SERVICE_UUID;
       charUUID    = ECHELON_DATA_UUID;
@@ -230,10 +230,6 @@ bool SpinBLEClient::connectToServer() {
         SS2K_LOG(BLE_CLIENT_LOG_TAG, "The characteristic value was: %s", value.c_str());
       }
 
-      /** registerForNotify() has been deprecated and replaced with subscribe() / unsubscribe().
-       *  Subscribe parameter defaults are: notifications=true, notifyCallback=nullptr, response=false.
-       *  Unsubscribe parameter defaults are: response=false.
-       */
       if (pChr->canNotify()) {
         // if(!pChr->registerForNotify(notifyCB)) {
         if (!pChr->subscribe(true, onNotify)) {
@@ -256,9 +252,8 @@ bool SpinBLEClient::connectToServer() {
       spinBLEClient.myBLEDevices[device_number].doConnect = false;
       this->reconnectTries                                = MAX_RECONNECT_TRIES;
       spinBLEClient.myBLEDevices[device_number].set(myDevice, pClient->getConnId(), serviceUUID, charUUID);
-      // vTaskDelay(100 / portTICK_PERIOD_MS); //Give time for connection to finalize.
+
       removeDuplicates(pClient);
-      postConnect(pClient);
     }
 
   } else {
@@ -390,8 +385,8 @@ void SpinBLEClient::scanProcess(int duration) {
 
   BLEScan *pBLEScan = BLEDevice::getScan();
   pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallback());
-  pBLEScan->setInterval(97);  
-  pBLEScan->setWindow(67);   
+  pBLEScan->setInterval(97);
+  pBLEScan->setWindow(67);
   pBLEScan->setDuplicateFilter(true);
   pBLEScan->setActiveScan(true);
   BLEScanResults foundDevices = pBLEScan->start(duration, true);
@@ -487,36 +482,67 @@ void SpinBLEClient::resetDevices() {
   }
 }
 
-void SpinBLEClient::postConnect(NimBLEClient *pClient) {
-  for (size_t i = 0; i < NUM_BLE_DEVICES; i++) {
-    if (pClient->getPeerAddress() == this->myBLEDevices[i].peerAddress) {
-      if ((this->myBLEDevices[i].charUUID == CYCLINGPOWERMEASUREMENT_UUID) || (this->myBLEDevices[i].charUUID == FITNESSMACHINEINDOORBIKEDATA_UUID) ||
-          (this->myBLEDevices[i].charUUID == FLYWHEEL_UART_RX_UUID) || (this->myBLEDevices[i].charUUID == ECHELON_DATA_UUID)) {
-        this->connectedPM = true;
-        SS2K_LOG(BLE_CLIENT_LOG_TAG, "Registered PM on Connect");
-        if (this->myBLEDevices[i].charUUID == ECHELON_DATA_UUID) {
-          NimBLERemoteCharacteristic *writeCharacteristic = pClient->getService(ECHELON_SERVICE_UUID)->getCharacteristic(ECHELON_WRITE_UUID);
-          if (writeCharacteristic == nullptr) {
-            SS2K_LOG(BLE_CLIENT_LOG_TAG, "Failed to find Echelon write characteristic UUID: %s", ECHELON_WRITE_UUID.toString().c_str());
-            pClient->disconnect();
-            return;
+void SpinBLEClient::FTMSControlPointWrite(const uint8_t *pData, int length) {
+  NimBLEClient *pClient = nullptr;
+  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);
+    if (writeCharacteristic) {
+      writeCharacteristic->writeValue(pData, length);
+      SS2K_LOG(BLE_CLIENT_LOG_TAG, "Sent FTMS");
+    }
+  }
+}
+
+void SpinBLEClient::postConnect() {
+  for (int i = 0; i < NUM_BLE_DEVICES; i++) {
+    if (!myBLEDevices[i].postConnected) {
+      if (NimBLEDevice::getClientByPeerAddress(myBLEDevices[i].peerAddress)) {
+        myBLEDevices[i].postConnected = true;
+        NimBLEClient *pClient         = NimBLEDevice::getClientByPeerAddress(myBLEDevices[i].peerAddress);
+        if ((this->myBLEDevices[i].charUUID == CYCLINGPOWERMEASUREMENT_UUID) || (this->myBLEDevices[i].charUUID == FITNESSMACHINEINDOORBIKEDATA_UUID) ||
+            (this->myBLEDevices[i].charUUID == FLYWHEEL_UART_RX_UUID) || (this->myBLEDevices[i].charUUID == ECHELON_DATA_UUID)) {
+          this->connectedPM = true;
+          SS2K_LOG(BLE_CLIENT_LOG_TAG, "Registered PM on Connect");
+
+          if (this->myBLEDevices[i].charUUID == ECHELON_DATA_UUID) {
+            NimBLERemoteCharacteristic *writeCharacteristic = pClient->getService(ECHELON_SERVICE_UUID)->getCharacteristic(ECHELON_WRITE_UUID);
+            if (writeCharacteristic == nullptr) {
+              SS2K_LOG(BLE_CLIENT_LOG_TAG, "Failed to find Echelon write characteristic UUID: %s", ECHELON_WRITE_UUID.toString().c_str());
+              pClient->disconnect();
+              return;
+            }
+            // Enable device notifications
+            byte message[] = {0xF0, 0xB0, 0x01, 0x01, 0xA2};
+            writeCharacteristic->writeValue(message, 5);
+            SS2K_LOG(BLE_CLIENT_LOG_TAG, "Activated Echelon callbacks.");
+          }
+
+          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.");
           }
-          // Enable device notifications
-          byte message[] = {0xF0, 0xB0, 0x01, 0x01, 0xA2};
-          writeCharacteristic->writeValue(message, 5);
-          SS2K_LOG(BLE_CLIENT_LOG_TAG, "Activated Echelon callbacks.");
         }
-        // spinBLEClient.removeDuplicates(pclient);
-        BLEDevice::getServer()->updateConnParams(pClient->getConnId(), 400, 400, 2, 1000);
-        return;
-      }
-      if ((this->myBLEDevices[i].charUUID == HEARTCHARACTERISTIC_UUID)) {
-        this->connectedHR = true;
-        SS2K_LOG(BLE_CLIENT_LOG_TAG, "Registered HRM on Connect");
+        if ((this->myBLEDevices[i].charUUID == HEARTCHARACTERISTIC_UUID)) {
+          this->connectedHR = true;
+          SS2K_LOG(BLE_CLIENT_LOG_TAG, "Registered HRM on Connect");
+        }
         BLEDevice::getServer()->updateConnParams(pClient->getConnId(), 400, 400, 2, 1000);
-        return;
-      } else {
-        SS2K_LOG(BLE_CLIENT_LOG_TAG, "These did not match|%s|%s|", pClient->getPeerAddress().toString().c_str(), this->myBLEDevices[i].peerAddress.toString().c_str());
       }
     }
   }

src/BLE_Common.cpp

@@ -103,7 +103,7 @@ void BLECommunications(void *pvParameters) {
       }
 
       processFTMSWrite();
-      // computeERG();
+      spinBLEClient.postConnect();
 
       if (BLEDevice::getAdvertising()) {
         if (!(BLEDevice::getAdvertising()->isAdvertising()) && (BLEDevice::getServer()->getConnectedCount() < CONFIG_BT_NIMBLE_MAX_CONNECTIONS - NUM_BLE_DEVICES)) {

src/BLE_Server.cpp

@@ -434,6 +434,10 @@ void processFTMSWrite() {
           ftmsStatus            = {FitnessMachineStatus::TargetPowerChanged, (uint8_t)rxValue[1], (uint8_t)rxValue[2]};
           ftmsTrainingStatus[1] = FitnessMachineTrainingStatus::Other;  // 0x0C;
           fitnessMachineTrainingStatus->setValue(ftmsTrainingStatus, 2);
+          // Adjust set point for powerCorrectionFactor and send to FTMS server (if connected)
+          int adjustedTarget         = targetWatts / userConfig.getPowerCorrectionFactor();
+          const uint8_t translated[] = {FitnessMachineControlPointProcedure::SetTargetPower, (uint8_t)(adjustedTarget % 256), (uint8_t)(adjustedTarget / 256)};
+          spinBLEClient.FTMSControlPointWrite(translated, 3);
         } else {
           returnValue[2] = FitnessMachineControlPointResultCode::OpCodeNotSupported;  // 0x02; no power meter connected, so no ERG
           logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> ERG Mode: No Power Meter Connected");
@@ -465,7 +469,8 @@ void processFTMSWrite() {
       } break;
 
       case FitnessMachineControlPointProcedure::SetIndoorBikeSimulationParameters: {  // sim mode
-        returnValue[2] = FitnessMachineControlPointResultCode::Success;               // 0x01;
+        spinBLEClient.FTMSControlPointWrite(pData, length);
+        returnValue[2] = FitnessMachineControlPointResultCode::Success;  // 0x01;
         pCharacteristic->setValue(returnValue, 3);
 
         signed char buf[2];