Merge pull request #698 from doudar/Resistance_level_fix

Resistance level fix

Author: doudar

CHANGELOG.md

@@ -14,6 +14,19 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Hardware
 
 
+## [25.11.4]
+
+### Added
+
+### Changed
+- Added support for reading resistance range from connected FTMS devices
+- Improved resistance mode control logic for bikes with and without native resistance reporting
+- Fixed resistance value parsing to correctly handle 16-bit values
+- Reduced default max brake watts from 1400w to 1000w. 
+
+### Hardware
+
+
 ## [25.10.19]
 
 ### Added

include/SmartSpin_parameters.h

@@ -22,6 +22,8 @@ class Measurement {
   bool simulate;
   int value;
   int target;
+  int min;
+  int max;
   unsigned long timestamp;
 
  public:
@@ -41,6 +43,13 @@ class Measurement {
     target          = tar;
     this->timestamp = millis();
   }
+
+  void setMin(int min) { this->min = min; }
+  int getMin() { return min; }
+
+  void setMax(int max) { this->max = max; }
+  int getMax() { return max; }
+
   int getTarget() { return target; }
 
   long getTimestamp() { return timestamp; }
@@ -49,6 +58,8 @@ class Measurement {
     this->simulate  = false;
     this->value     = 0;
     this->target    = 0;
+    this->min       = 0;
+    this->max       = 0;
     this->timestamp = millis();
   }
 };

include/settings.h

@@ -95,7 +95,7 @@ const char* const DEFAULT_PASSWORD = "password";
 
 // Default Max Watts that the brake on the spin bike can absorb from the user.
 // This is used to set the upper travel limit for the motor.
-#define DEFAULT_MAX_WATTS 1400
+#define DEFAULT_MAX_WATTS 1000
 
 // Minimum resistance on a Peloton Bike.
 // This is used to set the lower travel limit for the motor.

src/BLE_Client.cpp

