From 1a6a6043102dd27a6107a62993638b4d2b02a4e5 Mon Sep 17 00:00:00 2001
From: tombrazier <68918209+tombrazier@users.noreply.github.com>
Date: Sat, 18 Jun 2022 05:17:12 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20G2/G3=20Arcs=20stutter=20/?=
=?UTF-8?q?=20JD=20speed=20(#24362)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Marlin/src/gcode/motion/G2_G3.cpp | 205 +++++++++++++++---------------
1 file changed, 101 insertions(+), 104 deletions(-)
diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp
index dd1c1d14708..cd8225de690 100644
--- a/Marlin/src/gcode/motion/G2_G3.cpp
+++ b/Marlin/src/gcode/motion/G2_G3.cpp
@@ -197,8 +197,8 @@ 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
@@ -206,19 +206,18 @@ void plan_arc(
#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
+ // 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
+
+ CODE_ITEM_E(const float extruder_per_segment = 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
);
- #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++;
-
- // 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 ENABLED(SCARA_FEEDRATE_SCALING)
- const float inv_duration = scaled_fr_mm_s / segment_mm;
- #endif
-
- millis_t next_idle_ms = millis() + 200UL;
-
- #if N_ARC_CORRECTION > 1
- int8_t arc_recalc_count = N_ARC_CORRECTION;
- #endif
-
- for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
-
- thermalManager.manage_heater();
- const millis_t ms = millis();
- if (ELAPSED(ms, next_idle_ms)) {
- next_idle_ms = ms + 200UL;
- idle();
- }
+ millis_t next_idle_ms = millis() + 200UL;
#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;
+ int8_t arc_recalc_count = N_ARC_CORRECTION;
+ #endif
+
+ for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
+
+ 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;
+ }
+
+ // 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);
+
+ #if HAS_LEVELING && !PLANNER_LEVELING
+ planner.apply_leveling(raw);
#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;
+ if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
+ break;
}
-
- // 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);
-
- #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;
}
// Ensure last segment arrives at target location.