Move Z up before, or down after XY, for "blocking move"

Marlin/Marlin_main.cpp

@@ -1666,18 +1666,25 @@ static void clean_up_after_endstop_or_probe_move() {
 
     #else
 
-      feedrate = homing_feedrate[Z_AXIS];
+      // If Z needs to raise, do it before moving XY
-
+      if (current_position[Z_AXIS] < z) {
-      current_position[Z_AXIS] = z;
+        feedrate = homing_feedrate[Z_AXIS];
-      line_to_current_position();
+        current_position[Z_AXIS] = z;
-      stepper.synchronize();
+        line_to_current_position();
+      }
 
       feedrate = XY_PROBE_FEEDRATE;
-
       current_position[X_AXIS] = x;
       current_position[Y_AXIS] = y;
       line_to_current_position();
 
+      // If Z needs to lower, do it after moving XY
+      if (current_position[Z_AXIS] > z) {
+        feedrate = homing_feedrate[Z_AXIS];
+        current_position[Z_AXIS] = z;
+        line_to_current_position();
+      }
+
     #endif
 
     stepper.synchronize();