Marc roy

CHANGELOG.md

@@ -13,6 +13,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Added feed forward, disabled PowerTable for ERG lookup. 
 - Added tests and removal of duplicates in pt column. 
 - Added removal of negative numbers in pt table. 
+- Refined ERG mode (stateful increase/decrease handling, smarter wait timers, improved PID logging).
+- Adjusted FTMS resistance handling: ignore malformed IC Bike ranges, log raw range data, and skip IC Bike resistance samples.
+- Rounded cadence/power calculations across CSC, CyclePower, Peloton, FTMS decoding; clamp invalid cadence values.
+- Applied rounding for FTMS shift targets/resistance mapping and homing thresholds; use fabs in resistance model and paused duplicate cleanup.
 
 ### Hardware
 

include/BLE_Custom_Characteristic.h

@@ -62,6 +62,7 @@ const uint8_t BLE_hMin                  = 0x2A;  // Minimum homing value
 const uint8_t BLE_hMax                  = 0x2B;  // Maximum homing value
 const uint8_t BLE_homingSensitivity     = 0x2C;  // Homing sensitivity value
 const uint8_t BLE_pTab4Pwr              = 0x2D;  // Use power values for power table
+const uint8_t BLE_UDPLogging            = 0x2E;  // Enable or disable UDP logging
 
 class BLE_ss2kCustomCharacteristic {
  public:

include/ERG_Mode.h

@@ -14,34 +14,37 @@
 #define ERG_MODE_LOG_TAG     "ERG_Mode"
 #define ERG_MODE_DELAY       700
 
+struct Mode {
+  static const int MAINTAIN   = 0;
+  static const int DECREASING = 1;
+  static const int INCREASING = 2;
+};
+
 class ErgMode {
  public:
-   // What used to be in the ERGTaskLoop(). This is the main control function for ERG Mode and the powertable operations.
+  // What used to be in the ERGTaskLoop(). This is the main control function for ERG Mode and the powertable operations.
   void runERG();
   void computeErg();
   void _writeLog(float currentIncline, float newIncline, int currentSetPoint, int newSetPoint, int currentWatts, int newWatts, int currentCadence, int newCadence);
 
  private:
-  bool engineStopped   = false;
-  bool initialized     = false;
-  int setPoint         = 0;
-  int offsetMultiplier = 0;
-  int resistance       = 0;
-  int cadence          = 0;
+  bool engineStopped = false;
 
-  Measurement watts;
+  int mode = Mode::MAINTAIN;
+  Measurement prevWatts;
+  Measurement prevCadence;
 
   // check if user is spinning, reset incline if user stops spinning
   bool _userIsSpinning(int cadence, float incline);
 
   // calculate incline if setpoint (from Zwift) changes
-  int32_t _setPointChangeState(int newCadence, Measurement& newWatts);
+  int32_t _setPointChangeState();
 
   // calculate incline if setpoint is unchanged
-  int32_t _inSetpointState(int newCadence, Measurement& newWatts);
+  int32_t _inSetpointState();
 
   // update localvalues + incline, creates a log
-  void _updateValues(int newCadence, Measurement& newWatts, float newIncline);
+  void _updateValues(float newIncline);
 };
 
 extern ErgMode* ergMode;

include/PowerTable_Helpers.h

@@ -8,13 +8,10 @@
 #pragma once
 
 #include "SmartSpin_parameters.h"
-#include <vector>
 
 #define PTDATA_LOG_TAG "PTData"
 
 #define RETURN_ERROR               INT32_MIN
-#define FREE_HEAP_FOR_COMPLEX_MATH 30000
-#define COMPUTATION_TIMEOUT_MS 25
 
 class PowerEntry {
  public:
@@ -76,22 +73,41 @@ class PTData {
   TableRow tableRow[POWERTABLE_CAD_SIZE];
 };
 
+class ResistanceModel {
+ private:
+  // Coefficients (Normalizing makes these fit in float/double safely)
+  // Model: Z = b0 + b1*x + b2*y + b3*x^2 + b4*y^2 + b5*x*y
+  double b[6]      = {0};
+  bool isQuadratic = false;
+  bool isValid     = false;
+
+  // Normalization bounds (to keep math stable)
+  double minW = 0, maxW = 1;
+  double minR = 0, maxR = 1;
+
+  // Helper: Normalize a value to 0.0 - 1.0 range
+  double normW(double w) { return (w - minW) / (maxW - minW); }
+  double normR(double r) { return (r - minR) / (maxR - minR); }
+  bool solveMatrix(double A[6][6], double B[6], int n);
+
+ public:
+  void fit(const PTData& data);
+  int16_t predict(double watts, double rpm);
+  int predictWatts(int32_t resistance, float cadence);
+  bool getIsValid() { return isValid; }
+};
+
 class PTHelpers {
  public:
+  ResistanceModel resistanceModel;
   int32_t lookup(int watts, int cad, PTData& ptData);
-  float linearExtrapolate(std::pair<std::vector<float>, std::vector<float>> xy, size_t n, float j);
-  int32_t lookupWatts(int cad, int32_t targetPosition, PTData& ptData);
-  int32_t extrapolateCadenceWatts(int cad, float targetPosition, PTData& ptData);
-  int extrapolateWattsFromCadence(int cad, int32_t targetPosition, PTData& ptData);
-    // return number of readings in the table. If minReadings is set, it will only count entries with at least that many readings.
-  int getNumEntries(PTData& ptData, int minReadings = 0);
+  int lookupWatts(int cad, int32_t targetPosition, PTData& ptData);
   int getTotalReadings(PTData& ptData);
   ptIndex calculateIndex(int watts, int cad);
-  void enterData(PTData& ptData,ptIndex index, int pos);
+  void enterData(PTData& ptData, ptIndex index, int pos);
   void clean(PTData& ptData);
+  void fill(PTData& ptData);
   void fillGaps(PTData& ptData);
   bool fillAllWattColumns(PTData& ptData);
   bool fillAllCadenceLines(PTData& ptData);
-  std::pair<std::vector<float>, std::vector<float>> getRow(int row, PTData& ptData);
-  std::pair<std::vector<float>, std::vector<float>> getColumn(int column, PTData& ptData);
 };

include/Power_Table.h

@@ -17,7 +17,6 @@ class PowerTable {
  public:
   bool saveFlag                  = false;
   bool _hasBeenLoadedThisSession = false;
-  bool fillTableFlag           = false;
 
   PTData ptData;
   PTHelpers ptHelpers;

include/settings.h

@@ -274,10 +274,10 @@ constexpr const char* ANY = "any";
 #define RUNTIMECONFIG_JSON_SIZE 1000 + DEBUG_LOG_BUFFER_SIZE
 
 // Uncomment to use guardrails for ERG mode in the stepper loop.
-#define ERG_GUARDRAILS
+// #define ERG_GUARDRAILS
 
 // Uncomment to enable the use of the power table for ERG mode.
-// #define ERG_MODE_USE_POWER_TABLE
+#define ERG_MODE_USE_POWER_TABLE
 
 // Uncomment to use the PID controller for ERG mode.
 #define ERG_MODE_USE_PID

lib/SS2K/src/sensors/CscSensorData.cpp

@@ -88,10 +88,10 @@ void CscSensorData::decode(uint8_t *data, size_t length) {
         // Time is in 1/1024th of a second
         float revolutions = crankRevolutions - lastCrankRevolutions;
         float timeMinutes = (timeDiff / 1024.0f) / 60.0f;
-        float cadence = revolutions / timeMinutes;
+        float cadence = std::round(revolutions / timeMinutes);
 
         if (cadence > 1) {
-          if (cadence > 200) {  // Human is unlikely producing 200+ cadence
+          if (cadence > 200 || cadence < 0) {  // Human is unlikely producing 200+ cadence
             // Cadence Error: Could happen if cadence measurements were missed
             //                Leave cadence unchanged
             cadence = this->cadence;

lib/SS2K/src/sensors/CyclePowerData.cpp

@@ -122,7 +122,7 @@ void CyclePowerData::decode(uint8_t *data, size_t length) {
       // This casting behavior makes sure the roll over works correctly. Unit tests confirm
       const float crankChange = (uint16_t)((this->crankRev - this->lastCrankRev) * 1024);
       const float timeElapsed = (uint16_t)(this->crankEventTime - this->lastCrankEventTime);
-      float cadence           = (crankChange / timeElapsed) * 60;
+      float cadence           = std::round((crankChange / timeElapsed) * 60);  // cadence in RPM
       if (cadence > 1) {
         if (cadence > 200) {  // Human is unlikely producing 200+ cadence
           // Cadence Error: Could happen if cadence measurements were missed

lib/SS2K/src/sensors/FitnessMachineIndoorBikeData.cpp

@@ -81,7 +81,7 @@ void FitnessMachineIndoorBikeData::decode(uint8_t *data, size_t length) {
         }
         dataIndex += byteSize;
         value             = convert(value, byteSize, signedFlags[typeIndex]);
-        double_t result   = double_t(static_cast<int>((value * resolutions[typeIndex] * 10) + 0.5)) / 10.0;
+        double_t result   = double_t(static_cast<int>(std::round((value * resolutions[typeIndex] * 10) + 0.5)) / 10.0);
         values[typeIndex] = result;
         continue;
       }

lib/SS2K/src/sensors/PelotonData.cpp

@@ -49,7 +49,7 @@ void PelotonData::decode(uint8_t *data, size_t length) {
   switch (data[1]) {
     case PELOTON_POW_ID:
       if (value >= 0) {
-        power = value / 10;
+        power = std::round(value / 10);
       } else {
         power = 0;
       }

src/BLE_Client.cpp

@@ -724,7 +724,11 @@ void SpinBLEClient::postConnect() {
           }
           // update resistance range if supported:
           auto resistanceRangeCharacteristic = pClient->getService(FITNESSMACHINESERVICE_UUID)->getCharacteristic(FITNESSMACHINERESISTANCELEVELRANGE_UUID);
-          if (resistanceRangeCharacteristic && resistanceRangeCharacteristic->canRead()) {
+          // Schwinn IC4 bikes don't transmit in the proper format, so we need to ignore this on bikes with names that start with "IC Bike"
+          if (adevName.startsWith("IC Bike")) {
+            SS2K_LOG(BLE_CLIENT_LOG_TAG, "Ignoring FTMS Resistance Range characteristic on IC Bike device: %s", _BLEd.uniqueName.c_str());
+            resistanceRangeCharacteristic = nullptr;
+          } else if (resistanceRangeCharacteristic && resistanceRangeCharacteristic->canRead()) {
             auto rr = resistanceRangeCharacteristic->readValue();
             if (rr.size() >= 6) {
               const uint8_t* b = reinterpret_cast<const uint8_t*>(rr.data());
@@ -749,6 +753,15 @@ void SpinBLEClient::postConnect() {
 
               rtConfig->resistance.setMin(minRes);
               rtConfig->resistance.setMax(maxRes);
+              // log the entire characteristic info
+              String rrLog = "FTMS Resistance Range raw data:";
+              for (size_t i = 0; i < rr.size(); i++) {
+                char buf[3];
+                snprintf(buf, sizeof(buf), "%02X", static_cast<uint8_t>(rr[i]));
+                rrLog += " " + String(buf);
+              }
+              SS2K_LOG(BLE_CLIENT_LOG_TAG, "%s", rrLog.c_str());
+
               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());
@@ -931,8 +944,7 @@ void SpinBLEClient::checkBLEReconnect() {
   }
 
   if (offset > 0) {
-    SS2K_LOG(BLE_CLIENT_LOG_TAG, "Devices not connected: %s (cfgHRM='%s' cfgPM='%s' cfgRemote='%s')", 
-             notConnectedDevices, cfgHRM, cfgPM, cfgRemote);
+    SS2K_LOG(BLE_CLIENT_LOG_TAG, "Devices not connected: %s (cfgHRM='%s' cfgPM='%s' cfgRemote='%s')", notConnectedDevices, cfgHRM, cfgPM, cfgRemote);
     this->doScan = true;
   }
 }
@@ -1086,7 +1098,7 @@ void SpinBLEAdvertisedDevice::set(const NimBLEAdvertisedDevice* device, int id,
       for (auto& pService : services) {
         BLEUUID serviceUUID = pService->getUUID();
         if (serviceUUID == HEARTSERVICE_UUID) {
-          this->isHRM                = true;
+          this->isHRM = true;
           if (cfgHrmIsNone || cfgHrmIsAny || hrmNameMatch || hrmAddrMatch) {
             spinBLEClient.connectedHRM = true;
             SS2K_LOG(BLE_CLIENT_LOG_TAG, "Registered HRM on Connect");

src/BLE_Custom_Characteristic.cpp

@@ -814,6 +814,20 @@ void BLE_ss2kCustomCharacteristic::process(std::string rxValue) {
       }
       break;
 
+    case BLE_UDPLogging:  // 0x2E
+      LOG_BUF_APPEND("<-UDPLogging");
+      if (rxValue[0] == cc_read) {
+        returnValue[0] = cc_success;
+        returnValue[2] = (uint8_t)(userConfig->getUdpLogEnabled());
+        returnLength += 1;
+      }
+      if (rxValue[0] == cc_write) {
+        returnValue[0] = cc_success;
+        userConfig->setUdpLogEnabled(rxValue[2]);
+        LOG_BUF_APPEND("(%s)", userConfig->getUdpLogEnabled() ? "true" : "false");
+      }
+      break;
+
     default:
       LOG_BUF_APPEND("<-Unknown Characteristic");
       returnValue[0] = cc_error;
@@ -996,4 +1010,9 @@ void BLE_ss2kCustomCharacteristic::parseNemit() {
     }
     return;
   }
+  if (userConfig->getUdpLogEnabled() != _oldParams.getUdpLogEnabled()) {
+    _oldParams.setUdpLogEnabled(userConfig->getUdpLogEnabled());
+    BLE_ss2kCustomCharacteristic::notify(BLE_UDPLogging);
+    return;
+  }
 }