MBL Z adjustment on toolchange

Marlin/Marlin_main.cpp

@@ -6748,17 +6748,15 @@ inline void gcode_T(uint8_t tmp_extruder) {
             }
           #endif
 
-        #else // !AUTO_BED_LEVELING_FEATURE
+        #elif ENABLED(MESH_BED_LEVELING)
-
+
-          // The newly-selected extruder is actually at...
+          if (mbl.active()) {
-          for (int i=X_AXIS; i<=Y_AXIS; i++) {
+            float xpos = current_position[X_AXIS] - home_offset[X_AXIS],
-            float diff = hotend_offset[i][tmp_extruder] - hotend_offset[i][active_extruder];
+                  ypos = current_position[Y_AXIS] - home_offset[Y_AXIS];
-            current_position[i] += diff;
+            current_position[Z_AXIS] += mbl.get_z(xpos + xydiff[X_AXIS], ypos + xydiff[Y_AXIS]) - mbl.get_z(xpos, ypos);
-            position_shift[i] += diff; // Offset the coordinate space
-            update_software_endstops((AxisEnum)i);
           }
 
-        #endif // !AUTO_BED_LEVELING_FEATURE
+        #else // no bed leveling
 
           // The newly-selected extruder XY is actually at...
           current_position[X_AXIS] += xydiff[X_AXIS];