Planner block HOLD flag

Allows the Stepper ISR to wait until a given block is free for use. Allows Planner to plan the first move, which is split into two.

Marlin/src/module/planner.cpp

   #include "../feature/power.h"
 #endif
 
+// Delay for delivery of first block to the stepper ISR, if the queue contains 2 or
+// fewer movements. The delay is measured in milliseconds, and must be less than 250ms
+#define BLOCK_DELAY_FOR_1ST_MOVE 50
+
 Planner planner;
 
   // public:
 block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
 volatile uint8_t Planner::block_buffer_head,  // Index of the next block to be pushed
                  Planner::block_buffer_tail;  // Index of the busy block, if any
-uint16_t Planner::cleaning_buffer_counter;   // A counter to disable queuing of blocks
+uint16_t Planner::cleaning_buffer_counter;    // A counter to disable queuing of blocks
+uint8_t Planner::delay_before_delivering;     // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
 
 float Planner::max_feedrate_mm_s[XYZE_N],   // Max speeds in mm per second
       Planner::axis_steps_per_mm[XYZE_N],
     bed_level_matrix.set_to_identity();
   #endif
   clear_block_buffer();
+  delay_before_delivering = 0;
 }
 
 #if ENABLED(BEZIER_JERK_CONTROL)
   const bool was_enabled = STEPPER_ISR_ENABLED();
   if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
 
-  if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy.
+  // Don't update variables if block is busy: It is being interpreted by the planner
+  if (!TEST(block->flag, BLOCK_BIT_BUSY)) {
     block->accelerate_until = accelerate_steps;
     block->decelerate_after = accelerate_steps + plateau_steps;
     block->initial_rate = initial_rate;
   // that is why we set head to tail!
   block_buffer_head = block_buffer_tail;
 
+  // Restart the block delay for the first movement - As the queue was
+  // forced to empty, there's no risk the ISR will touch this.
+  delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+
   #if ENABLED(ULTRA_LCD)
     // Clear the accumulated runtime
     clear_block_buffer_runtime();
   // Discard the active block that led to the trigger
   discard_current_block();
 
-  // Discard the CONTINUED block, if any. Note the planner can only queue 1 continued
-  // block after a previous non continued block, as the condition to queue them
-  // is that there are no queued blocks at the time a new block is queued.
-  const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED);
-  if (discard) discard_current_block();
-
   // Reenable stepper ISR if it was enabled
   if (stepper_isr_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
     return true;
   }
 
+  // If this is the first added movement, reload the delay, otherwise, cancel it.
+  if (block_buffer_head == block_buffer_tail) {
+    // If it was the first queued block, restart the 1st block delivery delay, to
+    // give the planner an opportunity to queue more movements and plan them
+    // As there are no queued movements, the Stepper ISR will not touch this
+    // variable, so there is no risk setting this here (but it MUST be done
+    // before the following line!!)
+    delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+  }
+
   // Move buffer head
   block_buffer_head = next_buffer_head;
 
   block->position[C_AXIS] = position[C_AXIS];
   block->position[E_AXIS] = position[E_AXIS];
 
+  // If this is the first added movement, reload the delay, otherwise, cancel it.
+  if (block_buffer_head == block_buffer_tail) {
+    // If it was the first queued block, restart the 1st block delivery delay, to
+    // give the planner an opportunity to queue more movements and plan them
+    // As there are no queued movements, the Stepper ISR will not touch this
+    // variable, so there is no risk setting this here (but it MUST be done
+    // before the following line!!)
+    delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+  }
+
   block_buffer_head = next_buffer_head;
+
   stepper.wake_up();
 } // buffer_sync_block()
 
     SERIAL_ECHOLNPGM(")");
   //*/
 
