From 118ac61e596b18a8751ff1388a69be4adcc7e4a8 Mon Sep 17 00:00:00 2001
From: GMagician <GMagician@users.noreply.github.com>
Date: Wed, 22 Nov 2017 23:44:25 +0100
Subject: [PATCH] [2.0.x] Tool change bad behaviour fix

Previous fix did not work as expected when axeis not homed. It raise Z but never return to original position
---
 Marlin/src/module/tool_change.cpp | 16 ++++++----------
 1 file changed, 6 insertions(+), 10 deletions(-)

diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
index 706ff433ab..9646f972dc 100644
--- a/Marlin/src/module/tool_change.cpp
+++ b/Marlin/src/module/tool_change.cpp
@@ -493,11 +493,10 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
         SYNC_PLAN_POSITION_KINEMATIC();
 
         // Move to the "old position" (move the extruder into place)
+        #if ENABLED(SWITCHING_NOZZLE)
+          destination[Z_AXIS] += z_diff;  // Include the Z restore with the "move back"
+        #endif
         if (!no_move && IsRunning()) {
-          #if ENABLED(SWITCHING_NOZZLE)
-            if (z_raise != z_diff)
-              destination[Z_AXIS] += z_diff;  // Include the Z restore with the "move back"
-          #endif
           #if ENABLED(DEBUG_LEVELING_FEATURE)
             if (DEBUGGING(LEVELING)) DEBUG_POS("Move back", destination);
           #endif
@@ -505,12 +504,9 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
           do_blocking_move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS]);
         }
         #if ENABLED(SWITCHING_NOZZLE)
-          // Move back down, if needed. (Including when the new tool is higher.)
-          else if (z_raise != z_diff) {
-            set_destination_from_current(); // Prevent any XY move
-            destination[Z_AXIS] += z_diff;
-            feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS];
-            prepare_move_to_destination();
+          else {
+            // Move back down. (Including when the new tool is higher.)
+            do_blocking_move_to_z(destination[Z_AXIS], planner.max_feedrate_mm_s[Z_AXIS]);
           }
         #endif
       } // (tmp_extruder != active_extruder)