🐛 Fix G2/G3 Arcs stutter / JD speed (MarlinFirmware#24362)

Author: tombrazier

Marlin/src/gcode/motion/G2_G3.cpp

@@ -197,28 +197,27 @@ void plan_arc(
   // Feedrate for the move, scaled by the feedrate multiplier
   const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
 
-  // Get the nominal segment length based on settings
-  const float nominal_segment_mm = (
+  // Get the ideal segment length for the move based on settings
+  const float ideal_segment_mm = (
     #if ARC_SEGMENTS_PER_SEC  // Length based on segments per second and feedrate
       constrain(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM)
     #else
       MAX_ARC_SEGMENT_MM      // Length using the maximum segment size
     #endif
   );
 
-  // Number of whole segments based on the nominal segment length
-  const float nominal_segments = _MAX(FLOOR(flat_mm / nominal_segment_mm), min_segments);
+  // Number of whole segments based on the ideal segment length
+  const float nominal_segments = _MAX(FLOOR(flat_mm / ideal_segment_mm), min_segments),
+              nominal_segment_mm = flat_mm / nominal_segments;
 
-  // A new segment length based on the required minimum
-  const float segment_mm = constrain(flat_mm / nominal_segments, MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM);
+  // The number of whole segments in the arc, with best attempt to honor MIN_ARC_SEGMENT_MM and MAX_ARC_SEGMENT_MM
+  const uint16_t segments = nominal_segment_mm > (MAX_ARC_SEGMENT_MM) ? CEIL(flat_mm / (MAX_ARC_SEGMENT_MM)) :
+                            nominal_segment_mm < (MIN_ARC_SEGMENT_MM) ? _MAX(1, FLOOR(flat_mm / (MIN_ARC_SEGMENT_MM))) :
+                            nominal_segments;
 
-  // The number of whole segments in the arc, ignoring the remainder
-  uint16_t segments = FLOOR(flat_mm / segment_mm);
-
-  // Are the segments now too few to reach the destination?
-  const float segmented_length = segment_mm * segments;
-  const bool tooshort = segmented_length < flat_mm - 0.0001f;
-  const float proportion = tooshort ? segmented_length / flat_mm : 1.0f;
+  #if ENABLED(SCARA_FEEDRATE_SCALING)
+    const float inv_duration = (scaled_fr_mm_s / flat_mm) * segments;
+  #endif
 
   /**
    * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
@@ -246,108 +245,106 @@ void plan_arc(
    * a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
    * This is important when there are successive arc motions.
    */
-  // Vector rotation matrix values
-  xyze_pos_t raw;
-  const float theta_per_segment = proportion * 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 = proportion * travel_L / segments,
-      const float per_segment_I = proportion * travel_I / segments,
-      const float per_segment_J = proportion * travel_J / segments,
-      const float per_segment_K = proportion * travel_K / segments,
-      const float per_segment_U = proportion * travel_U / segments,
-      const float per_segment_V = proportion * travel_V / segments,
-      const float per_segment_W = proportion * travel_W / segments
-    );
-  #endif
 
-  CODE_ITEM_E(const float extruder_per_segment = proportion * travel_E / segments);
-
-  // For shortened segments, run all but the remainder in the loop
-  if (tooshort) segments++;
+  xyze_pos_t raw;
 
-  // 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
-  );
+  // do not 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
 
-  #if ENABLED(SCARA_FEEDRATE_SCALING)
-    const float inv_duration = scaled_fr_mm_s / segment_mm;
-  #endif
+    CODE_ITEM_E(const float extruder_per_segment = travel_E / segments);
 
-  millis_t next_idle_ms = millis() + 200UL;
+    // 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
+    );
 
-  #if N_ARC_CORRECTION > 1
-    int8_t arc_recalc_count = N_ARC_CORRECTION;
-  #endif
+    millis_t next_idle_ms = millis() + 200UL;
 
-  for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
+    #if N_ARC_CORRECTION > 1
+      int8_t arc_recalc_count = N_ARC_CORRECTION;
+    #endif
 
-    thermalManager.manage_heater();
-    const millis_t ms = millis();
-    if (ELAPSED(ms, next_idle_ms)) {
-      next_idle_ms = ms + 200UL;
-      idle();
-    }
+    for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
 
-    #if N_ARC_CORRECTION > 1
-      if (--arc_recalc_count) {
-        // Apply vector rotation matrix to previous rvec.a / 1
-        const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;
-        rvec.a = rvec.a * cos_T - rvec.b * sin_T;
-        rvec.b = r_new_Y;
+      thermalManager.manage_heater();
+      const millis_t ms = millis();
+      if (ELAPSED(ms, next_idle_ms)) {
+        next_idle_ms = ms + 200UL;
+        idle();
       }
-      else
-    #endif
-    {
+
       #if N_ARC_CORRECTION > 1
-        arc_recalc_count = N_ARC_CORRECTION;
+        if (--arc_recalc_count) {
+          // Apply vector rotation matrix to previous rvec.a / 1
+          const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;
+          rvec.a = rvec.a * cos_T - rvec.b * sin_T;
+          rvec.b = r_new_Y;
+        }
+        else
       #endif
+      {
+        #if N_ARC_CORRECTION > 1
+          arc_recalc_count = N_ARC_CORRECTION;
+        #endif
+
+        // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
+        // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
+        // To reduce stuttering, the sin and cos could be computed at different times.
+        // For now, compute both at the same time.
+        const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
+        rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
+        rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
+      }
 
-      // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
-      // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
-      // To reduce stuttering, the sin and cos could be computed at different times.
-      // For now, compute both at the same time.
-      const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
-      rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
-      rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
-    }
-
-    // Update raw location
-    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
-    );
+      // Update raw location
+      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
+      );
 
-    apply_motion_limits(raw);
+      apply_motion_limits(raw);
 
-    #if HAS_LEVELING && !PLANNER_LEVELING
-      planner.apply_leveling(raw);
-    #endif
+      #if HAS_LEVELING && !PLANNER_LEVELING
+        planner.apply_leveling(raw);
+      #endif
 
-    if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
-      break;
+      if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
+        break;
+    }
   }
 
   // Ensure last segment arrives at target location.