diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index a0b3c19b8..7a1b85a2b 100755
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -310,9 +310,9 @@ extern int8_t lcd_change_fil_state;
 extern float default_retraction;
 
 #ifdef TMC2130
-void homeaxis(int axis, uint8_t cnt = 1, uint8_t* pstep = 0);
+bool homeaxis(int axis, bool doError = true, uint8_t cnt = 1, uint8_t* pstep = 0);
 #else
-void homeaxis(int axis, uint8_t cnt = 1);
+bool homeaxis(int axis, bool doError = true, uint8_t cnt = 1);
 #endif //TMC2130
 
 
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 220c3611e..24491c876 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1892,10 +1892,6 @@ static void axis_is_at_home(int axis) {
   max_pos[axis] =          base_max_pos(axis) + cs.add_homing[axis];
 }
 
-
-inline void set_current_to_destination() { memcpy(current_position, destination, sizeof(current_position)); }
-inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
-
 //! @return original feedmultiply
 static int setup_for_endstop_move(bool enable_endstops_now = true) {
     saved_feedrate = feedrate;
@@ -2191,9 +2187,9 @@ bool calibrate_z_auto()
 #endif //TMC2130
 
 #ifdef TMC2130
-void homeaxis(int axis, uint8_t cnt, uint8_t* pstep)
+bool homeaxis(int axis, bool doError, uint8_t cnt, uint8_t* pstep)
 #else
-void homeaxis(int axis, uint8_t cnt)
+bool homeaxis(int axis, bool doError, uint8_t cnt)
 #endif //TMC2130
 {
 	bool endstops_enabled  = enable_endstops(true); //RP: endstops should be allways enabled durring homing
@@ -2308,8 +2304,10 @@ void homeaxis(int axis, uint8_t cnt)
 #ifdef TMC2130
 		if (READ(Z_TMC2130_DIAG) != 0) { //Z crash
 			FORCE_HIGH_POWER_END;
-			kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
-			return; 
+			if (doError) kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
+            current_position[axis] = -5; //assume that nozzle crashed into bed
+            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+			return 0; 
 		}
 #endif //TMC2130
         current_position[axis] = 0;
@@ -2324,8 +2322,10 @@ void homeaxis(int axis, uint8_t cnt)
 #ifdef TMC2130
 		if (READ(Z_TMC2130_DIAG) != 0) { //Z crash
 			FORCE_HIGH_POWER_END;
-			kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
-			return; 
+			if (doError) kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
+            current_position[axis] = -5; //assume that nozzle crashed into bed
+            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+			return 0; 
 		}
 #endif //TMC2130
         axis_is_at_home(axis);
@@ -2338,6 +2338,7 @@ void homeaxis(int axis, uint8_t cnt)
 #endif	
     }
     enable_endstops(endstops_enabled);
+    return 1;
 }
 
 /**/
diff --git a/Firmware/planner.h b/Firmware/planner.h
index 23509acfc..2adcf94e7 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -173,6 +173,9 @@ void plan_set_e_position(const float &e);
 // Reset the E position to zero at the start of the next segment
 void plan_reset_next_e();
 
+inline void set_current_to_destination() { memcpy(current_position, destination, sizeof(current_position)); }
+inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
+
 extern bool e_active();
 
 void check_axes_activity();
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index 108d00b1d..3a4e5b51e 100755
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -994,7 +994,7 @@ bool tmc2130_home_calibrate(uint8_t axis)
 	uint8_t step[16];
 	uint8_t cnt[16];
 	uint8_t val[16];
-	homeaxis(axis, 16, step);
+	homeaxis(axis, true, 16, step);
 	bubblesort_uint8(step, 16, 0);
 	printf_P(PSTR("sorted samples:\n"));
 	for (uint8_t i = 0; i < 16; i++)
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index c803528f7..40d7ef3ac 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -7727,21 +7727,31 @@ bool lcd_selftest()
 #ifdef TMC2130
 		tmc2130_home_exit();
 		enable_endstops(false);
-		current_position[X_AXIS] = current_position[X_AXIS] + 14;
-		current_position[Y_AXIS] = current_position[Y_AXIS] + 12;
 #endif
 
 		//homeaxis(X_AXIS);
 		//homeaxis(Y_AXIS);
+        current_position[X_AXIS] += pgm_read_float(bed_ref_points_4);
+		current_position[Y_AXIS] += pgm_read_float(bed_ref_points_4+1);
+#ifdef TMC2130
+		//current_position[X_AXIS] += 0;
+		current_position[Y_AXIS] += 4;
+#endif //TMC2130
 		current_position[Z_AXIS] = current_position[Z_AXIS] + 10;
 		plan_buffer_line_curposXYZE(manual_feedrate[0] / 60, active_extruder);
 		st_synchronize();
+        set_destination_to_current();
 		_progress = lcd_selftest_screen(TestScreen::AxisZ, _progress, 3, true, 1500);
-		_result = lcd_selfcheck_axis(2, Z_MAX_POS);
-		if (eeprom_read_byte((uint8_t*)EEPROM_WIZARD_ACTIVE) != 1) {
-			enquecommand_P(PSTR("G28 W"));
-			enquecommand_P(PSTR("G1 Z15 F1000"));
-		}
+#ifdef TMC2130
+		_result = homeaxis(Z_AXIS, false);
+#else
+        _result = lcd_selfcheck_axis(Z_AXIS, Z_MAX_POS);
+#endif //TMC2130
+
+		//raise Z to not damage the bed during and hotend testing
+		current_position[Z_AXIS] += 20;
+		plan_buffer_line_curposXYZE(manual_feedrate[0] / 60, active_extruder);
+		st_synchronize();
 	}
 
 #ifdef TMC2130
@@ -7965,7 +7975,7 @@ static bool lcd_selfcheck_axis_sg(unsigned char axis) {
 }
 #endif //TMC2130
 
-//#ifndef TMC2130
+#ifndef TMC2130
 
 static bool lcd_selfcheck_axis(int _axis, int _travel)
 {
@@ -8064,12 +8074,13 @@ static bool lcd_selfcheck_axis(int _axis, int _travel)
 		{
 			lcd_selftest_error(TestError::Motor, _error_1, _error_2);
 		}
-	}
+	}    
+	current_position[_axis] = 0; //simulate axis home to avoid negative numbers for axis position, especially Z.
+	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 
 	return _stepresult;
 }
 
-#ifndef TMC2130
 static bool lcd_selfcheck_pulleys(int axis)
 {
 	float tmp_motor_loud[3] = DEFAULT_PWM_MOTOR_CURRENT_LOUD;
@@ -8114,9 +8125,6 @@ static bool lcd_selfcheck_pulleys(int axis)
 			((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING) == 1)) {
 			endstop_triggered = true;
 			if (current_position_init - 1 <= current_position[axis] && current_position_init + 1 >= current_position[axis]) {
-				current_position[axis] += (axis == X_AXIS) ? 13 : 9;
-				plan_buffer_line_curposXYZE(manual_feedrate[0] / 60, active_extruder);
-				st_synchronize();
 				return(true);
 			}
 			else {