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

@@ -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;
 }

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.

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