Split first move to planner for better chaining

Address #8573, #8595

Marlin/planner.cpp

@@ -687,69 +687,35 @@ void Planner::calculate_volumetric_multipliers() {
 #endif // PLANNER_LEVELING
 
 /**
- * Planner::_buffer_line
- *
- * Add a new linear movement to the buffer.
+ * Planner::_buffer_steps
  *
- * Leveling and kinematics should be applied ahead of calling this.
+ * Add a new linear movement to the buffer (in terms of steps).
  *
- *  a,b,c,e     - target positions in mm or degrees
- *  fr_mm_s     - (target) speed of the move
- *  extruder    - target extruder
+ *  target    - target position in steps units
+ *  fr_mm_s   - (target) speed of the move
+ *  extruder  - target extruder
  */
-void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder) {
-
-  // The target position of the tool in absolute steps
-  // Calculate target position in absolute steps
-  //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
-  const long target[XYZE] = {
-    LROUND(a * axis_steps_per_mm[X_AXIS]),
-    LROUND(b * axis_steps_per_mm[Y_AXIS]),
-    LROUND(c * axis_steps_per_mm[Z_AXIS]),
-    LROUND(e * axis_steps_per_mm[E_AXIS_N])
-  };
-
-  // When changing extruders recalculate steps corresponding to the E position
-  #if ENABLED(DISTINCT_E_FACTORS)
-    if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
-      position[E_AXIS] = LROUND(position[E_AXIS] * axis_steps_per_mm[E_AXIS_N] * steps_to_mm[E_AXIS + last_extruder]);
-      last_extruder = extruder;
-    }
-  #endif
+void Planner::_buffer_steps(const int32_t target[XYZE], float fr_mm_s, const uint8_t extruder) {
 
   const int32_t da = target[X_AXIS] - position[X_AXIS],
                 db = target[Y_AXIS] - position[Y_AXIS],
                 dc = target[Z_AXIS] - position[Z_AXIS];
 
-  /*
-  SERIAL_ECHOPAIR("  Planner FR:", fr_mm_s);
-  SERIAL_CHAR(' ');
-  #if IS_KINEMATIC
-    SERIAL_ECHOPAIR("A:", a);
-    SERIAL_ECHOPAIR(" (", da);
-    SERIAL_ECHOPAIR(") B:", b);
-  #else
-    SERIAL_ECHOPAIR("X:", a);
+  int32_t de = target[E_AXIS] - position[E_AXIS];
+
+  /* <-- add a slash to enable
+    SERIAL_ECHOPAIR("  _buffer_steps FR:", fr_mm_s);
+    SERIAL_ECHOPAIR(" A:", target[A_AXIS]);
     SERIAL_ECHOPAIR(" (", da);
-    SERIAL_ECHOPAIR(") Y:", b);
-  #endif
-  SERIAL_ECHOPAIR(" (", db);
-  #if ENABLED(DELTA)
-    SERIAL_ECHOPAIR(") C:", c);
-  #else
-    SERIAL_ECHOPAIR(") Z:", c);
-  #endif
-  SERIAL_ECHOPAIR(" (", dc);
-  SERIAL_CHAR(')');
-  SERIAL_EOL();
+    SERIAL_ECHOPAIR(" steps) B:", target[B_AXIS]);
+    SERIAL_ECHOPAIR(" (", db);
+    SERIAL_ECHOLNPGM(" steps) C:", target[C_AXIS]);
+    SERIAL_ECHOPAIR(" (", dc);
+    SERIAL_ECHOLNPGM(" steps) E:", target[E_AXIS]);
+    SERIAL_ECHOPAIR(" (", de);
+    SERIAL_ECHOLNPGM(" steps)");
   //*/
 
-  // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
-  if (DEBUGGING(DRYRUN))
-    position[E_AXIS] = target[E_AXIS];
-
-  int32_t de = target[E_AXIS] - position[E_AXIS];
-
   #if ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE)
     if (de) {
       #if ENABLED(PREVENT_COLD_EXTRUSION)
@@ -1056,6 +1022,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
     // Segment time im micro seconds
     uint32_t segment_time_us = LROUND(1000000.0 / inverse_secs);
   #endif
+
   #if ENABLED(SLOWDOWN)
     if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / 2 - 1)) {
       if (segment_time_us < min_segment_time_us) {
@@ -1294,7 +1261,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
     }
   }
 
