diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 0f8b93b0..2dbf9465 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1057,7 +1057,8 @@ void setup()
 #ifdef TMC2130
 	uint8_t silentMode = eeprom_read_byte((uint8_t*)EEPROM_SILENT);
 	if (silentMode == 0xff) silentMode = 0;
-	tmc2130_mode = silentMode?TMC2130_MODE_SILENT:TMC2130_MODE_NORMAL;
+//	tmc2130_mode = silentMode?TMC2130_MODE_SILENT:TMC2130_MODE_NORMAL;
+	tmc2130_mode = TMC2130_MODE_NORMAL;
 	uint8_t crashdet = eeprom_read_byte((uint8_t*)EEPROM_CRASH_DET);
 	if (crashdet)
 	{
@@ -1104,6 +1105,11 @@ void setup()
 #endif //TMC2130
 
 	st_init();    // Initialize stepper, this enables interrupts!
+
+#ifdef TMC2130
+	tmc2130_mode = silentMode?TMC2130_MODE_SILENT:TMC2130_MODE_NORMAL;
+	tmc2130_init();
+#endif //TMC2130
     
 	setup_photpin();
 
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index 58f84cf4..9f87de82 100644
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -260,6 +260,8 @@ void tmc2130_home_enter(uint8_t axes_mask)
 {
 //	printf_P(PSTR("tmc2130_home_enter(axes_mask=0x%02x)\n"), axes_mask);
 #ifdef TMC2130_SG_HOMING
+	if (axes_mask & 0x03) //X or Y
+		tmc2130_wait_standstill_xy(1000);
 	for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) //X Y and Z axes
 	{
 		uint8_t mask = (X_AXIS_MASK << axis);
@@ -283,6 +285,8 @@ void tmc2130_home_exit()
 {
 //	printf_P(PSTR("tmc2130_home_exit sg_homing_axes_mask=0x%02x\n"), sg_homing_axes_mask);
 #ifdef TMC2130_SG_HOMING
+	if (sg_homing_axes_mask & 0x03) //X or Y
+		tmc2130_wait_standstill_xy(1000);
 	if (sg_homing_axes_mask)
 	{
 		for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) //X Y and Z axes
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index c7cf7431..9649e313 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -6137,6 +6137,7 @@ bool lcd_selftest()
 	_result = lcd_selftest_manual_fan_check(0, false);
 #endif //defined(TACH_0)
 	
+
 	if (_result)
 	{
 		_progress = lcd_selftest_screen(0, _progress, 3, true, 2000);
@@ -6163,6 +6164,7 @@ bool lcd_selftest()
 		_result = lcd_selfcheck_check_heater(false);
 	}
 
+
 	if (_result)
 	{
 		//current_position[Z_AXIS] += 15;									//move Z axis higher to avoid false triggering of Z end stop in case that we are very low - just above heatbed
@@ -6416,6 +6418,7 @@ static bool lcd_selfcheck_axis_sg(char axis) {
 
 static bool lcd_selfcheck_axis(int _axis, int _travel)
 {
+//	printf_P(PSTR("lcd_selfcheck_axis %d, %d\n"), _axis, _travel);
 	bool _stepdone = false;
 	bool _stepresult = false;
 	int _progress = 0;
@@ -6429,10 +6432,13 @@ static bool lcd_selfcheck_axis(int _axis, int _travel)
 
 		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
 		st_synchronize();
-
+#ifdef TMC2130
+		if ((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1))
+#else //TMC2130
 		if (((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING) == 1) ||
 			((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING) == 1) ||
 			((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1))
+#endif //TMC2130
 		{
 			if (_axis == 0)
 			{
@@ -6450,6 +6456,7 @@ static bool lcd_selfcheck_axis(int _axis, int _travel)
 			{
 				_stepresult = ((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) ? true : false;
 				_err_endstop = ((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING) == 1) ? 0 : 1;
+	printf_P(PSTR("lcd_selfcheck_axis %d, %d\n"), _stepresult, _err_endstop);
 				/*disable_x();
 				disable_y();
 				disable_z();*/
@@ -6492,6 +6499,7 @@ static bool lcd_selfcheck_axis(int _axis, int _travel)
 		if (_err_endstop == 1) _error_2 = "Y";
 		if (_err_endstop == 2) _error_2 = "Z";
 
+
 		if (_travel_done >= _travel)
 		{
 			lcd_selftest_error(5, _error_1, _error_2);
diff --git a/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h b/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h
index a74750b6..b128f0f8 100644
--- a/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h
@@ -154,6 +154,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 //#define DEBUG_DISABLE_LCD_STATUS_LINE  //empty four lcd line
 //#define DEBUG_DISABLE_PREVENT_EXTRUDER //cold extrusion and long extrusion allowed
 //#define DEBUG_DISABLE_PRUSA_STATISTICS //disable prusa_statistics() mesages
+//#define DEBUG_DISABLE_FORCE_SELFTEST //disable force selftest
 //#define DEBUG_XSTEP_DUP_PIN 21   //duplicate x-step output to pin 21 (SCL on P3)
 //#define DEBUG_YSTEP_DUP_PIN 21   //duplicate y-step output to pin 21 (SCL on P3)
 //#define DEBUG_BLINK_ACTIVE