From 6277395e1ec9ff57d642ad3f7b2e25890d6eafd6 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 31 May 2016 15:16:21 -0700
Subject: [PATCH] set_current_position_from_planner() after
 stepper.quick_stop()

---
 Marlin/Marlin.h        |  1 +
 Marlin/Marlin_main.cpp | 20 +++++++++++++++++++-
 Marlin/endstops.cpp    |  1 +
 Marlin/ultralcd.cpp    |  1 +
 4 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index d5bd54f077..ee2453615c 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -225,6 +225,7 @@ void ok_to_send();
 
 void reset_bed_level();
 void prepare_move();
+void set_current_position_from_planner();
 void kill(const char*);
 
 #if ENABLED(FILAMENT_RUNOUT_SENSOR)
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 15add97bbf..53053221f2 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -5889,13 +5889,31 @@ inline void gcode_M400() { stepper.synchronize(); }
 
 #endif // FILAMENT_WIDTH_SENSOR
 
+void set_current_position_from_planner() {
+  stepper.synchronize();
+  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+    vector_3 pos = planner.adjusted_position(); // values directly from steppers...
+    current_position[X_AXIS] = pos.x;
+    current_position[Y_AXIS] = pos.y;
+    current_position[Z_AXIS] = pos.z;
+  #else
+    current_position[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
+    current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
+    current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
+  #endif
+  sync_plan_position();                       // ...re-apply to planner position
+}
+
 /**
  * M410: Quickstop - Abort all planned moves
  *
  * This will stop the carriages mid-move, so most likely they
  * will be out of sync with the stepper position after this.
  */
-inline void gcode_M410() { stepper.quick_stop(); }
+inline void gcode_M410() {
+  stepper.quick_stop();
+  set_current_position_from_planner();
+}
 
 
 #if ENABLED(MESH_BED_LEVELING)
diff --git a/Marlin/endstops.cpp b/Marlin/endstops.cpp
index f5dea69766..91f6d31999 100644
--- a/Marlin/endstops.cpp
+++ b/Marlin/endstops.cpp
@@ -187,6 +187,7 @@ void Endstops::report_state() {
         card.sdprinting = false;
         card.closefile();
         stepper.quick_stop();
+        set_current_position_from_planner();
         thermalManager.disable_all_heaters(); // switch off all heaters.
       }
     #endif
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 4a81f45f9d..9080f7e1c0 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -482,6 +482,7 @@ inline void line_to_current(AxisEnum axis) {
 
   static void lcd_sdcard_stop() {
     stepper.quick_stop();
+    set_current_position_from_planner();
     card.sdprinting = false;
     card.closefile();
     print_job_timer.stop();