Merge pull request #562 from doudar/Big_Table_2

Big table 2

CHANGELOG.md

@@ -39,6 +39,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Fixed bug in CPS service for Wahoo app (and probably others).
 - Depreciated the SPIFFS->LittleFS upgrader.
 - Increased ERG mode sensitivity.
+- removed extra logging when loading table.
+- prevent table returns from going in the wrong direction.
 
 
 ### Hardware

include/ERG_Mode.h

@@ -14,7 +14,7 @@
 #define ERG_MODE_LOG_CSV_TAG "ERG_Mode_CSV"
 #define POWERTABLE_LOG_TAG "PTable"
 #define ERG_MODE_DELAY 700
-#define RETURN_ERROR INT16_MIN
+#define RETURN_ERROR INT32_MIN
 
 extern TaskHandle_t ErgTask;
 void setupERG();
@@ -23,7 +23,7 @@ void ergTaskLoop(void* pvParameters);
 class PowerEntry {
  public:
  int watts;
- int targetPosition;
+ int32_t targetPosition;
  int cad;
  int readings;
 

include/SmartSpin_parameters.h

@@ -55,13 +55,13 @@ class Measurement {
 
 class RuntimeParameters {
  private:
- float targetIncline = 0.0;
- float currentIncline = 0.0;
+ double targetIncline = 0.0;
+ double currentIncline = 0.0;
  float simulatedSpeed = 0.0;
  uint8_t FTMSMode = 0x00;
  int shifterPosition = 0;
- int minStep = -DEFAULT_STEPPER_TRAVEL;
- int maxStep = DEFAULT_STEPPER_TRAVEL;
+ int32_t minStep = -DEFAULT_STEPPER_TRAVEL;
+ int32_t maxStep = DEFAULT_STEPPER_TRAVEL;
  int minResistance = -DEFAULT_RESISTANCE_RANGE;
  int maxResistance = DEFAULT_RESISTANCE_RANGE;
  bool simTargetWatts = false;

include/settings.h

@@ -65,7 +65,7 @@ const char * const DEFAULT_PASSWORD = "password";
 #define STEPPER_ACCELERATION 3000
 
 // Stepper Max Speed in steps/s
-#define DEFAULT_STEPPER_SPEED 1500
+#define DEFAULT_STEPPER_SPEED 3500
 
 // Default ERG Sensitivity. Predicated on # of Shifts (further defined by shift steps) per 30 watts of resistance change.
 // I.E. If the difference between ERG target and Current watts were 30, and the Shift step is defined as 600 steps,

src/BLE_Custom_Characteristic.cpp

@@ -9,32 +9,78 @@
 
 Custom Characteristic for userConfig Variable manipulation via BLE
 
-An example follows to read/write 26.3kph to simulatedSpeed:
-simulatedSpeed is a float and first needs to be converted to int by *10 for transmission, so convert 26.3kph to 263 (multiply by 10)
-Decimal 263 == hexidecimal 0107 but the data needs to be converted to LSO, MSO to match the rest of the BLE spec so 263 == 0x07, 0x01 (LSO,MSO)
+**Overview:**
+This characteristic allows for reading and writing various user configuration parameters via BLE. The format for writing and reading data follows a specific protocol.
+
+**Writing Data:**
+- Format:
+ 0x02, <variable>, <LSO>, <MSO>
+ - 0x02: Operator for write
+ - <variable>: The identifier for the variable to be written
+ - <LSO>: Least significant byte of the value
+ - <MSO>: Most significant byte of the value
+
+- Example:
+ To write 26.3 kph to simulatedSpeed:
+ - Convert 26.3 to an integer by multiplying by 10: 263
+ - Convert 263 to hexadecimal: 0x0107
+ - Swap bytes for little-endian format: 0x07, 0x01
+ - Write command: 0x02, 0x06, 0x07, 0x01
+
+**Reading Data:**
+- Format:
+ 0x01, <variable>
+ - 0x01: Operator for read
+ - <variable>: The identifier for the variable to be read
+
+- Example:
+ To read the value of simulatedSpeed:
+ - Read command: 0x01, 0x06
+
+**Server Response:**
+- For both read and write operations, the server responds with:
+ 0x80, <variable>, <LSO>, <MSO>
+ - 0x80: Status indicating success
+ - <variable>: The identifier for the variable
+ - <LSO>: Least significant byte of the value
+ - <MSO>: Most significant byte of the value
+
+**Detailed Variable Handling:**
+- Some float values are multiplied by 10 or 100 for transmission.
+- True values are > 00, and false values are 00.
+
+**Examples for Other Variables:**
+
+1. Incline (0x02):
+ - Read command: 0x01, 0x02
+ - Server response for 5.5% incline:
+ - Stored as integer: 55 (multiplied by 10)
+ - Hexadecimal: 0x0037
+ - Little-endian: 0x37, 0x00
+ - Response: 0x80, 0x02, 0x37, 0x00
+
+2. Simulated Watts (0x03):
+ - Read command: 0x01, 0x03
+ - Server response for 200 watts:
+ - Integer: 200
+ - Hexadecimal: 0x00C8
+ - Little-endian: 0xC8, 0x00
+ - Response: 0x80, 0x03, 0xC8, 0x00
+
+3. Simulated Heart Rate (0x04):
+ - Read command: 0x01, 0x04
+ - Server response for 75 bpm:
+ - Integer: 75
+ - Hexadecimal: 0x004B
+ - Little-endian: 0x4B, 0x00
+ - Response: 0x80, 0x04, 0x4B, 0x00
+
+4. Device Name (0x07):
+ - Read command: 0x01, 0x07
+ - Server response for "MyDevice":
+ - ASCII for "MyDevice": 0x4D, 0x79, 0x44, 0x65, 0x76, 0x69, 0x63, 0x65
+ - Response: 0x80, 0x07, 0x4D, 0x79, 0x44, 0x65, 0x76, 0x69, 0x63, 0x65
 
-So,
-
-If client wants to write (0x02) int value 263 (0x07 0x01) to simulatedSpeed(0x06):
-
-Client Writes:
-0x02, 0x06, 0x07, 0x01
-(operator, variable, LSO, MSO)
-
-Server will then indicate:
-0x80, 0x06, 0x07, 0x01
-(status, variable, LSO, MSO)
-
-Example to cc_read (0x01) from simulatedSpeed (0x06)
-
-Client Writes:
-0x01, 0x06
-Server will then indicate:
-0x80, 0x06, 0x07, 0x01
- cc_success, simulatedSpeed,0x07,0x01
-
-Pay special attention to the float values below. Since they have to be transmitted as an int, some are converted *100, others are converted *10.
-True values are >00. False are 00.
 */
 
 #include <BLE_Common.h>

src/BLE_Server.cpp

@@ -210,7 +210,7 @@ void calculateInstPwrFromHR() {
  oldHR = newHR; // Copying HR from Last loop
  newHR = rtConfig->hr.getValue();
 
- delta = (newHR - oldHR) / (BLE_CLIENT_DELAY / 1000) + 1;
+ delta = (newHR - oldHR) / ((BLE_CLIENT_DELAY / 1000) + 1);
 
  // userConfig->setSimulatedWatts((s1Pwr*s2HR)-(s2Pwr*S1HR))/(S2HR-s1HR)+(userConfig->getSimulatedHr(*((s1Pwr-s2Pwr)/(s1HR-s2HR)));
  int avgP = ((userPWC->session1Pwr * userPWC->session2HR) - (userPWC->session2Pwr * userPWC->session1HR)) / (userPWC->session2HR - userPWC->session1HR) +

src/ERG_Mode.cpp

@@ -40,42 +40,43 @@ void ergTaskLoop(void* pvParameters) {
  PowerBuffer powerBuffer;
 
  ergMode._writeLogHeader();
- bool isInErgMode = false;
  bool hasConnectedPowerMeter = false;
  bool simulationRunning = false;
  int loopCounter = 0;
 
  while (true) {
  // be quiet while updating via BLE
  while (ss2k->isUpdating) {
- vTaskDelay(100);
+ vTaskDelay(ERG_MODE_DELAY / portTICK_PERIOD_MS);
  }
 
  vTaskDelay(ERG_MODE_DELAY / portTICK_PERIOD_MS);
 
- hasConnectedPowerMeter = spinBLEClient.connectedPM;
- simulationRunning = rtConfig->watts.getTarget();
- if (!simulationRunning) {
- simulationRunning = rtConfig->watts.getSimulate();
- }
+ if (rtConfig->cad.getValue() > 0 && rtConfig->watts.getValue() > 0) {
+ hasConnectedPowerMeter = spinBLEClient.connectedPM;
+ simulationRunning = rtConfig->watts.getTarget();
+ if (!simulationRunning) {
+ simulationRunning = rtConfig->watts.getSimulate();
+ }
 
- // add values to torque table
- powerTable->processPowerValue(powerBuffer, rtConfig->cad.getValue(), rtConfig->watts);
+  // add values to torque table
+  powerTable->processPowerValue(powerBuffer, rtConfig->cad.getValue(), rtConfig->watts);
 
- // compute ERG
- if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetPower) && (hasConnectedPowerMeter || simulationRunning)) {
- ergMode.computeErg();
- }
+  // compute ERG
+  if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetPower) && (hasConnectedPowerMeter || simulationRunning)) {
+  ergMode.computeErg();
+  }
 
- // resistance mode
- if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetResistanceLevel) && (rtConfig->getMaxResistance() != DEFAULT_RESISTANCE_RANGE)) {
- ergMode.computeResistance();
- }
+  // resistance mode
+  if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetResistanceLevel) && (rtConfig->getMaxResistance() != DEFAULT_RESISTANCE_RANGE)) {
+  ergMode.computeResistance();
+  }
 
