bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Fix G2/G3 rounding error (#15507)

Browse Source
This commit is contained in:
Ed Williams 2019-10-10 14:50:08 -10:00 committed by Scott Lahteine
parent 2cf35267c4
commit d8aeeb8ff6
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Marlin/src/gcode/motion/G2_G3.cpp
View File

@ -285,12 +285,13 @@ void GcodeSuite::G2_G3(const bool clockwise) {
if (r) {
const xy_pos_t p1 = current_position, p2 = destination;
if (p1 != p2) {
const xy_pos_t d = p2 - p1, m = (p1 + p2) * 0.5f; // XY distance and midpoint
const xy_pos_t d2 = (p2 - p1) * 0.5f; // XY vector to midpoint of move from current
const float e = clockwise ^ (r < 0) ? -1 : 1, // clockwise -1/1, counterclockwise 1/-1
len = d.magnitude(), // Total move length
h = SQRT((r - d * 0.5f) * (r + d * 0.5f)); // Distance to the arc pivot-point
const xy_pos_t s = { d.x, -d.y }; // Inverse Slope of the perpendicular bisector
arc_offset = m + s * RECIPROCAL(len) * e * h - p1; // The calculated offset
len = d2.magnitude(), // Distance to mid-point of move from current
h2 = (r - len) * (r + len), // factored to reduce rounding error
h = (h2 >= 0) ? SQRT(h2) : 0.0f; // Distance to the arc pivot-point from midpoint
const xy_pos_t s = { -d2.y, d2.x } / len; // Unit vector along perpendicular bisector
arc_offset = d2 + s * e * h; // The calculated offset (mid-point if |r| <= len)
}
}
}