Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

merging ERG-mode-rewrite into develop on laptop #3

Merged
merged 4 commits into from
Jan 12, 2025
+147 −118
Merged

merging ERG-mode-rewrite into develop on laptop #3

Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2d2ce2b
created and set totalTravel
benv12 Nov 5, 2024
ac3c6fb
caught up date with develop
benv12 Dec 19, 2024
66fd6ee
implemented neighbor weighting for extrapFillTable() function.
benv12 Jan 12, 2025
db82a4d
Update changelog for version 24.12.17
actions-user Jan 12, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
implemented neighbor weighting for extrapFillTable() function.
@benv12
benv12 committed Jan 12, 2025
commit 66fd6eecae41915ea7d1177b542ae772688a6349
3 changes: 0 additions & 3 deletions include/SmartSpin_parameters.h
View file
Original file line number Diff line number Diff line change
@@ -196,9 +196,6 @@ class userParameters {
void setShifterDir(bool shd) { shifterDir = shd; }
bool getShifterDir() { return shifterDir; }

void setTotalTravel(int tT) { totalTravel = tT; }
bool getTotalTravel() { return totalTravel; }

void setUdpLogEnabled(bool enabled) { udpLogEnabled = enabled; }
bool getUdpLogEnabled() { return udpLogEnabled; }

16 changes: 0 additions & 16 deletions src/BLE_Custom_Characteristic.cpp
View file
Original file line number Diff line number Diff line change
@@ -739,22 +739,6 @@ void BLE_ss2kCustomCharacteristic::process(std::string rxValue) {
ss2k->setTargetPosition(int32_t((uint8_t)(rxValue[2]) << 0 | (uint8_t)(rxValue[3]) << 8 | (uint8_t)(rxValue[4]) << 16 | (uint8_t)(rxValue[5]) << 24));
logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, " (%f)", userConfig->getHMax());
}

case BLE_totalTravel: // 0x2C
logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "<-totalTravel");
if (rxValue[0] == cc_read) {
returnValue[0] = cc_success;
returnValue[2] = (uint8_t)(userConfig->getTotalTravel() & 0xff);
returnValue[3] = (uint8_t)(userConfig->getTotalTravel() >> 8);
returnValue[4] = (uint8_t)(userConfig->getTotalTravel() >> 16);
returnValue[5] = (uint8_t)(userConfig->getTotalTravel() >> 24);
returnLength += 4;
}
if (rxValue[0] == cc_write) {
returnValue[0] = cc_success;
ss2k->setTargetPosition(int32_t((uint8_t)(rxValue[2]) << 0 | (uint8_t)(rxValue[3]) << 8 | (uint8_t)(rxValue[4]) << 16 | (uint8_t)(rxValue[5]) << 24));
logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, " (%f)", userConfig->getTotalTravel());
}
break;

case BLE_homingSensitivity: // 0x2C
254 changes: 136 additions & 118 deletions src/ERG_Mode.cpp
View file
Original file line number Diff line number Diff line change
@@ -15,6 +15,7 @@
#include <cmath>
#include <limits>
#include <numeric>
#include <unordered_map>

PowerTable* powerTable = new PowerTable;

@@ -108,9 +109,9 @@ int PowerBuffer::getReadings() {
return ret;
}

