From 574153a8107bc616cc6c0ea20f4d43df7a6dc953 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 6 Dec 2017 16:19:11 -0600
Subject: [PATCH] No lcd_map_control needed in quick_stop

---
 Marlin/stepper.cpp  |  7 +------
 Marlin/stepper.h    |  3 ++-
 Marlin/ultralcd.cpp | 26 +++++++++++++++++---------
 3 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 6bc1602fc..96872450a 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -1224,12 +1224,7 @@ void Stepper::finish_and_disable() {
 }
 
 void Stepper::quick_stop() {
-  #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(ULTIPANEL)
-    if (!ubl.lcd_map_control)
-      cleaning_buffer_counter = 5000;
-  #else
-    cleaning_buffer_counter = 5000;
-  #endif
+  cleaning_buffer_counter = 5000;
   DISABLE_STEPPER_DRIVER_INTERRUPT();
   while (planner.blocks_queued()) planner.discard_current_block();
   current_block = NULL;
diff --git a/Marlin/stepper.h b/Marlin/stepper.h
index a19810376..56a2537b0 100644
--- a/Marlin/stepper.h
+++ b/Marlin/stepper.h
@@ -101,10 +101,11 @@ class Stepper {
       static uint32_t motor_current_setting[3];
     #endif
 
+    static int16_t cleaning_buffer_counter;
+
   private:
 
     static uint8_t last_direction_bits;        // The next stepping-bits to be output
-    static int16_t cleaning_buffer_counter;
 
     #if ENABLED(X_DUAL_ENDSTOPS)
       static bool locked_x_motor, locked_x2_motor;
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 3add5adae..a20a7b37f 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -559,7 +559,7 @@ uint16_t max_display_update_time = 0;
     if (no_reentry) return;
     // Make this the current handler till all moves are done
     no_reentry = true;
-    screenFunc_t old_screen = currentScreen;
+    const screenFunc_t old_screen = currentScreen;
     lcd_goto_screen(_lcd_synchronize);
     stepper.synchronize();
     no_reentry = false;
@@ -2336,6 +2336,19 @@ void kill_screen(const char* lcd_msg) {
     void set_current_from_steppers_for_axis(const AxisEnum axis);
     void sync_plan_position();
 
+    void _lcd_do_nothing() {}
+    void _lcd_hard_stop() {
+      stepper.quick_stop();
+      const screenFunc_t old_screen = currentScreen;
+      currentScreen = _lcd_do_nothing;
+      while (planner.movesplanned()) idle();
+      currentScreen = old_screen;
+      stepper.cleaning_buffer_counter = 0;
+      set_current_from_steppers_for_axis(ALL_AXES);
+      sync_plan_position();
+      refresh_cmd_timeout();
+    }
+
     void _lcd_ubl_output_map_lcd() {
       static int16_t step_scaler = 0;
 
@@ -2380,15 +2393,10 @@ void kill_screen(const char* lcd_msg) {
       if (lcdDrawUpdate) {
         lcd_implementation_ubl_plot(x_plot, y_plot);
 
-        ubl_map_move_to_xy(); // Move to current location
+        if (planner.movesplanned()) // If the nozzle is already moving, cancel the move.
+          _lcd_hard_stop();
 
-        if (planner.movesplanned() > 1) { // if the nozzle is moving, cancel the move.  There is a new location
-          stepper.quick_stop();
-          set_current_from_steppers_for_axis(ALL_AXES);
-          sync_plan_position();
-          ubl_map_move_to_xy(); // Move to new location
-          refresh_cmd_timeout();
-        }
+        ubl_map_move_to_xy();       // Move to new location
       }
     }