-  if (moves_queued > 1 && !UNEAR_ZERO(previous_nominal_speed)) {
+  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,
     // then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
@@ -1368,7 +1335,6 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
   previous_safe_speed = safe_speed;
 
   #if ENABLED(LIN_ADVANCE)
-
     /**
      *
      * Use LIN_ADVANCE for blocks if all these are true:
@@ -1407,9 +1373,79 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
 
   recalculate();
 
+} // _buffer_steps()
+
+/**
+ * Planner::_buffer_line
+ *
+ * Add a new linear movement to the buffer in axis units.
+ *
+ * Leveling and kinematics should be applied ahead of calling this.
+ *
+ *  a,b,c,e   - target positions in mm and/or degrees
+ *  fr_mm_s   - (target) speed of the move
+ *  extruder  - target extruder
+ */
+void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder) {
+  // When changing extruders recalculate steps corresponding to the E position
+  #if ENABLED(DISTINCT_E_FACTORS)
+    if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
+      position[E_AXIS] = LROUND(position[E_AXIS] * axis_steps_per_mm[E_AXIS_N] * steps_to_mm[E_AXIS + last_extruder]);
+      last_extruder = extruder;
+    }
+  #endif
+
+  // The target position of the tool in absolute steps
+  // Calculate target position in absolute steps
+  const int32_t target[XYZE] = {
+    LROUND(a * axis_steps_per_mm[X_AXIS]),
+    LROUND(b * axis_steps_per_mm[Y_AXIS]),
+    LROUND(c * axis_steps_per_mm[Z_AXIS]),
+    LROUND(e * axis_steps_per_mm[E_AXIS_N])
+  };
+
+  /* <-- add a slash to enable
+    SERIAL_ECHOPAIR("  _buffer_line FR:", fr_mm_s);
+    #if IS_KINEMATIC
+      SERIAL_ECHOPAIR(" A:", a);
+      SERIAL_ECHOPAIR(" (", target[A_AXIS]);
+      SERIAL_ECHOPAIR(" steps) B:", b);
+    #else
+      SERIAL_ECHOPAIR(" X:", a);
+      SERIAL_ECHOPAIR(" (", target[X_AXIS]);
+      SERIAL_ECHOPAIR(" steps) Y:", b);
+    #endif
+    SERIAL_ECHOPAIR(" (", target[Y_AXIS]);
+    #if ENABLED(DELTA)
+      SERIAL_ECHOPAIR(" steps) C:", c);
+    #else
+      SERIAL_ECHOPAIR(" steps) Z:", c);
+    #endif
+    SERIAL_ECHOPAIR(" (", target[Z_AXIS]);
+    SERIAL_ECHOPAIR(" steps) E:", e);
+    SERIAL_ECHOPAIR(" (", target[E_AXIS]);
+    SERIAL_ECHOLNPGM(" steps)");
+  //*/
+
+  // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
+  if (DEBUGGING(DRYRUN))
+    position[E_AXIS] = target[E_AXIS];
+
+  // Always split the first move in two so it can chain
+  if (!blocks_queued()) {
+    DISABLE_STEPPER_DRIVER_INTERRUPT();
+    #define _BETWEEN(A) (position[A##_AXIS] + target[A##_AXIS]) >> 1
+    const int32_t between[XYZE] = { _BETWEEN(X), _BETWEEN(Y), _BETWEEN(Z), _BETWEEN(E) };
+    _buffer_steps(between, fr_mm_s, extruder);
+    _buffer_steps(target, fr_mm_s, extruder);
+    ENABLE_STEPPER_DRIVER_INTERRUPT();
+  }
+  else
+    _buffer_steps(target, fr_mm_s, extruder);
+
   stepper.wake_up();
 
-} // buffer_line()
+} // _buffer_line()
 
 /**
  * Directly set the planner XYZ position (and stepper positions)

Marlin/planner.h

@@ -349,17 +349,28 @@ class Planner {
     #endif
 
     /**
+     * Planner::_buffer_steps
+     *
+     * Add a new linear movement to the buffer (in terms of steps).
+     *
+     *  target    - target position in steps units
+     *  fr_mm_s   - (target) speed of the move
+     *  extruder  - target extruder
+     */
+    static void _buffer_steps(const int32_t target[XYZE], float fr_mm_s, const uint8_t extruder);
+
+    /**
      * Planner::_buffer_line
      *
-     * Add a new direct linear movement to the buffer.
+     * Add a new linear movement to the buffer in axis units.
      *
-     * Leveling and kinematics should be applied ahead of this.
+     * Leveling and kinematics should be applied ahead of calling this.
      *
-     *  a,b,c,e   - target position in mm or degrees
-     *  fr_mm_s   - (target) speed of the move (mm/s)
+     *  a,b,c,e   - target positions in mm and/or degrees
+     *  fr_mm_s   - (target) speed of the move
      *  extruder  - target extruder
      */
-    static void _buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder);
+    static void _buffer_line(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder);
 
     static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
 

Marlin/stepper.cpp

@@ -286,9 +286,6 @@ volatile long Stepper::endstops_trigsteps[XYZ];
 
 // Some useful constants
 
-#define ENABLE_STEPPER_DRIVER_INTERRUPT()  SBI(TIMSK1, OCIE1A)
-#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
-
 /**
  *         __________________________
  *        /|                        |\     _________________         ^

Marlin/stepper.h

@@ -52,6 +52,9 @@
 class Stepper;
 extern Stepper stepper;
 
+#define ENABLE_STEPPER_DRIVER_INTERRUPT()  SBI(TIMSK1, OCIE1A)
+#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
+
 // intRes = intIn1 * intIn2 >> 16
 // uses:
 // r26 to store 0