From 477e36afab6f9e2cccf3856d6e689230cd27a177 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 20 Dec 2017 15:59:20 -0600
Subject: [PATCH] Revert "[bugfix-1.1.x] COREXY stutter moves (planner.cpp
 changes)  (#8697)"

This reverts commit 86b65e52c4dd72cf55a3f943fd861602d8421772.
---
 Marlin/planner.cpp | 43 +++++++++++++++++++++++++++++++------------
 1 file changed, 31 insertions(+), 12 deletions(-)

diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 74a24d6c4..f83cbd4b0 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -1065,6 +1065,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
     CRITICAL_SECTION_END
   #endif
 
+  block->nominal_speed = block->millimeters * inverse_secs;           //   (mm/sec) Always > 0
+  block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
+
   #if ENABLED(FILAMENT_WIDTH_SENSOR)
     static float filwidth_e_count = 0, filwidth_delay_dist = 0;
 
@@ -1099,14 +1102,10 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
     }
   #endif
 
-  // Calculate and limit speed in mm/sec for each axis, calculate minimum acceleration ratio
+  // Calculate and limit speed in mm/sec for each axis
   float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
-  float max_stepper_speed = 0, min_axis_accel_ratio = 1; // ratio < 1 means acceleration ramp needed
   LOOP_XYZE(i) {
     const float cs = FABS((current_speed[i] = delta_mm[i] * inverse_secs));
-    if (cs > max_jerk[i])
-      NOMORE(min_axis_accel_ratio, max_jerk[i] / cs);
-    NOLESS(max_stepper_speed, cs);
     #if ENABLED(DISTINCT_E_FACTORS)
       if (i == E_AXIS) i += extruder;
     #endif
@@ -1151,9 +1150,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
     }
   #endif // XY_FREQUENCY_LIMIT
 
-  block->nominal_speed = max_stepper_speed; // (mm/sec) Always > 0
-  block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
-
   // Correct the speed
   if (speed_factor < 1.0) {
     LOOP_XYZE(i) current_speed[i] *= speed_factor;
@@ -1161,9 +1157,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
     block->nominal_rate *= speed_factor;
   }
 
-  float safe_speed = block->nominal_speed * min_axis_accel_ratio;
-  static float previous_safe_speed;
-
   // Compute and limit the acceleration rate for the trapezoid generator.
   const float steps_per_mm = block->step_event_count * inverse_millimeters;
   uint32_t accel;
@@ -1265,6 +1258,32 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
     }
   #endif
 
+  /**
+   * Adapted from Průša MKS firmware
+   * https://github.com/prusa3d/Prusa-Firmware
+   *
+   * Start with a safe speed (from which the machine may halt to stop immediately).
+   */
+
+  // Exit speed limited by a jerk to full halt of a previous last segment
+  static float previous_safe_speed;
+
+  float safe_speed = block->nominal_speed;
+  uint8_t limited = 0;
+  LOOP_XYZE(i) {
+    const float jerk = FABS(current_speed[i]), maxj = max_jerk[i];
+    if (jerk > maxj) {
+      if (limited) {
+        const float mjerk = maxj * block->nominal_speed;
+        if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
+      }
+      else {
+        ++limited;
+        safe_speed = maxj;
+      }
+    }
+  }
+
   if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
     // Estimate a maximum velocity allowed at a joint of two successive segments.
     // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
@@ -1276,7 +1295,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
 
     // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
     float v_factor = 1;
-    uint8_t limited = 0;
+    limited = 0;
 
     // Now limit the jerk in all axes.
     const float smaller_speed_factor = vmax_junction / previous_nominal_speed;