Implement median

Firmware/xyzcal.cpp

@@ -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);