@@ -401,7 +401,7 @@ void ScanCallbacks::onResult(const NimBLEAdvertisedDevice* advertisedDevice) {
     if (serviceInfo) {
       SS2K_LOG(BLE_CLIENT_LOG_TAG, "Supported Device: %s with service %s", aDevName.c_str(), serviceInfo->name.c_str());
       const NimBLEUUID& primaryServiceUUID = serviceInfo->serviceUUID;
-      //check to see if we're already connected to this device
+      // check to see if we're already connected to this device
       for (size_t i = 0; i < NUM_BLE_DEVICES; i++) {
         if (spinBLEClient.myBLEDevices[i].advertisedDevice != nullptr) {
           if (aDevName == String(spinBLEClient.myBLEDevices[i].uniqueName.c_str())) {
@@ -671,50 +671,80 @@ void SpinBLEClient::postConnect() {
           // Enable device notifications
           byte message[] = {0xF0, 0xB0, 0x01, 0x01, 0xA2};
           writeCharacteristic->writeValue(message, 5);
-          SS2K_LOG(BLE_CLIENT_LOG_TAG, "Activated Echelon callbacks.");
+          SS2K_LOG(BLE_CLIENT_LOG_TAG, "Activated Echelon callbacks on device: %s", _BLEd.uniqueName.c_str());
           rtConfig->setMinResistance(MIN_ECHELON_RESISTANCE);
           rtConfig->setMaxResistance(MAX_ECHELON_RESISTANCE);
         }
 
-        if ((_BLEd.charUUID == FITNESSMACHINEINDOORBIKEDATA_UUID)) {
-          SS2K_LOG(BLE_CLIENT_LOG_TAG, "Updating Connection Params for: %s", _BLEd.peerAddress.toString().c_str());
-          spinBLEClient.handleBattInfo(pClient, true);
+        if (pClient->getService(FITNESSMACHINESERVICE_UUID)) {
+          SS2K_LOG(BLE_CLIENT_LOG_TAG, "Initializing FTMS on device: %s", _BLEd.uniqueName.c_str());
 
           auto featuresCharacteristic = pClient->getService(FITNESSMACHINESERVICE_UUID)->getCharacteristic(FITNESSMACHINEFEATURE_UUID);
           if (featuresCharacteristic == nullptr) {
             SS2K_LOG(BLE_CLIENT_LOG_TAG, "Failed to find FTMS features characteristic UUID: %s", FITNESSMACHINEFEATURE_UUID.toString().c_str());
-            return;
-          }
-
-          if (featuresCharacteristic->canRead()) {
-            auto value = featuresCharacteristic->readValue();
-            if (value.size() < sizeof(uint64_t)) {
-              SS2K_LOG(BLE_CLIENT_LOG_TAG, "Failed to read FTMS features characteristic");
-              return;
-            }
-
-            // We're only interested in the machine fitness features, not the target setting features.
-            auto features = *reinterpret_cast<const uint32_t*>(value.data());
-            if (!(features & FitnessMachineFeatureFlags::Types::ElapsedTimeSupported) || !(features & FitnessMachineFeatureFlags::Types::RemainingTimeSupported)) {
-              SS2K_LOG(BLE_CLIENT_LOG_TAG, "FTMS Control Point StartOrResume not supported");
-              return;
+          } else {
+            if (featuresCharacteristic->canRead()) {
+              auto value = featuresCharacteristic->readValue();
+              if (value.size() < sizeof(uint64_t)) {
+                SS2K_LOG(BLE_CLIENT_LOG_TAG, "Failed to read FTMS features characteristic for: %s", _BLEd.uniqueName.c_str());
+              } else {
+                // We're only interested in the machine fitness features, not the target setting features.
+                auto features = *reinterpret_cast<const uint32_t*>(value.data());
+                if (!(features & FitnessMachineFeatureFlags::Types::ElapsedTimeSupported) || !(features & FitnessMachineFeatureFlags::Types::RemainingTimeSupported)) {
+                  SS2K_LOG(BLE_CLIENT_LOG_TAG, "FTMS Control Point StartOrResume not supported on: %s", _BLEd.uniqueName.c_str());
+                }
+
+                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, on %s", FITNESSMACHINECONTROLPOINT_UUID.toString().c_str(),
+                           _BLEd.uniqueName.c_str());
+                } else {
+                  // If we would like to control an external FTMS trainer. With most spin bikes we would want this off, but it's useful if you want to use the SmartSpin2k as an
+                  // appliance.
+                  if (userConfig->getFTMSControlPointWrite()) {
+                    writeCharacteristic->writeValue(FitnessMachineControlPointProcedure::RequestControl, 1);
+                    delay(BLE_NOTIFY_DELAY);
+                    SS2K_LOG(BLE_CLIENT_LOG_TAG, "Activated FTMS Training on device: %s", _BLEd.uniqueName.c_str());
+                  }
+                  writeCharacteristic->writeValue(FitnessMachineControlPointProcedure::StartOrResume, 1);
+                }
+              }
             }
           }
+          // update resistance range if supported:
+          auto resistanceRangeCharacteristic = pClient->getService(FITNESSMACHINESERVICE_UUID)->getCharacteristic(FITNESSMACHINERESISTANCELEVELRANGE_UUID);
+          if (resistanceRangeCharacteristic && resistanceRangeCharacteristic->canRead()) {
+            auto rr = resistanceRangeCharacteristic->readValue();
+            if (rr.size() >= 6) {
+              const uint8_t* b = reinterpret_cast<const uint8_t*>(rr.data());
+              int16_t minRaw   = static_cast<int16_t>(b[0] | (static_cast<uint16_t>(b[1]) << 8));
+              int16_t maxRaw   = static_cast<int16_t>(b[2] | (static_cast<uint16_t>(b[3]) << 8));
+              uint16_t incRaw  = static_cast<uint16_t>(b[4] | (static_cast<uint16_t>(b[5]) << 8));
+
+              float incF = static_cast<float>(incRaw) / 10.0f;  // FTMS resolution 0.1, convert to actual increment value
+              float minF = (static_cast<float>(minRaw) / 10.0f)/incF;  // Convert FTMS 0.1 units to normalized scale
+              float maxF = (static_cast<float>(maxRaw) / 10.0f)/incF;  // Convert FTMS 0.1 units to normalized scale
+
+              // Internal resistance is integer-based; round to nearest
+              int minRes = static_cast<int>(minF >= 0.0f ? (minF + 0.5f) : (minF - 0.5f));
+              int maxRes = static_cast<int>(maxF >= 0.0f ? (maxF + 0.5f) : (maxF - 0.5f));
+
+              if (minRes > maxRes) {
+                // Defensive: swap if device reports reversed
+                int tmp = minRes;
+                minRes  = maxRes;
+                maxRes  = tmp;
+              }
 
-          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;
-          }
-
-          // If we would like to control an external FTMS trainer. With most spin bikes we would want this off, but it's useful if you want to use the SmartSpin2k as an
-          // appliance.
-          if (userConfig->getFTMSControlPointWrite()) {
-            writeCharacteristic->writeValue(FitnessMachineControlPointProcedure::RequestControl, 1);
-            delay(BLE_NOTIFY_DELAY);
-            SS2K_LOG(BLE_CLIENT_LOG_TAG, "Activated FTMS Training.");
+              rtConfig->resistance.setMin(minRes);
+              rtConfig->resistance.setMax(maxRes);
+              SS2K_LOG(BLE_CLIENT_LOG_TAG, "FTMS Resistance Range: raw min=%.1f raw max=%.1f inc=%.1f -> set %d->%d", minF, maxF, incF, minRes, maxRes);
+            } else {
+              SS2K_LOG(BLE_CLIENT_LOG_TAG, "FTMS Resistance Range characteristic too short (%d bytes)", rr.size());
+            }
+          } else {
+            SS2K_LOG(BLE_CLIENT_LOG_TAG, "FTMS Resistance Range characteristic unavailable or unreadable");
           }
-          writeCharacteristic->writeValue(FitnessMachineControlPointProcedure::StartOrResume, 1);
         }
       }
     }

src/BLE_Fitness_Machine_Service.cpp

@@ -23,7 +23,7 @@ BLE_Fitness_Machine_Service::BLE_Fitness_Machine_Service()
 
 void BLE_Fitness_Machine_Service::setupService(NimBLEServer *pServer, MyCharacteristicCallbacks *chrCallbacks) {
   // Resistance, IPower, HeartRate
-  uint8_t ftmsResistanceLevelRange[6] = {0x01, 0x00, 0x64, 0x00, 0x01, 0x00};  // 1:100 increment 1
+  uint8_t ftmsResistanceLevelRange[6] = {0x01, 0x00, 0x64, 0x00, 0x0A, 0x00};  // .1:10 increment .1
   uint8_t ftmsPowerRange[6]           = {0x01, 0x00, 0xA0, 0x0F, 0x01, 0x00};  // 1:4000 watts increment 1
   uint8_t ftmsInclinationRange[6]     = {0x38, 0xff, 0xc8, 0x00, 0x01, 0x00};  // -20.0:20.0 increment .1
   // Fitness Machine Feature Flags Setup
@@ -99,7 +99,7 @@ void BLE_Fitness_Machine_Service::update() {
   // Add resistance
   int resistanceValue;
   // Check if bike has resistance reporting capability or resistance simulation enabled
-  bool hasResistanceReporting = (rtConfig->resistance.getSimulate() || 
+  bool hasResistanceReporting = (!rtConfig->resistance.getSimulate() && 
                                 (rtConfig->resistance.getTimestamp() > 0 && 
                                  (millis() - rtConfig->resistance.getTimestamp()) < 5000));
 
@@ -109,6 +109,8 @@ void BLE_Fitness_Machine_Service::update() {
   } else {
     // Calculate resistance from stepper position for bikes that don't report resistance
     resistanceValue = this->calculateResistanceFromPosition();
+    rtConfig->resistance.setValue(resistanceValue);
+    rtConfig->resistance.setSimulate(true); // Mark as simulated
   }
   ftmsIndoorBikeData.push_back(static_cast<uint8_t>(resistanceValue & 0xff));
   ftmsIndoorBikeData.push_back(static_cast<uint8_t>(resistanceValue >> 8));
@@ -190,13 +192,13 @@ void BLE_Fitness_Machine_Service::processFTMSWrite() {
 
         case FitnessMachineControlPointProcedure::SetTargetResistanceLevel: {
           rtConfig->setFTMSMode((uint8_t)rxValue[0]);
-          int16_t requestedResistance = (int16_t)rxValue[1];
+          int16_t requestedResistance = (int16_t)((rxValue[2] << 8) | rxValue[1]);
 
           if (requestedResistance >= rtConfig->getMinResistance() && requestedResistance <= rtConfig->getMaxResistance()) {
             rtConfig->resistance.setTarget(requestedResistance);
 
             // For bikes that don't report resistance, calculate stepper position from resistance level (0-100)
-            bool hasResistanceReporting = (rtConfig->resistance.getSimulate() || 
+            bool hasResistanceReporting = (!rtConfig->resistance.getSimulate() && 
                                           (rtConfig->resistance.getTimestamp() > 0 && 
                                            (millis() - rtConfig->resistance.getTimestamp()) < 5000));
 

src/Main.cpp

@@ -34,19 +34,19 @@ HardwareSerial auxSerial(1);
 AuxSerialBuffer auxSerialBuffer;
 
 FastAccelStepperEngine engine = FastAccelStepperEngine();
-FastAccelStepper *stepper     = NULL;
+FastAccelStepper* stepper     = NULL;
 
 TaskHandle_t maintenanceLoopTask;
 
 Boards boards;
 Board currentBoard;
 
 ///////////// Initialize the Config /////////////
-ErgMode *ergMode            = new ErgMode;
-PowerTable *powerTable      = new PowerTable;
-SS2K *ss2k                  = new SS2K;
-userParameters *userConfig  = new userParameters;
-RuntimeParameters *rtConfig = new RuntimeParameters;
+ErgMode* ergMode            = new ErgMode;
+PowerTable* powerTable      = new PowerTable;
+SS2K* ss2k                  = new SS2K;
+userParameters* userConfig  = new userParameters;
+RuntimeParameters* rtConfig = new RuntimeParameters;
 
 ///////////// Log Appender /////////////
 UdpAppender udpAppender;
@@ -178,12 +178,12 @@ void loop() {  // Delete this task so we can make one that's more memory efficie
   vTaskDelete(NULL);
 }
 
-void SS2K::maintenanceLoop(void *pvParameters) {
+void SS2K::maintenanceLoop(void* pvParameters) {
   static unsigned long intervalTimer2 = millis();
   static unsigned long rebootTimer    = millis();
 
   while (true) {
-    delay(5);
+    delay(10);
 
     // be quiet while updating via BLE
     if (!ss2k->isUpdating) {
@@ -321,8 +321,9 @@ void SS2K::maintenanceLoop(void *pvParameters) {
       }
 #endif  // DEBUG_STACK
       // Log userParameters
-      SS2K_LOG(MAIN_LOG_TAG, "PM Con %d, CAD con %d, HRM Con %d, W %d, Cad %d, HR %d, Gear %d, Target Position %d", spinBLEClient.connectedPM, spinBLEClient.connectedCD,
-               spinBLEClient.connectedHRM, rtConfig->watts.getValue(), rtConfig->cad.getValue(), rtConfig->hr.getValue(), rtConfig->getShifterPosition(), ss2k->targetPosition);
+      SS2K_LOG(MAIN_LOG_TAG, "PM Con %d, CAD con %d, HRM Con %d, W %d, Cad %d, HR %d, Gear %d, Res %d, Target Position %d", spinBLEClient.connectedPM, spinBLEClient.connectedCD,
+               spinBLEClient.connectedHRM, rtConfig->watts.getValue(), rtConfig->cad.getValue(), rtConfig->hr.getValue(), rtConfig->getShifterPosition(),
+               rtConfig->resistance.getValue(), ss2k->targetPosition);
 
       intervalTimer2 = millis();
     }
@@ -433,9 +434,44 @@ void SS2K::moveStepper() {
         }
 #endif
         ss2k->targetPosition = rtConfig->getTargetIncline();
-      } else if (rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetResistanceLevel) {
-        int actualDelta = rtConfig->resistance.getTarget() - rtConfig->resistance.getValue();
-        rtConfig->setTargetIncline(ss2k->getCurrentPosition() + ((userConfig->getERGSensitivity() * 3) * actualDelta));
+      } else if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetResistanceLevel)) {
+        // Get absolute position for a given resistance percent (0-100)
+        if (rtConfig->resistance.getSimulate()) {
+          int32_t minPos, maxPos;
+          bool usePwr = false;
+          if (userConfig->getHMin() != INT32_MIN && userConfig->getHMax() != INT32_MIN) {
+            minPos = userConfig->getHMin();
+            maxPos = userConfig->getHMax();
+          } else if(rtConfig->getMinStep() != -DEFAULT_STEPPER_TRAVEL && rtConfig->getMaxStep() != DEFAULT_STEPPER_TRAVEL) {
+            minPos = rtConfig->getMinStep();
+            maxPos = rtConfig->getMaxStep();
+          } else{ //No good position information. Fallback to using ERG
+            minPos = userConfig->getMinWatts();
+            maxPos = userConfig->getMaxWatts();
+            usePwr = true;
+          }
+          int resistancePercent = rtConfig->resistance.getTarget();
+          if (resistancePercent < 0) resistancePercent = 0;
+          if (resistancePercent > 100) resistancePercent = 100;
+          int64_t span = (int64_t)maxPos - (int64_t)minPos;
+          int32_t pos = minPos + (int32_t)((span * resistancePercent) / 100);
+          if (usePwr) { //fallback to using ERG
+            rtConfig->watts.setTarget(pos); 
+            rtConfig->setFTMSMode(FitnessMachineControlPointProcedure::SetTargetPower);
+            return;
+          }
+          rtConfig->setTargetIncline(pos);
+        } else {
+          int actualDelta = rtConfig->resistance.getTarget() - rtConfig->resistance.getValue();
+          int direction   = (actualDelta > 0) ? 1 : -1;
+          if (abs(actualDelta) > 20 - userConfig->getERGSensitivity()) {
+            rtConfig->setTargetIncline(ss2k->getCurrentPosition() + userConfig->getShiftStep() * direction);
+          } else if (abs(actualDelta) > 3) {
+            rtConfig->setTargetIncline(ss2k->getCurrentPosition() + actualDelta + (userConfig->getERGSensitivity() * direction));
+          } else {
+            rtConfig->setTargetIncline(ss2k->getCurrentPosition() + actualDelta);
+          }
+        }
         ss2k->targetPosition = rtConfig->getTargetIncline();
       } else {
         // Simulation Mode
@@ -777,7 +813,7 @@ void SS2K::txSerial() {  // Serial.printf(" Before TX ");
 }
 bool SS2K::pelotonConnected() {
   txCheck = TX_CHECK_INTERVAL;
-  if (rtConfig->resistance.getValue() > 0) {
+  if (millis() - rtConfig->resistance.getTimestamp() < 5000 && !rtConfig->resistance.getSimulate()) {
     rtConfig->setMinResistance(MIN_PELOTON_RESISTANCE);
     rtConfig->setMaxResistance(MAX_PELOTON_RESISTANCE);
     return true;

src/Power_Table.cpp

@@ -95,7 +95,7 @@ void PowerTable::setStepperMinMax() {
   }
 
   // if the FTMS device reports resistance feedback, skip estimating min_max
-  if (rtConfig->resistance.getValue() > 0) {
+  if (rtConfig->resistance.getValue() > 0 && !rtConfig->resistance.getSimulate()) {
     rtConfig->setMinStep(-DEFAULT_STEPPER_TRAVEL);
     rtConfig->setMaxStep(DEFAULT_STEPPER_TRAVEL);
     SS2K_LOG(POWERTABLE_LOG_TAG, "Using Resistance Travel Limits");

src/SensorCollector.cpp

@@ -84,6 +84,7 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni
   }
 
   if (sensorData->hasResistance()) {
+    rtConfig->resistance.setSimulate(false); // Mark as real data
     if ((ss2k->pelotonIsConnected) && (charUUID != PELOTON_DATA_UUID)) {
       // Peloton connected but using BLE Power Meter. So skip resistance for UUID's that aren't Peloton.
     } else {