diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 2bf581356..760339220 100755
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -253,6 +253,7 @@ uint16_t restore_interrupted_gcode();
 float __attribute__((noinline)) get_feedrate_mm_s(const float feedrate_mm_min);
 
 #ifdef TMC2130
+void check_Z_crash(void);
 void homeaxis(uint8_t axis, uint8_t cnt = 1, uint8_t* pstep = 0);
 #else
 void homeaxis(uint8_t axis, uint8_t cnt = 1);
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 5d0ca197a..157b97ab7 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -2193,7 +2193,7 @@ bool calibrate_z_auto()
 #endif //TMC2130
 
 #ifdef TMC2130
-static void check_Z_crash(void)
+void check_Z_crash(void)
 {
 	if (!READ(Z_TMC2130_DIAG)) { //Z crash
 		FORCE_HIGH_POWER_END;
@@ -3278,9 +3278,6 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level)
 					final_result = true;
 				}
 			}
-#ifdef TMC2130
-			tmc2130_home_exit();
-#endif
 		}
 		else
 		{
diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp
index b2d9c3819..b1ded7247 100644
--- a/Firmware/mesh_bed_calibration.cpp
+++ b/Firmware/mesh_bed_calibration.cpp
@@ -2838,6 +2838,10 @@ void go_home_with_z_lift()
 // Returns false if the reference values are more than 3mm far away.
 bool sample_mesh_and_store_reference()
 {
+    bool result = false;
+#ifdef TMC2130
+    tmc2130_home_enter(Z_AXIS_MASK);
+#endif
     bool endstops_enabled  = enable_endstops(false);
     bool endstop_z_enabled = enable_z_endstop(false);
 
@@ -2853,30 +2857,25 @@ bool sample_mesh_and_store_reference()
     // Sample Z heights for the mesh bed leveling.
     // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
     {
-        // The first point defines the reference.
+        // Lower Z to the mesh search height with stall detection
+        enable_endstops(true);
         current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
         go_to_current(homing_feedrate[Z_AXIS]/60);
+#ifdef TMC2130
+        check_Z_crash();
+#endif
+        enable_endstops(false);
+
+        // Move XY to first point
         current_position[X_AXIS] = BED_X0;
         current_position[Y_AXIS] = BED_Y0;
         world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
         go_to_current(homing_feedrate[X_AXIS]/60);
         set_destination_to_current();
-        enable_endstops(true);
         homeaxis(Z_AXIS);
-
-#ifdef TMC2130
-		if (!axis_known_position[Z_AXIS] && (!READ(Z_TMC2130_DIAG))) //Z crash
-		{
-			kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
-			return false;
-		}
-#endif //TMC2130
-
-        enable_endstops(false);
 		if (!find_bed_induction_sensor_point_z()) //Z crash or deviation > 50um
 		{
 			kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
-			return false;
 		}
         mbl.set_z(0, 0, current_position[Z_AXIS]);
     }
@@ -2902,7 +2901,6 @@ bool sample_mesh_and_store_reference()
 		if (!find_bed_induction_sensor_point_z()) //Z crash or deviation > 50um
 		{
 			kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
-			return false;
 		}
         // Get cords of measuring point
        
@@ -2921,7 +2919,7 @@ bool sample_mesh_and_store_reference()
             // The span of the Z offsets is extreme. Give up.
             // Homing failed on some of the points.
             SERIAL_PROTOCOLLNPGM("Exreme span of the Z values!");
-            return false;
+            goto end;
         }
     }
 
@@ -2961,9 +2959,14 @@ bool sample_mesh_and_store_reference()
 
     go_home_with_z_lift();
 
+    result = true;
+end:
     enable_endstops(endstops_enabled);
     enable_z_endstop(endstop_z_enabled);
-    return true;
+#ifdef TMC2130
+    tmc2130_home_exit();
+#endif
+    return result;
 }
 
 #ifndef NEW_XYZCAL
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index bf920de4e..30b2a4389 100755
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -553,7 +553,9 @@ static void SetCurrents(const uint8_t axis, const MotorCurrents &curr) {
     };
 
     IHoldRun ihold_irun(iHold, iRun);
-
+#ifdef DEBUG_TMC_CURRENTS
+	printf_P(PSTR("SetCurrents(axis=%u, iHold=%u, iRun=%u, vsense=%u, reg=%08lX)\n"), axis, iHold, iRun, curr.getvSense(), ihold_irun.dw);
+#endif //DEBUG_TMC_CURRENTS
     tmc2130_wr(axis, TMC2130_REG_IHOLD_IRUN, ihold_irun.dw);
 }