void PowerTable::processPowerValue(PowerBuffer& powerBuffer, int cadence, Measurement watts) {
void PowerTable::processPowerValue(PowerBuffer& powerBuffer, int cadence, Measurement watts) { //this basically checks the constaraints and if everything is good it adds it into the powerbuffer. no need to change
if ((cadence >= (MINIMUM_TABLE_CAD - (POWERTABLE_CAD_INCREMENT / 2))) &&
(cadence <= (MINIMUM_TABLE_CAD + (POWERTABLE_CAD_INCREMENT * POWERTABLE_CAD_SIZE) - (POWERTABLE_CAD_SIZE / 2))) && (watts.getValue() > 10) &&
(cadence <= (MINIMUM_TABLE_CAD + (POWERTABLE_CAD_INCREMENT * POWERTABLE_CAD_SIZE) - (POWERTABLE_CAD_SIZE / 2))) && (watts.getValue() > 10) && //adding constraints
(watts.getValue() < (POWERTABLE_WATT_SIZE * POWERTABLE_WATT_INCREMENT))) {
if (powerBuffer.powerEntry[0].readings == 0) {
// Take Initial reading
@@ -402,48 +403,80 @@ void PowerTable::fillTable() {

// Fill each empty cell by linear interpolation
for (int i = 0; i < POWERTABLE_CAD_SIZE; ++i) {
// Interpolate horizontally
for (int j = 0; j < POWERTABLE_WATT_SIZE; ++j) {
if (this->tableRow[i].tableEntry[j].targetPosition == INT16_MIN) {
// Find nearest left and right non-empty cells
int left = j - 1;
while (left >= 0 && this->tableRow[i].tableEntry[left].targetPosition == INT16_MIN) left--;
int right = j + 1;
while (right < POWERTABLE_WATT_SIZE && this->tableRow[i].tableEntry[right].targetPosition == INT16_MIN) right++;

//create two hash maps for row and col
std::unordered_map<int, std::unordered_map<int, int16_t>> rowMap;
std::unordered_map<int, std::unordered_map<int, int16_t>> colMap;

if (left >= 0 && right < POWERTABLE_WATT_SIZE) {
// Linear interpolation
tempValue = this->tableRow[i].tableEntry[left].targetPosition +
(this->tableRow[i].tableEntry[right].targetPosition - this->tableRow[i].tableEntry[left].targetPosition) * (j - left) / (right - left);
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
//store non-empty values
for (int i = 0; i < POWERTABLE_CAD_SIZE; ++i) {
for (int j = 0; j < POWERTABLE_WATT_SIZE; ++j) {
if (this->tableRow[i].tableEntry[j].targetPosition != INT16_MIN) {
rowMap[i][j] = this->tableRow[i].tableEntry[j].targetPosition;
colMap[j][i] = this->tableRow[i].tableEntry[j].targetPosition;
}
}
}
}
}
}

for (int j = 0; j < POWERTABLE_WATT_SIZE; ++j) {
// Interpolate vertically
//horizontal interpolation
for (int i = 0; i < POWERTABLE_CAD_SIZE; ++i) {
if (this->tableRow[i].tableEntry[j].targetPosition == INT16_MIN) {
// Find nearest top and bottom non-empty cells
int top = i - 1;
while (top >= 0 && this->tableRow[top].tableEntry[j].targetPosition == INT16_MIN) top--;
int bottom = i + 1;
while (bottom < POWERTABLE_CAD_SIZE && this->tableRow[bottom].tableEntry[j].targetPosition == INT16_MIN) bottom++;
for (int j = 0; j < POWERTABLE_WATT_SIZE; ++j) {
if (this->tableRow[i].tableEntry[j].targetPosition == INT16_MIN) {

if (top >= 0 && bottom < POWERTABLE_CAD_SIZE) {
// Linear interpolation
tempValue = this->tableRow[top].tableEntry[j].targetPosition +
(this->tableRow[bottom].tableEntry[j].targetPosition - this->tableRow[top].tableEntry[j].targetPosition) * (i - top) / (bottom - top);
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
int left = j - 1;
while (left >= 0 && rowMap[i].find(left) == rowMap[i].end())
left--;

int right = j + 1;
while (right < POWERTABLE_WATT_SIZE && rowMap[i].find(right) == rowMap[i].end())
right++;

if (left >= 0 && right < POWERTABLE_WATT_SIZE) {

int16_t tempValue = rowMap[i][left] + (rowMap[i][right] - rowMap[i][left]) * (j - left) / (right - left);

if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
}
}
}
}
}
}

//vertical interpolation
for (int j = 0; j < POWERTABLE_WATT_SIZE; ++j) {
for (int i = 0; i < POWERTABLE_CAD_SIZE; ++i) {
if (this->tableRow[i].tableEntry[j].targetPosition == INT16_MIN) {

int top = i - 1;
while (top >= 0 && colMap[j].find(top) == colMap[j].end())
top--;

int bottom = i + 1;
while (bottom < POWERTABLE_CAD_SIZE && colMap[j].find(bottom) == colMap[j].end())
bottom++;

if (top >= 0 && bottom < POWERTABLE_CAD_SIZE) {

int16_t tempValue = colMap[j][top] + (colMap[j][bottom] - colMap[j][top]) * (i - top) / (bottom - top);

if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
}
}
}
}
}
}

int weightedAverage(int leftValue, int rightValue, int leftWeight, int rightWeight){
if (leftWeight + rightWeight == 0){
return INT16_MIN;
}
int weightedValue = (leftValue * leftWeight + rightValue * rightWeight) / (leftWeight + rightWeight);
SS2K_LOG(POWERTABLE_LOG_TAG, "Weighted Value: %f", weightedValue);
return (leftValue * leftWeight + rightValue * rightWeight) / (leftWeight + rightWeight);
}

void PowerTable::extrapFillTable() {
@@ -481,37 +514,34 @@ void PowerTable::extrapFillTable() {
if (left >= 0 && right < POWERTABLE_WATT_SIZE) {
// Linear extrapolation
if (this->tableRow[i].tableEntry[left].targetPosition != INT16_MIN && this->tableRow[i].tableEntry[right].targetPosition != INT16_MIN) {
if (j < left) {
// Extrapolate to the left
tempValue = this->tableRow[i].tableEntry[left].targetPosition -
(this->tableRow[i].tableEntry[right].targetPosition - this->tableRow[i].tableEntry[left].targetPosition) / (right - left) * (left - j);
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
} else if (j > right) {
// Extrapolate to the right
tempValue = this->tableRow[i].tableEntry[right].targetPosition +
(this->tableRow[i].tableEntry[right].targetPosition - this->tableRow[i].tableEntry[left].targetPosition) / (right - left) * (j - right);
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}

int leftWeight = (j - left);
int rightWeight = (right - j);
int tempValue = weightedAverage(this->tableRow[i].tableEntry[left].targetPosition, this->tableRow[i].tableEntry[right].targetPosition, leftWeight, rightWeight);
SS2K_LOG(POWERTABLE_LOG_TAG, "Weighted Avg: %f", tempValue)

if(this->testNeighbors(i, j, tempValue).allNeighborsPassed){
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
}
} else if (left - 1 >= 0) {
} else if (left>= 0) {
// Only left value available, extrapolate to the right
if (this->tableRow[i].tableEntry[left].targetPosition != INT16_MIN && this->tableRow[i].tableEntry[left - 1].targetPosition != INT16_MIN) {
tempValue = this->tableRow[i].tableEntry[left].targetPosition +
(j - left) * (left > 0 ? this->tableRow[i].tableEntry[left].targetPosition - this->tableRow[i].tableEntry[left - 1].targetPosition : 1);
if (this->tableRow[i].tableEntry[left].targetPosition != INT16_MIN) {
int rigthWeight = 1;
int tempValue = this->tableRow[i].tableEntry[left].targetPosition + rigthWeight;
SS2K_LOG(POWERTABLE_LOG_TAG, "Weighted Avg: %f", tempValue)

if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
}
} else if (right + 1 < POWERTABLE_WATT_SIZE) {
} else if (right < POWERTABLE_WATT_SIZE) {
// Only right value available, extrapolate to the left
if (this->tableRow[i].tableEntry[right + 1].targetPosition != INT16_MIN && this->tableRow[i].tableEntry[right].targetPosition != INT16_MIN) {
tempValue =
this->tableRow[i].tableEntry[right].targetPosition -
(right - j) * (right < POWERTABLE_WATT_SIZE - 1 ? this->tableRow[i].tableEntry[right + 1].targetPosition - this->tableRow[i].tableEntry[right].targetPosition : 1);
if (this->tableRow[i].tableEntry[right].targetPosition != INT16_MIN) {
int leftWeight = 1;
int tempValue = this->tableRow[i].tableEntry[right].targetPosition - leftWeight;
SS2K_LOG(POWERTABLE_LOG_TAG, "Weighted Avg: %f", tempValue)

if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
@@ -541,43 +571,39 @@ void PowerTable::extrapFillTable() {
// Find nearest right non-empty cell
int right = j + 1;
while (right < POWERTABLE_WATT_SIZE && this->tableRow[i].tableEntry[right].targetPosition == INT16_MIN) right++;
if (this->tableRow[i].tableEntry[left].targetPosition != INT16_MIN && this->tableRow[i].tableEntry[right].targetPosition != INT16_MIN) {

if (left >= 0 && right < POWERTABLE_WATT_SIZE) {
// Linear extrapolation
if (j < left) {
// Extrapolate to the left
tempValue = this->tableRow[i].tableEntry[left].targetPosition -
(this->tableRow[i].tableEntry[right].targetPosition - this->tableRow[i].tableEntry[left].targetPosition) / (right - left) * (left - j);
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
if (this->tableRow[i].tableEntry[left].targetPosition != INT16_MIN && this->tableRow[i].tableEntry[right].targetPosition != INT16_MIN) {

int leftWeight = (j - left);
int rightWeight = (right - j);
int tempValue = weightedAverage(this->tableRow[i].tableEntry[left].targetPosition, this->tableRow[i].tableEntry[right].targetPosition, leftWeight, rightWeight);
SS2K_LOG(POWERTABLE_LOG_TAG, "Weighted Avg: %f", tempValue)

} else if (j > right) {
// Extrapolate to the right
tempValue = this->tableRow[i].tableEntry[right].targetPosition +
(this->tableRow[i].tableEntry[right].targetPosition - this->tableRow[i].tableEntry[left].targetPosition) / (right - left) * (j - right);
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;

if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
}
} else if (left >= 1) {
} else if (left >= 0) {
// Only left value available, extrapolate to the right
if (this->tableRow[i].tableEntry[left].targetPosition != INT16_MIN && this->tableRow[i].tableEntry[left - 1].targetPosition != INT16_MIN) {
tempValue = this->tableRow[i].tableEntry[left].targetPosition +
(j - left) * (left > 0 ? this->tableRow[i].tableEntry[left].targetPosition - this->tableRow[i].tableEntry[left - 1].targetPosition : 1);
if (this->tableRow[i].tableEntry[left].targetPosition != INT16_MIN) {
if (this->tableRow[i].tableEntry[left].targetPosition != INT16_MIN){
int tempValue = this->tableRow[i].tableEntry[left].targetPosition + 1;
SS2K_LOG(POWERTABLE_LOG_TAG, "Weighted Avg: %f", tempValue)
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
}
}
} else if (right + 1 < POWERTABLE_WATT_SIZE) {
} else if (right < POWERTABLE_WATT_SIZE) {
// Only right value available, extrapolate to the left
if (this->tableRow[i].tableEntry[right].targetPosition != INT16_MIN && this->tableRow[i].tableEntry[right + 1].targetPosition != INT16_MIN) {
tempValue = this->tableRow[i].tableEntry[right].targetPosition -
(right - j) *
(right < POWERTABLE_WATT_SIZE - 1 ? this->tableRow[i].tableEntry[right + 1].targetPosition - this->tableRow[i].tableEntry[right].targetPosition : 1);
if (this->tableRow[i].tableEntry[right].targetPosition != INT16_MIN) {
int tempValue = this->tableRow[i].tableEntry[right].targetPosition - 1;
SS2K_LOG(POWERTABLE_LOG_TAG, "Weighted Avg: %f", tempValue)
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
}
}
}
@@ -598,41 +624,33 @@ void PowerTable::extrapFillTable() {
while (bottom < POWERTABLE_CAD_SIZE && this->tableRow[bottom].tableEntry[j].targetPosition == INT16_MIN) bottom++;

if (top >= 0 && bottom < POWERTABLE_CAD_SIZE) {
// Linear extrapolation
if (i < top) {
// Extrapolate upwards
tempValue = this->tableRow[top].tableEntry[j].targetPosition -
(this->tableRow[bottom].tableEntry[j].targetPosition - this->tableRow[top].tableEntry[j].targetPosition) / (bottom - top) * (top - i);
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
} else if (i > bottom) {
// Extrapolate downwards
tempValue = this->tableRow[bottom].tableEntry[j].targetPosition +
(this->tableRow[bottom].tableEntry[j].targetPosition - this->tableRow[top].tableEntry[j].targetPosition) / (bottom - top) * (i - bottom);
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}

if (this->tableRow[top].tableEntry[j].targetPosition != INT16_MIN && this->tableRow[bottom].tableEntry[j].targetPosition != INT16_MIN) {
int topWeight = (i - top);
int bottomWeight = (bottom - i);
int tempValue = weightedAverage(this->tableRow[top].tableEntry[j].targetPosition, this->tableRow[bottom].tableEntry[j].targetPosition, topWeight, bottomWeight);
SS2K_LOG(POWERTABLE_LOG_TAG, "Weighted Avg: %f", tempValue)
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
} else if (top >= 1) {
// Only top value available, extrapolate downwards
if (this->tableRow[top].tableEntry[j].targetPosition != INT16_MIN && this->tableRow[top - 1].tableEntry[j].targetPosition != INT16_MIN) {
tempValue = this->tableRow[top].tableEntry[j].targetPosition +
(i - top) * (top > 0 ? this->tableRow[top].tableEntry[j].targetPosition - this->tableRow[top - 1].tableEntry[j].targetPosition : 1);
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
} else {
}
}

} else if (top >= 0) {
if (this->tableRow[top].tableEntry[j].targetPosition != INT16_MIN) {
int tempValue = this->tableRow[top].tableEntry[j].targetPosition + 1;
SS2K_LOG(POWERTABLE_LOG_TAG, "Weighted Avg: %f", tempValue)
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
} else if (bottom + 1 < POWERTABLE_CAD_SIZE) {
// Only bottom value available, extrapolate upwards
if (this->tableRow[bottom].tableEntry[j].targetPosition != INT16_MIN && this->tableRow[bottom + 1].tableEntry[j].targetPosition != INT16_MIN) {
tempValue = this->tableRow[bottom].tableEntry[j].targetPosition -
(bottom - i) *
(bottom < POWERTABLE_CAD_SIZE - 1 ? this->tableRow[bottom + 1].tableEntry[j].targetPosition - this->tableRow[bottom].tableEntry[j].targetPosition : 1);
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
}

} else if (bottom < POWERTABLE_CAD_SIZE) {
if (this->tableRow[bottom].tableEntry[j].targetPosition != INT16_MIN) {
int tempValue = this->tableRow[bottom].tableEntry[j].targetPosition - 1;
SS2K_LOG(POWERTABLE_LOG_TAG, "Weighted Avg: %f", tempValue)
if (this->testNeighbors(i, j, tempValue).allNeighborsPassed) {
this->tableRow[i].tableEntry[j].targetPosition = tempValue;
}
}
}
}
Loading