From 8190483eeb94151b4d9a1d0d48d132de5b285059 Mon Sep 17 00:00:00 2001
From: Sebastianv650 <sebastian_v650@kabelmail.de>
Date: Mon, 21 Nov 2016 17:49:16 +0100
Subject: [PATCH] Improvement for ENSURE_SMOOTH_MOVES

Instead of waiting for a single long block, compare the complete block
buffer runtime for the long_move() check.
---
 Marlin/planner.cpp |  5 +++++
 Marlin/planner.h   | 13 ++++++++++++-
 Marlin/stepper.cpp |  3 +++
 3 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index c9d681e9be..2de5f3fb7f 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -136,6 +136,10 @@ float Planner::previous_speed[NUM_AXIS],
   float Planner::position_float[NUM_AXIS] = { 0 };
 #endif
 
+#if ENABLED(ENSURE_SMOOTH_MOVES)
+  uint32_t Planner::block_buffer_runtime_us = 0;
+#endif
+
 /**
  * Class and Instance Methods
  */
@@ -954,6 +958,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
       segment_time = (MIN_BLOCK_TIME) * 1000UL;
     }
     block->segment_time = segment_time;
+    block_buffer_runtime_us += segment_time;
   #endif
 
   block->nominal_speed = block->millimeters * inverse_mm_s; // (mm/sec) Always > 0
diff --git a/Marlin/planner.h b/Marlin/planner.h
index 3121f19431..2cbdaa1153 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -206,6 +206,10 @@ class Planner {
       static float extruder_advance_k;
     #endif
 
+    #if ENABLED(ENSURE_SMOOTH_MOVES)
+      static uint32_t block_buffer_runtime_us; //Theoretical block buffer runtime in µs
+    #endif
+
   public:
 
     /**
@@ -363,6 +367,9 @@ class Planner {
     static block_t* get_current_block() {
       if (blocks_queued()) {
         block_t* block = &block_buffer[block_buffer_tail];
+        #if ENABLED(ENSURE_SMOOTH_MOVES)
+          block_buffer_runtime_us -= block->segment_time; //We can't be sure how long an active block will take, so don't count it.
+        #endif
         SBI(block->flag, BLOCK_BIT_BUSY);
         return block;
       }
@@ -374,11 +381,15 @@ class Planner {
       static bool long_move() {
         if (blocks_queued()) {
           block_t* block = &block_buffer[block_buffer_tail];
-          return block->segment_time > (LCD_UPDATE_THRESHOLD) * 1000UL;
+          return block_buffer_runtime_us > (LCD_UPDATE_THRESHOLD) * 1000UL + (MIN_BLOCK_TIME) * 3000UL;
         }
         else
           return true;
       }
+      
+      static void clear_block_buffer_runtime(){
+        block_buffer_runtime_us = 0;
+      }
     #endif
 
     #if ENABLED(AUTOTEMP)
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 974dbad0e9..bba001755b 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -1067,6 +1067,9 @@ void Stepper::finish_and_disable() {
 
 void Stepper::quick_stop() {
   cleaning_buffer_counter = 5000;
+  #if ENABLED(ENSURE_SMOOTH_MOVES)
+    planner.clear_block_buffer_runtime();
+  #endif
   DISABLE_STEPPER_DRIVER_INTERRUPT();
   while (planner.blocks_queued()) planner.discard_current_block();
   current_block = NULL;