-  // Always split the first move into two (if not homing or probing)
-  if (!has_blocks_queued()) {
-
-    #define _BETWEEN(A) (position[_AXIS(A)] + target[_AXIS(A)]) >> 1
-    const int32_t between[ABCE] = { _BETWEEN(A), _BETWEEN(B), _BETWEEN(C), _BETWEEN(E) };
-
-    #if HAS_POSITION_FLOAT
-      #define _BETWEEN_F(A) (position_float[_AXIS(A)] + target_float[_AXIS(A)]) * 0.5
-      const float between_float[ABCE] = { _BETWEEN_F(A), _BETWEEN_F(B), _BETWEEN_F(C), _BETWEEN_F(E) };
-    #endif
-
-    // The new head value is not assigned yet
-    uint8_t buffer_head = 0;
-    bool added = false;
-
-    uint8_t next_buffer_head;
-    block_t *block = get_next_free_block(next_buffer_head, 2);
-
-    // Fill the block with the specified movement
+  // Queue the movement
     if (
-      _populate_block(block, true, between
-        #if HAS_POSITION_FLOAT
-          , between_float
-        #endif
-        , fr_mm_s, extruder, millimeters * 0.5
-      )
-    ) {
-      // Movement accepted - Point to the next reserved block
-      block = &block_buffer[next_buffer_head];
-
-      // Store into the new to be stored head
-      buffer_head = next_buffer_head;
-      added = true;
-
-      // And advance the pointer to the next unused slot
-      next_buffer_head = next_block_index(next_buffer_head);
-    }
-
-    // Fill the second part of the block with the 2nd part of the movement
-    if (
-      _populate_block(block, true, target
-        #if HAS_POSITION_FLOAT
-          , target_float
-        #endif
-        , fr_mm_s, extruder, millimeters * 0.5
-      )
-    ) {
-      // Movement accepted - If this block is a continuation
-      // of the previous one, mark it as such
-      if (added) SBI(block->flag, BLOCK_BIT_CONTINUED);
-
-      // Store into the new to be stored head
-      buffer_head = next_buffer_head;
-      added = true;
-    }
-
-    // If any of the movements was added
-    if (added) {
-
-      // Move buffer head and add all the blocks that were filled
-      // successfully to the movement queue.
-      block_buffer_head = buffer_head;
-
-      // Update the position (only when a move was queued)
-      static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
-      COPY(position, target);
-      #if HAS_POSITION_FLOAT
-        COPY(position_float, target_float);
-      #endif
-
-      // Recalculate and optimize trapezoidal speed profiles
-      recalculate();
-    }
-  }
-  else if (
     !_buffer_steps(target
       #if HAS_POSITION_FLOAT
         , target_float

Marlin/src/module/planner.h

   // from a safe speed (in consideration of jerking from zero speed).
   BLOCK_BIT_NOMINAL_LENGTH,
 
-  // The block is busy
+  // The block is busy, being interpreted by the stepper ISR
   BLOCK_BIT_BUSY,
 
   // The block is segment 2+ of a longer move
     static block_t block_buffer[BLOCK_BUFFER_SIZE];
     static volatile uint8_t block_buffer_head,      // Index of the next block to be pushed
                             block_buffer_tail;      // Index of the busy block, if any
-    static int16_t cleaning_buffer_counter;         // A counter to disable queuing of blocks
+    static uint16_t cleaning_buffer_counter;        // A counter to disable queuing of blocks
+    static uint8_t delay_before_delivering;         // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
 
     #if ENABLED(DISTINCT_E_FACTORS)
       static uint8_t last_extruder;                 // Respond to extruder change
      * WARNING: Called from Stepper ISR context!
      */
     static block_t* get_current_block() {
-      if (has_blocks_queued()) {
-        block_t * const block = &block_buffer[block_buffer_tail];
 
-        // If the block has no trapezoid calculated, it's unsafe to execute.
-        if (movesplanned() > 1) {
-          const block_t * const next = &block_buffer[next_block_index(block_buffer_tail)];
-          if (TEST(block->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE))
-            return NULL;
+      // Get the number of moves in the planner queue so far
+      uint8_t nr_moves = movesplanned();
+
+      // If there are any moves queued ...
+      if (nr_moves) {
+
+        // If there is still delay of delivery of blocks running, decrement it
+        if (delay_before_delivering) {
+          --delay_before_delivering;
+          // If the number of movements queued is less than 3, and there is still time
+          //  to wait, do not deliver anything
+          if (nr_moves < 3 && delay_before_delivering) return NULL;
+          delay_before_delivering = 0;
         }
-        else if (TEST(block->flag, BLOCK_BIT_RECALCULATE))
-          return NULL;
+
+        // If we are here, there is no excuse to deliver the block
+        block_t * const block = &block_buffer[block_buffer_tail];
+
+        // No trapezoid calculated? Don't execute yet.
+        if ( TEST(block->flag, BLOCK_BIT_RECALCULATE)
+          || (movesplanned() > 1 && TEST(block_buffer[next_block_index(block_buffer_tail)].flag, BLOCK_BIT_RECALCULATE))
+        ) return NULL;
 
         #if ENABLED(ULTRA_LCD)
           block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
         #endif
+
+        // Mark the block as busy, so the planner does not attempt to replan it
         SBI(block->flag, BLOCK_BIT_BUSY);
         return block;
       }
       else {
+        // The queue became empty
         #if ENABLED(ULTRA_LCD)
           clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
         #endif