From 3cd7659924830e5639737f92766f796c0b632f62 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 6 Dec 2017 23:57:57 -0600
Subject: [PATCH] Improved Core-compatible jerk code

---
 Marlin/src/module/planner.cpp | 39 ++++++++++-------------------------
 1 file changed, 11 insertions(+), 28 deletions(-)

diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp
index fc2dd2acd..0bdd3ef30 100644
--- a/Marlin/src/module/planner.cpp
+++ b/Marlin/src/module/planner.cpp
@@ -1090,10 +1090,13 @@ 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 and limit speed in mm/sec for each axis, calculate minimum acceleration ratio
   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));
+    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
@@ -1138,6 +1141,9 @@ 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;
@@ -1145,6 +1151,9 @@ 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;
@@ -1246,32 +1255,6 @@ 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,
@@ -1283,7 +1266,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;
-    limited = 0;
+    uint8_t limited = 0;
 
     // Now limit the jerk in all axes.
     const float smaller_speed_factor = vmax_junction / previous_nominal_speed;