From 7907e14cbf390c35c30f311ed58635683d107a9e Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 23 Aug 2022 17:16:15 +0200
Subject: [PATCH] Resync planner position upon exiting xyzcal

Split the planner sync code out of planner_abort_hard() so that we can
independently resync the planner position from the counters.

This is needed in xyzcal as we directly modify the stepper counters
(bypassing both planner and stepper).

Call this new function instead of planner_abort_hard() when leaving, so
that motion can resume in the middle of the gcode_M45 instruction.
---
 Firmware/planner.cpp | 29 +++++++++++++++++++----------
 Firmware/planner.h   |  3 +++
 Firmware/xyzcal.cpp  |  4 +++-
 3 files changed, 25 insertions(+), 11 deletions(-)

diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index 2b07c77bc..c3f5262cc 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -609,12 +609,8 @@ static inline void planner_update_queue_min_counter()
 
 extern volatile uint32_t step_events_completed; // The number of step events executed in the current block
 
-void planner_abort_hard()
+void planner_reset_position()
 {
-    // Abort the stepper routine and flush the planner queue.
-    DISABLE_STEPPER_DRIVER_INTERRUPT();
-
-    // Now the front-end (the Marlin_main.cpp with its current_position) is out of sync.
     // First update the planner's current position in the physical motor steps.
     position[X_AXIS] = st_get_position(X_AXIS);
     position[Y_AXIS] = st_get_position(Y_AXIS);
@@ -626,6 +622,7 @@ void planner_abort_hard()
     current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
     current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
     current_position[E_AXIS] = st_get_position_mm(E_AXIS);
+
     // Apply the mesh bed leveling correction to the Z axis.
 #ifdef MESH_BED_LEVELING
     if (mbl.active) {
@@ -658,11 +655,6 @@ void planner_abort_hard()
 #endif
     }
 #endif
-    // Relay to planner wait routine, that the current line shall be canceled.
-    planner_aborted = true;
-
-    // Clear the planner queue, reset and re-enable the stepper timer.
-    quickStop();
 
     // Apply inverse world correction matrix.
     machine2world(current_position[X_AXIS], current_position[Y_AXIS]);
@@ -670,10 +662,27 @@ void planner_abort_hard()
 #ifdef LIN_ADVANCE
     memcpy(position_float, current_position, sizeof(position_float));
 #endif
+}
+
+void planner_abort_hard()
+{
+    // Abort the stepper routine and flush the planner queue.
+    DISABLE_STEPPER_DRIVER_INTERRUPT();
+
+    // Now the front-end (the Marlin_main.cpp with its current_position) is out of sync.
+    planner_reset_position();
+
+    // Relay to planner wait routine that the current line shall be canceled.
+    planner_aborted = true;
+
+    // Clear the planner queue, reset and re-enable the stepper timer.
+    quickStop();
+
     // Resets planner junction speeds. Assumes start from rest.
     previous_nominal_speed = 0.0;
     memset(previous_speed, 0, sizeof(previous_speed));
 
+    // Reset position sync requests
     plan_reset_next_e_queue = false;
     plan_reset_next_e_sched = false;
 }
diff --git a/Firmware/planner.h b/Firmware/planner.h
index 75066a80b..8534288ea 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -255,6 +255,9 @@ FORCE_INLINE bool planner_queue_full() {
     return block_buffer_tail == next_block_index;
 }
 
+// Reset machine position from stepper counters
+extern void planner_reset_position();
+
 // Abort the stepper routine, clean up the block queue,
 // wait for the steppers to stop,
 // update planner's current position and the current_position of the front end.
diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp
index b8acc9480..e8689d3b5 100644
--- a/Firmware/xyzcal.cpp
+++ b/Firmware/xyzcal.cpp
@@ -158,7 +158,9 @@ void xyzcal_meassure_enter(void)
 void xyzcal_meassure_leave(void)
 {
 	DBG(_n("xyzcal_meassure_leave\n"));
-    planner_abort_hard();
+
+	// resync planner position from counters (changed by xyzcal_update_pos)
+	planner_reset_position();
 
 	// re-enable interrupts
 #ifdef WATCHDOG