From 915203f545f6d3114a5965a19125545f5592c42e Mon Sep 17 00:00:00 2001
From: tombrazier <68918209+tombrazier@users.noreply.github.com>
Date: Mon, 18 Jul 2022 06:51:44 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=A9=B9=20Arc/Planner=20optimization=20fol?=
 =?UTF-8?q?lowup=20(#24509)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Marlin/src/gcode/motion/G2_G3.cpp | 10 ++++++----
 Marlin/src/module/planner.cpp     |  2 +-
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp
index c45204c6f6..1a0a9c8ccc 100644
--- a/Marlin/src/gcode/motion/G2_G3.cpp
+++ b/Marlin/src/gcode/motion/G2_G3.cpp
@@ -294,11 +294,12 @@ void plan_arc(
     // An arc can always complete within limits from a speed which...
     // a) is <= any configured maximum speed,
     // b) does not require centripetal force greater than any configured maximum acceleration,
-    // c) allows the print head to stop in the remining length of the curve within all configured maximum accelerations.
+    // c) is <= nominal speed,
+    // d) allows the print head to stop in the remining length of the curve within all configured maximum accelerations.
     // The last has to be calculated every time through the loop.
     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_acceleration_mm_per_s2[axis_q]),
-                limiting_speed_sqr = _MIN(sq(limiting_speed), limiting_accel * radius);
+                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
@@ -357,12 +358,12 @@ void plan_arc(
 
       // calculate safe speed for stopping by the end of the arc
       arc_mm_remaining -= segment_mm;
-
-      hints.curve_radius = i > 1 ? radius : 0;
       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))
         break;
+
+      hints.curve_radius = radius;
     }
   }
 
@@ -383,6 +384,7 @@ void plan_arc(
   #endif
 
   hints.curve_radius = 0;
+  hints.safe_exit_speed_sqr = 0.0f;
   planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
 
   #if ENABLED(AUTO_BED_LEVELING_UBL)
diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp
index de1351babf..b4455ca5ec 100644
--- a/Marlin/src/module/planner.cpp
+++ b/Marlin/src/module/planner.cpp
@@ -806,7 +806,7 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
     float accelerate_steps_float = (nominal_rate_sq - sq(float(initial_rate))) * (0.5f / accel);
     accelerate_steps = CEIL(accelerate_steps_float);
     const float decelerate_steps_float = (nominal_rate_sq - sq(float(final_rate))) * (0.5f / accel);
-    decelerate_steps = decelerate_steps_float;
+    decelerate_steps = FLOOR(decelerate_steps_float);
 
     // Steps between acceleration and deceleration, if any
     plateau_steps -= accelerate_steps + decelerate_steps;