3DPrinting/Prusa-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Implement median

Browse Source
This commit is contained in:
espr14 2020-12-30 15:15:17 +01:00
parent 13fefaa1bf
commit a9fd09ca3f
1 changed files with 21 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
Firmware/xyzcal.cpp
View File

@ -800,6 +800,24 @@ float highest(float *points, const uint8_t num_points){
return max;
}
/// slow bubble sort but short
void sort(float *points, const uint8_t num_points){
/// one direction bubble sort
for (uint8_t i = 0; i < num_points; ++i){
for (uint8_t j = 0; j < num_points - i - 1; ++j){
if (points[j] > points[j + 1])
SWAP(points[j], points[j + 1]);
}
}
}
/// sort array and returns median value
/// don't send empty array or nullptr
float median(float *points, const uint8_t num_points){
sort(points, num_points);
return points[num_points / 2];
}
/// Searches for circle iteratively
/// Uses points on the perimeter. If point is high it pushes circle out of the center (shift or change of radius),
/// otherwise to the center.
@ -846,18 +864,11 @@ void dynamic_circle(uint8_t *matrix_32x32, float &x, float &y, float &r, uint8_t
}
}
/// remove extreme values (slow but simple)
for (uint8_t j = 0; j < blocks / 2; ++j){
remove_highest(shifts_x, blocks);
remove_highest(shifts_y, blocks);
remove_highest(shifts_r, blocks);
}
/// median is the highest now
norm = 1.f / (32.f * (num_points * 3 / 4));
x += CLAMP(highest(shifts_x, blocks) * norm, -max_val, max_val);
y += CLAMP(highest(shifts_y, blocks) * norm, -max_val, max_val);
r += CLAMP(highest(shifts_r, blocks) * norm, -max_val, max_val);
x += CLAMP(median(shifts_x, blocks) * norm, -max_val, max_val);
y += CLAMP(median(shifts_y, blocks) * norm, -max_val, max_val);
r += CLAMP(median(shifts_r, blocks) * norm, -max_val, max_val);
r = MAX(2, r);