From a5d6f6ac9880090d2412bbea0027885b95c98ea2 Mon Sep 17 00:00:00 2001
From: espr14 <espr14@gmail.com>
Date: Tue, 13 Apr 2021 03:10:21 +0200
Subject: [PATCH] Fix Resume Print with UBL (#21564)

Co-authored-by: Scott Lahteine <github@thinkyhead.com>
---
 Marlin/src/feature/pause.cpp                    | 10 ++++++----
 Marlin/src/gcode/feature/pause/M701_M702.cpp    | 17 ++++++++++++-----
 .../lcd/extui/lib/dgus/mks/DGUSDisplayDef.cpp   | 13 +++++++++++--
 Marlin/src/module/tool_change.cpp               |  5 +++--
 4 files changed, 32 insertions(+), 13 deletions(-)

diff --git a/Marlin/src/feature/pause.cpp b/Marlin/src/feature/pause.cpp
index 5990c09a90..8a1701d0b6 100644
--- a/Marlin/src/feature/pause.cpp
+++ b/Marlin/src/feature/pause.cpp
@@ -597,11 +597,13 @@ void resume_print(const_float_t slow_load_length/*=0*/, const_float_t fast_load_
   unscaled_e_move(-(PAUSE_PARK_RETRACT_LENGTH), feedRate_t(PAUSE_PARK_RETRACT_FEEDRATE));
 
   if (!axes_should_home()) {
-    // Move XY to starting position, then Z
-    do_blocking_move_to_xy(resume_position, feedRate_t(NOZZLE_PARK_XY_FEEDRATE));
+    // Move XY back to saved position
+    destination.set(resume_position.x, resume_position.y, current_position.z);
+    prepare_internal_move_to_destination(NOZZLE_PARK_XY_FEEDRATE);
 
-    // Move Z_AXIS to saved position
-    do_blocking_move_to_z(resume_position.z, feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+    // Move Z back to saved position
+    destination.z = resume_position.z;
+    prepare_internal_move_to_destination(NOZZLE_PARK_Z_FEEDRATE);
   }
 
   // Unretract
diff --git a/Marlin/src/gcode/feature/pause/M701_M702.cpp b/Marlin/src/gcode/feature/pause/M701_M702.cpp
index 9a2b774936..6be63346dc 100644
--- a/Marlin/src/gcode/feature/pause/M701_M702.cpp
+++ b/Marlin/src/gcode/feature/pause/M701_M702.cpp
@@ -88,9 +88,17 @@ void GcodeSuite::M701() {
       tool_change(target_extruder, false);
   #endif
 
-  // Lift Z axis
-  if (park_point.z > 0)
-    do_blocking_move_to_z(_MIN(current_position.z + park_point.z, Z_MAX_POS), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+  auto move_z_by = [](const_float_t zdist) {
+    if (zdist) {
+      destination = current_position;
+      destination.z += zdist;
+      prepare_internal_move_to_destination(NOZZLE_PARK_Z_FEEDRATE);
+    }
+  };
+
+  // Raise the Z axis (with max limit)
+  const float park_raise = _MIN(0, park_point.z, (Z_MAX_POS) - current_position.z);
+  move_z_by(park_raise);
 
   // Load filament
   #if HAS_PRUSA_MMU2
@@ -113,8 +121,7 @@ void GcodeSuite::M701() {
   #endif
 
   // Restore Z axis
-  if (park_point.z > 0)
-    do_blocking_move_to_z(_MAX(current_position.z - park_point.z, 0), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+  move_z_by(-park_raise);
 
   #if HAS_MULTI_EXTRUDER && (HAS_PRUSA_MMU1 || !HAS_MMU)
     // Restore toolhead if it was changed
diff --git a/Marlin/src/lcd/extui/lib/dgus/mks/DGUSDisplayDef.cpp b/Marlin/src/lcd/extui/lib/dgus/mks/DGUSDisplayDef.cpp
index 911965b9ae..01c3adbda7 100644
--- a/Marlin/src/lcd/extui/lib/dgus/mks/DGUSDisplayDef.cpp
+++ b/Marlin/src/lcd/extui/lib/dgus/mks/DGUSDisplayDef.cpp
@@ -75,9 +75,18 @@ xyz_pos_t position_before_pause;
 void MKS_pause_print_move() {
   queue.exhaust();
   position_before_pause = current_position;
-  do_blocking_move_to(X_MIN_POS + mks_park_pos.x, Y_MIN_POS + mks_park_pos.y, current_position.z + mks_park_pos.z);
+  destination.z = _MIN(current_position.z + mks_park_pos.z, Z_MAX_POS);
+  prepare_internal_move_to_destination(NOZZLE_PARK_Z_FEEDRATE);
+  destination.set(X_MIN_POS + mks_park_pos.x, Y_MIN_POS + mks_park_pos.y);
+  prepare_internal_move_to_destination(NOZZLE_PARK_XY_FEEDRATE);
+}
+
+void MKS_resume_print_move() {
+  destination.set(position_before_pause.x, position_before_pause.y);
+  prepare_internal_move_to_destination(NOZZLE_PARK_XY_FEEDRATE);
+  destination.z = position_before_pause.z;
+  prepare_internal_move_to_destination(NOZZLE_PARK_Z_FEEDRATE);
 }
-void MKS_resume_print_move() { do_blocking_move_to(position_before_pause); }
 
 float z_offset_add = 0;
 
diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
index 0475af6ad2..3dd63e814c 100644
--- a/Marlin/src/module/tool_change.cpp
+++ b/Marlin/src/module/tool_change.cpp
@@ -836,9 +836,10 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
       #if ENABLED(TOOLCHANGE_PARK)
         if (ok) {
           #if ENABLED(TOOLCHANGE_NO_RETURN)
-            do_blocking_move_to_z(destination.z, planner.settings.max_feedrate_mm_s[Z_AXIS]);
+            destination.set(current_position.x, current_position.y);
+            prepare_internal_move_to_destination(planner.settings.max_feedrate_mm_s[Z_AXIS]);
           #else
-            do_blocking_move_to(destination, MMM_TO_MMS(TOOLCHANGE_PARK_XY_FEEDRATE));
+            prepare_internal_move_to_destination(MMM_TO_MMS(TOOLCHANGE_PARK_XY_FEEDRATE));
           #endif
         }
       #endif