- // Set Min and Max Stepper positions
- if (loopCounter > 50) {
- loopCounter = 0;
- powerTable->setStepperMinMax();
+ // Set Min and Max Stepper positions
+ if (loopCounter > 50) {
+ loopCounter = 0;
+ powerTable->setStepperMinMax();
+ }
  }
 
  if (ss2k->resetPowerTableFlag) {
@@ -147,7 +148,7 @@ void PowerTable::processPowerValue(PowerBuffer& powerBuffer, int cadence, Measur
 
 // Set min / max stepper position
 void PowerTable::setStepperMinMax() {
- int _return = RETURN_ERROR;
+ int32_t _return = RETURN_ERROR;
 
  // if the FTMS device reports resistance feedback, skip estimating min_max
  if (rtConfig->resistance.getValue() > 0) {
@@ -183,7 +184,7 @@ void PowerTable::setStepperMinMax() {
  // never set less than one shift above current incline.
  if ((_return <= rtConfig->getCurrentIncline()) && (rtConfig->watts.getValue() < userConfig->getMaxWatts())) {
  _return = rtConfig->getCurrentIncline() + userConfig->getShiftStep();
-  SS2K_LOG(ERG_MODE_LOG_TAG, "Max Position too close to current incline: %d", _return);
+ SS2K_LOG(ERG_MODE_LOG_TAG, "Max Position too close to current incline: %d", _return);
  }
  // never set below min step.
  if (_return <= rtConfig->getMinStep()) {
@@ -196,7 +197,7 @@ void PowerTable::setStepperMinMax() {
  }
 }
 
-int PowerTable::lookup(int watts, int cad) {
+int32_t PowerTable::lookup(int watts, int cad) {
  int cadIndex = round(((float)cad - (float)MINIMUM_TABLE_CAD) / (float)POWERTABLE_CAD_INCREMENT);
  int wattIndex = round((float)watts / (float)POWERTABLE_WATT_INCREMENT);
 
@@ -211,12 +212,14 @@ int PowerTable::lookup(int watts, int cad) {
  int extrapRow1 = -1, extrapRow2 = -1;
  for (int i = 0; i < POWERTABLE_CAD_SIZE; ++i) {
  if (this->tableRow[i].tableEntry[wattIndex].targetPosition != INT16_MIN) {
- if (extrapRow1 == -1) {
- extrapRow1 = i;
- } else {
- extrapRow2 = i;
- break;
- }
+ extrapRow1 = i;
+ break;
+ }
+ }
+ for (int i = POWERTABLE_CAD_SIZE - 1; i >= 0; --i) {
+ if (this->tableRow[i].tableEntry[wattIndex].targetPosition != INT16_MIN) {
+ extrapRow2 = i;
+ break;
  }
  }
  if (extrapRow1 != -1 && extrapRow2 != -1) {
@@ -254,7 +257,7 @@ int PowerTable::lookup(int watts, int cad) {
  }
  }
  // Not enough data.
- return INT16_MIN;
+ return INT32_MIN;
  }
 
  // Edge cases out of the way, we should be able to interpolate.
@@ -313,9 +316,8 @@ int PowerTable::lookup(int watts, int cad) {
  }
 
  int ret = (sum / count) * 100;
- SS2K_LOG(ERG_MODE_LOG_TAG, "Lookup result: %dw %dcad %d",watts,cad, ret);
+ SS2K_LOG(ERG_MODE_LOG_TAG, "Lookup result: %dw %dcad %d", watts, cad, ret);
  return ret;
-
 }
 
 // returns class of all neighbors that are found and within expected values.
@@ -733,8 +735,8 @@ void PowerTable::clean() {
 
 void PowerTable::newEntry(PowerBuffer& powerBuffer) {
  // these are floats so that we make sure division works correctly.
- float watts = 0;
- float cad = 0;
+ float watts  = 0;
+ float cad  = 0;
  float targetPosition = 0;
 
  // First, take the power buffer and average all of the samples together.
@@ -930,7 +932,7 @@ bool PowerTable::_manageSaveState() {
  }
  this->tableRow[i].tableEntry[j].targetPosition = savedTargetPosition;
  this->tableRow[i].tableEntry[j].readings = savedReadings;
- SS2K_LOG(POWERTABLE_LOG_TAG, "Position %d, %d, Target %d, Readings %d, loaded", i, j, this->tableRow[i].tableEntry[j].targetPosition,
+ //SS2K_LOG(POWERTABLE_LOG_TAG, "Position %d, %d, Target %d, Readings %d, loaded", i, j, this->tableRow[i].tableEntry[j].targetPosition,
  this->tableRow[i].tableEntry[j].readings);
  }
  }
@@ -1076,14 +1078,15 @@ void ErgMode::computeResistance() {
  return;
  }
 
- int newSetPoint = rtConfig->resistance.getTarget();
  int actualDelta = rtConfig->resistance.getTarget() - rtConfig->resistance.getValue();
- rtConfig->setTargetIncline(rtConfig->getTargetIncline() + (100 * actualDelta));
- if (actualDelta == 0) {
+ if (actualDelta != 0) {
+ rtConfig->setTargetIncline(rtConfig->getTargetIncline() + (100 * actualDelta));
+ } else {
  rtConfig->setTargetIncline(rtConfig->getCurrentIncline());
  }
  oldResistance = rtConfig->resistance;
 }
+
 // as a note, Trainer Road sends 50w target whenever the app is connected.
 void ErgMode::computeErg() {
  Measurement newWatts = rtConfig->watts;
@@ -1119,6 +1122,20 @@ void ErgMode::computeErg() {
 
 void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) {
  int32_t tableResult = powerTable->lookup(newWatts.getTarget(), newCadence);
+
+ // Sanity check for targets
+ if (tableResult != RETURN_ERROR) {
+ if (rtConfig->watts.getValue() > newWatts.getTarget() && tableResult > rtConfig->getCurrentIncline()) {
+ SS2K_LOG(ERG_MODE_LOG_TAG, "Table Result Failed High Test: %d", tableResult);
+ tableResult = RETURN_ERROR;
+ }
+ if (rtConfig->watts.getValue() < newWatts.getTarget() && tableResult < rtConfig->getCurrentIncline()) {
+ SS2K_LOG(ERG_MODE_LOG_TAG, "Table Result Failed Low Test: %d", tableResult);
+ tableResult = RETURN_ERROR;
+ }
+ }
+
+ // Handle return errors
  if (tableResult == RETURN_ERROR) {
  int wattChange = newWatts.getTarget() - newWatts.getValue();
  float deviation = ((float)wattChange * 100.0) / ((float)newWatts.getTarget());
@@ -1139,7 +1156,7 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) {
  i++;
  }
 
- vTaskDelay((ERG_MODE_DELAY * 3) / portTICK_PERIOD_MS); // Wait for power meter to register new watts
+ vTaskDelay((ERG_MODE_DELAY * 2) / portTICK_PERIOD_MS); // Wait for power meter to register new watts
 }
 
 void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) {