diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
index 706ff433abeebba5d08c59bed2b14c2b773cdbbf..9646f972dcf8b71a31c28c972648f65acf3d46f2 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)