🐛 Fixes for G2/G3 arcs (#26170)

Author: wavexx

Marlin/src/gcode/motion/G2_G3.cpp

@@ -82,14 +82,17 @@ void plan_arc(
               rt_X = cart[axis_p] - center_P,
               rt_Y = cart[axis_q] - center_Q;
 
-  ARC_LIJKUVW_CODE(
-    const float start_L = current_position[axis_l],
-    const float start_I = current_position.i,
-    const float start_J = current_position.j,
-    const float start_K = current_position.k,
-    const float start_U = current_position.u,
-    const float start_V = current_position.v,
-    const float start_W = current_position.w
+  // Starting position of the move for all non-arc axes
+  // i.e., only one of X, Y, or Z, plus the rest.
+  ARC_LIJKUVWE_CODE(
+    float start_L = current_position[axis_l],
+    float start_I = current_position.i,
+    float start_J = current_position.j,
+    float start_K = current_position.k,
+    float start_U = current_position.u,
+    float start_V = current_position.v,
+    float start_W = current_position.w,
+    float start_E = current_position.e
   );
 
   // Angle of rotation between position and target from the circle center.
@@ -125,6 +128,7 @@ void plan_arc(
     min_segments = CEIL((MIN_CIRCLE_SEGMENTS) * portion_of_circle);     // Minimum segments for the arc
   }
 
+  // Total travel on all the non-arc axes
   ARC_LIJKUVWE_CODE(
     float travel_L = cart[axis_l] - start_L,
     float travel_I = cart.i       - start_I,
@@ -133,7 +137,7 @@ void plan_arc(
     float travel_U = cart.u       - start_U,
     float travel_V = cart.v       - start_V,
     float travel_W = cart.w       - start_W,
-    float travel_E = cart.e       - current_position.e
+    float travel_E = cart.e       - start_E
   );
 
   // If "P" specified circles, call plan_arc recursively then continue with the rest of the arc
@@ -166,15 +170,29 @@ void plan_arc(
       );
       plan_arc(temp_position, offset, clockwise, 0);            // Plan a single whole circle
     }
+
+    // Get starting coordinates for the remainder from the current position
+    ARC_LIJKUVWE_CODE(
+      start_L = current_position[axis_l],
+      start_I = current_position.i,
+      start_J = current_position.j,
+      start_K = current_position.k,
+      start_U = current_position.u,
+      start_V = current_position.v,
+      start_W = current_position.w,
+      start_E = current_position.e
+    );
+
+    // Update travel distance for the remainder
     ARC_LIJKUVWE_CODE(
-      travel_L = cart[axis_l] - current_position[axis_l],       // Linear X, Y, or Z
-      travel_I = cart.i       - current_position.i,             // The rest are also non-arc
-      travel_J = cart.j       - current_position.j,
-      travel_K = cart.k       - current_position.k,
-      travel_U = cart.u       - current_position.u,
-      travel_V = cart.v       - current_position.v,
-      travel_W = cart.w       - current_position.w,
-      travel_E = cart.e       - current_position.e
+      travel_L = cart[axis_l] - start_L,                        // Linear X, Y, or Z
+      travel_I = cart.i       - start_I,                        // The rest are also non-arc
+      travel_J = cart.j       - start_J,
+      travel_K = cart.k       - start_K,
+      travel_U = cart.u       - start_U,
+      travel_V = cart.v       - start_V,
+      travel_W = cart.w       - start_W,
+      travel_E = cart.e       - start_E
     );
   }
 
@@ -256,38 +274,35 @@ void plan_arc(
 
   xyze_pos_t raw;
 
-  // do not calculate rotation parameters for trivial single-segment arcs
+  // Don't calculate rotation parameters for trivial single-segment arcs
   if (segments > 1) {
     // Vector rotation matrix values
     const float theta_per_segment = angular_travel / segments,
                 sq_theta_per_segment = sq(theta_per_segment),
                 sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
                 cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
 
-    #if DISABLED(AUTO_BED_LEVELING_UBL)
-      ARC_LIJKUVW_CODE(
-        const float per_segment_L = travel_L / segments,
-        const float per_segment_I = travel_I / segments,
-        const float per_segment_J = travel_J / segments,
-        const float per_segment_K = travel_K / segments,
-        const float per_segment_U = travel_U / segments,
-        const float per_segment_V = travel_V / segments,
-        const float per_segment_W = travel_W / segments
-      );
-    #endif
-
-    CODE_ITEM_E(const float extruder_per_segment = travel_E / segments);
+    ARC_LIJKUVWE_CODE(
+      const float per_segment_L = travel_L / segments,
+      const float per_segment_I = travel_I / segments,
+      const float per_segment_J = travel_J / segments,
+      const float per_segment_K = travel_K / segments,
+      const float per_segment_U = travel_U / segments,
+      const float per_segment_V = travel_V / segments,
+      const float per_segment_W = travel_W / segments,
+      const float per_segment_E = travel_E / segments
+    );
 
     // Initialize all linear axes and E
     ARC_LIJKUVWE_CODE(
-      raw[axis_l] = current_position[axis_l],
-      raw.i       = current_position.i,
-      raw.j       = current_position.j,
-      raw.k       = current_position.k,
-      raw.u       = current_position.u,
-      raw.v       = current_position.v,
-      raw.w       = current_position.w,
-      raw.e       = current_position.e
+      raw[axis_l] = start_L,
+      raw.i       = start_I,
+      raw.j       = start_J,
+      raw.k       = start_K,
+      raw.u       = start_U,
+      raw.v       = start_V,
+      raw.w       = start_W,
+      raw.e       = start_E
     );
 
     millis_t next_idle_ms = millis() + 200UL;
@@ -305,7 +320,6 @@ void plan_arc(
     const float limiting_accel = _MIN(planner.settings.max_acceleration_mm_per_s2[axis_p], planner.settings.max_acceleration_mm_per_s2[axis_q]),
                 limiting_speed = _MIN(planner.settings.max_feedrate_mm_s[axis_p], planner.settings.max_feedrate_mm_s[axis_q]),
                 limiting_speed_sqr = _MIN(sq(limiting_speed), limiting_accel * radius, sq(scaled_fr_mm_s));
-    float arc_mm_remaining = flat_mm;
 
     for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
 
@@ -343,16 +357,14 @@ void plan_arc(
       raw[axis_p] = center_P + rvec.a;
       raw[axis_q] = center_Q + rvec.b;
       ARC_LIJKUVWE_CODE(
-        #if ENABLED(AUTO_BED_LEVELING_UBL)
-          raw[axis_l] = start_L,
-          raw.i = start_I, raw.j = start_J, raw.k = start_K,
-          raw.u = start_U, raw.v = start_V, raw.w = start_V
-        #else
-          raw[axis_l] += per_segment_L,
-          raw.i += per_segment_I, raw.j += per_segment_J, raw.k += per_segment_K,
-          raw.u += per_segment_U, raw.v += per_segment_V, raw.w += per_segment_W
-        #endif
-        , raw.e += extruder_per_segment
+        raw[axis_l] = start_L + per_segment_L * i,
+        raw.i       = start_I + per_segment_I * i,
+        raw.j       = start_J + per_segment_J * i,
+        raw.k       = start_K + per_segment_K * i,
+        raw.u       = start_U + per_segment_U * i,
+        raw.v       = start_V + per_segment_V * i,
+        raw.w       = start_W + per_segment_W * i,
+        raw.e       = start_E + per_segment_E * i
       );
 
       apply_motion_limits(raw);
@@ -362,7 +374,7 @@ void plan_arc(
       #endif
 
       // calculate safe speed for stopping by the end of the arc
-      arc_mm_remaining -= segment_mm;
+      const float arc_mm_remaining = flat_mm - segment_mm * i;
       hints.safe_exit_speed_sqr = _MIN(limiting_speed_sqr, 2 * limiting_accel * arc_mm_remaining);
 
       if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints))
@@ -374,13 +386,6 @@ void plan_arc(
 
   // Ensure last segment arrives at target location.
   raw = cart;
-  #if ENABLED(AUTO_BED_LEVELING_UBL)
-    ARC_LIJKUVW_CODE(
-      raw[axis_l] = start_L,
-      raw.i = start_I, raw.j = start_J, raw.k = start_K,
-      raw.u = start_U, raw.v = start_V, raw.w = start_W
-    );
-  #endif
 
   apply_motion_limits(raw);
 
@@ -392,14 +397,7 @@ void plan_arc(
   hints.safe_exit_speed_sqr = 0.0f;
   planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
 
-  #if ENABLED(AUTO_BED_LEVELING_UBL)
-    ARC_LIJKUVW_CODE(
-      raw[axis_l] = start_L,
-      raw.i = start_I, raw.j = start_J, raw.k = start_K,
-      raw.u = start_U, raw.v = start_V, raw.w = start_W
-    );
-  #endif
-  current_position = raw;
+  current_position = cart;
 
 } // plan_arc