Prevent Z misaligment on tool change (#16518)

Marlin/src/module/tool_change.cpp

@@ -929,11 +929,20 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
       #elif ENABLED(SWITCHING_NOZZLE) && !SWITCHING_NOZZLE_TWO_SERVOS   // Switching Nozzle (single servo)
         // Raise by a configured distance to avoid workpiece, except with
         // SWITCHING_NOZZLE_TWO_SERVOS, as both nozzles will lift instead.
-        current_position.z += _MAX(-diff.z, 0.0) + toolchange_settings.z_raise;
-        #if HAS_SOFTWARE_ENDSTOPS
-          NOMORE(current_position.z, soft_endstop.max.z);
-        #endif
-        if (!no_move) fast_line_to_current(Z_AXIS);
+        if (!no_move) {
+          #if HAS_SOFTWARE_ENDSTOPS
+            const float maxz = _MIN(soft_endstop.max.z, Z_MAX_POS);
+          #else
+            constexpr float maxz = Z_MAX_POS;
+          #endif
+
+          // Check if Z has space to compensate at least z_offset, and if not, just abort now
+          const float newz = current_position.z + _MAX(-diff.z, 0.0);
+          if (newz > maxz) return;
+
+          current_position.z = _MIN(newz + toolchange_settings.z_raise, maxz);
+          fast_line_to_current(Z_AXIS);
+        }
         move_nozzle_servo(new_tool);
       #endif
 
@@ -942,7 +951,7 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
       #endif
 
       // The newly-selected extruder XYZ is actually at...
-      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Offset Tool XY by { ", diff.x, ", ", diff.y, ", ", diff.z, " }");
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Offset Tool XYZ by { ", diff.x, ", ", diff.y, ", ", diff.z, " }");
       current_position += diff;
 
       // Tell the planner the new "current position"