From 6b9bf8e8fe409d59a8b52bb26230742a0c4744b5 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Wed, 21 Sep 2016 17:31:32 -0500
Subject: [PATCH] Add Planner::sync_from_steppers

Use to sync the planner after an interrupted move (when not overriding
the logical position).
---
 Marlin/Marlin_main.cpp | 6 +++---
 Marlin/planner.cpp     | 7 +++++++
 Marlin/planner.h       | 5 +++++
 3 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 39f0ea1e2..138db0e67 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1996,12 +1996,12 @@ static void clean_up_after_endstop_or_probe_move() {
     // Clear endstop flags
     endstops.hit_on_purpose();
 
+    // Tell the planner where we actually are
+    planner.sync_from_steppers();
+
     // Get Z where the steppers were interrupted
     set_current_from_steppers_for_axis(Z_AXIS);
 
-    // Tell the planner where we actually are
-    SYNC_PLAN_POSITION_KINEMATIC();
-
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) DEBUG_POS("<<< do_probe_move", current_position);
     #endif
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index a672cbfc5..c90f5571c 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -1210,6 +1210,13 @@ void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
   LOOP_XYZE(i) previous_speed[i] = 0.0;
 }
 
+/**
+ * Sync from the stepper positions. (e.g., after an interrupted move)
+ */
+void Planner::sync_from_steppers() {
+  LOOP_XYZE(i) position[i] = stepper.position((AxisEnum)i);
+}
+
 /**
  * Directly set the planner E position (hence the stepper E position).
  */
diff --git a/Marlin/planner.h b/Marlin/planner.h
index e38e9e5f0..17408afca 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -242,6 +242,11 @@ class Planner {
      */
     static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float& e);
 
+    /**
+     * Sync from the stepper positions. (e.g., after an interrupted move)
+     */
+    static void sync_from_steppers();
+
     /**
      * Set the E position (mm) of the planner (and the E stepper)
      */