diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 6e2f5b61..a192372f 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1992,6 +1992,39 @@ inline void gcode_M900() {
     }
 #endif // LIN_ADVANCE
 
+#ifdef TMC2130
+bool calibrate_z_auto()
+{
+	lcd_display_message_fullscreen_P(MSG_CALIBRATE_Z_AUTO);
+	bool endstops_enabled  = enable_endstops(true);
+	int axis_up_dir = -home_dir(Z_AXIS);
+	tmc2130_home_enter(Z_AXIS_MASK);
+	current_position[Z_AXIS] = 0;
+	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+	set_destination_to_current();
+	destination[Z_AXIS] += (1.1 * max_length(Z_AXIS) * axis_up_dir);
+	feedrate = homing_feedrate[Z_AXIS];
+	plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+	tmc2130_home_restart(Z_AXIS);
+	st_synchronize();
+//	current_position[axis] = 0;
+//	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+	tmc2130_home_exit();
+    enable_endstops(false);
+	current_position[Z_AXIS] = 0;
+	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+	set_destination_to_current();
+	destination[Z_AXIS] += 10 * axis_up_dir; //10mm up
+	feedrate = homing_feedrate[Z_AXIS] / 2;
+	plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+	st_synchronize();
+    enable_endstops(endstops_enabled);
+    current_position[Z_AXIS] = Z_MAX_POS-3.f;
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+	return true;
+}
+#endif //TMC2130
+
 void homeaxis(int axis)
 {
 	bool endstops_enabled  = enable_endstops(true); //RP: endstops should be allways enabled durring homming
@@ -3858,10 +3891,15 @@ void process_commands()
         // Home in the XY plane.
         //set_destination_to_current();
         setup_for_endstop_move();
-        home_xy();
+		lcd_display_message_fullscreen_P(MSG_AUTO_HOME);
+		home_xy();
 
         // Let the user move the Z axes up to the end stoppers.
+#ifdef TMC2130
+        if (calibrate_z_auto()) {
+#else //TMC2130
         if (lcd_calibrate_z_end_stop_manual( onlyZ )) {
+#endif //TMC2130
             refresh_cmd_timeout();
 			if (((degHotend(0) > MAX_HOTEND_TEMP_CALIBRATION) || (degBed() > MAX_BED_TEMP_CALIBRATION)) && (!onlyZ)) {
 				lcd_wait_for_cool_down();
@@ -5696,12 +5734,15 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 
 	case 916: // M916 Set sg_thrs
     {
-		if (code_seen('X')) sg_thrs_x = code_value();
-		if (code_seen('Y')) sg_thrs_y = code_value();
-		MYSERIAL.print("sg_thrs_x=");
-		MYSERIAL.print(sg_thrs_x, DEC);
-		MYSERIAL.print(" sg_thrs_y=");
-		MYSERIAL.println(sg_thrs_y, DEC);
+		if (code_seen('X')) tmc2131_axis_sg_thr[X_AXIS] = code_value();
+		if (code_seen('Y')) tmc2131_axis_sg_thr[Y_AXIS] = code_value();
+		if (code_seen('Z')) tmc2131_axis_sg_thr[Z_AXIS] = code_value();
+		MYSERIAL.print("tmc2131_axis_sg_thr[X]=");
+		MYSERIAL.print(tmc2131_axis_sg_thr[X_AXIS], DEC);
+		MYSERIAL.print("tmc2131_axis_sg_thr[Y]=");
+		MYSERIAL.print(tmc2131_axis_sg_thr[Y_AXIS], DEC);
+		MYSERIAL.print("tmc2131_axis_sg_thr[Z]=");
+		MYSERIAL.print(tmc2131_axis_sg_thr[Z_AXIS], DEC);
     }
     break;
 
@@ -5993,17 +6034,18 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 		}
 		break;
 	case 3:
-		MYSERIAL.print("fsensor_enable()");
+		calibrate_z_auto();
+/*		MYSERIAL.print("fsensor_enable()");
 #ifdef PAT9125
 		fsensor_enable();
-#endif
+#endif*/
 		break;
 	case 4:
-		MYSERIAL.print("fsensor_disable()");
+/*		MYSERIAL.print("fsensor_disable()");
 #ifdef PAT9125
 		fsensor_disable();
 #endif            
-		break;
+		break;*/
 	case 5:
 		{
 /*			MYSERIAL.print("tmc2130_rd_MSCNT(0)=");
diff --git a/Firmware/language_all.cpp b/Firmware/language_all.cpp
index 6489ee20..3a85906d 100644
--- a/Firmware/language_all.cpp
+++ b/Firmware/language_all.cpp
@@ -1096,6 +1096,21 @@ const char * const MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION_LANG_TABLE[LANG_NUM] P
 	MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION_EN
 };
 
+const char MSG_CALIBRATE_Z_AUTO_EN[] PROGMEM = "Calibrating Z...";
+const char MSG_CALIBRATE_Z_AUTO_CZ[] PROGMEM = "Calibrating Z...";
+const char MSG_CALIBRATE_Z_AUTO_IT[] PROGMEM = "Calibrating Z...";
+const char MSG_CALIBRATE_Z_AUTO_ES[] PROGMEM = "Calibrating Z...";
+const char MSG_CALIBRATE_Z_AUTO_PL[] PROGMEM = "Calibrating Z...";
+const char MSG_CALIBRATE_Z_AUTO_DE[] PROGMEM = "Calibrating Z...";
+const char * const MSG_CALIBRATE_Z_AUTO_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_CALIBRATE_Z_AUTO_EN,
+	MSG_CALIBRATE_Z_AUTO_CZ,
+	MSG_CALIBRATE_Z_AUTO_IT,
+	MSG_CALIBRATE_Z_AUTO_ES,
+	MSG_CALIBRATE_Z_AUTO_PL,
+	MSG_CALIBRATE_Z_AUTO_DE
+};
+
 const char MSG_FIND_BED_OFFSET_AND_SKEW_LINE1_EN[] PROGMEM = "Searching bed calibration point";
 const char MSG_FIND_BED_OFFSET_AND_SKEW_LINE1_CZ[] PROGMEM = "Hledam kalibracni bod podlozky";
 const char MSG_FIND_BED_OFFSET_AND_SKEW_LINE1_IT[] PROGMEM = "Ricerca del letto punto di calibraz.";
@@ -2848,35 +2863,35 @@ const char * const MSG_SHOW_END_STOPS_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_SHOW_END_STOPS_DE
 };
 
-const char MSG_FSENSOR_OFF_EN[] PROGMEM = "Filam. probe [off]";
-const char MSG_FSENSOR_OFF_CZ[] PROGMEM = "Filam. probe [off]";
-const char MSG_FSENSOR_OFF_IT[] PROGMEM = "Filam. probe [off]";
-const char MSG_FSENSOR_OFF_ES[] PROGMEM = "Filam. probe [off]";
-const char MSG_FSENSOR_OFF_PL[] PROGMEM = "Filam. probe [off]";
-const char MSG_FSENSOR_OFF_DE[] PROGMEM = "Filam. probe [off]";
-const char * const MSG_FSENSOR_OFF_LANG_TABLE[LANG_NUM] PROGMEM = {
-	MSG_FSENSOR_OFF_EN,
-	MSG_FSENSOR_OFF_CZ,
-	MSG_FSENSOR_OFF_IT,
-	MSG_FSENSOR_OFF_ES,
-	MSG_FSENSOR_OFF_PL,
-	MSG_FSENSOR_OFF_DE
-};
-
-const char MSG_FSENSOR_ON_EN[] PROGMEM = "Filam. probe  [on]";
-const char MSG_FSENSOR_ON_CZ[] PROGMEM = "Filam. probe  [on]";
-const char MSG_FSENSOR_ON_IT[] PROGMEM = "Filam. probe  [on]";
-const char MSG_FSENSOR_ON_ES[] PROGMEM = "Filam. probe  [on]";
-const char MSG_FSENSOR_ON_PL[] PROGMEM = "Filam. probe  [on]";
-const char MSG_FSENSOR_ON_DE[] PROGMEM = "Filam. probe  [on]";
-const char * const MSG_FSENSOR_ON_LANG_TABLE[LANG_NUM] PROGMEM = {
-	MSG_FSENSOR_ON_EN,
-	MSG_FSENSOR_ON_CZ,
-	MSG_FSENSOR_ON_IT,
-	MSG_FSENSOR_ON_ES,
-	MSG_FSENSOR_ON_PL,
-	MSG_FSENSOR_ON_DE
-};
+const char MSG_FSENSOR_OFF_EN[] PROGMEM = "Filam. probe [off]";
+const char MSG_FSENSOR_OFF_CZ[] PROGMEM = "Filam. probe [off]";
+const char MSG_FSENSOR_OFF_IT[] PROGMEM = "Filam. probe [off]";
+const char MSG_FSENSOR_OFF_ES[] PROGMEM = "Filam. probe [off]";
+const char MSG_FSENSOR_OFF_PL[] PROGMEM = "Filam. probe [off]";
+const char MSG_FSENSOR_OFF_DE[] PROGMEM = "Filam. probe [off]";
+const char * const MSG_FSENSOR_OFF_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_FSENSOR_OFF_EN,
+	MSG_FSENSOR_OFF_CZ,
+	MSG_FSENSOR_OFF_IT,
+	MSG_FSENSOR_OFF_ES,
+	MSG_FSENSOR_OFF_PL,
+	MSG_FSENSOR_OFF_DE
+};
+
+const char MSG_FSENSOR_ON_EN[] PROGMEM = "Filam. probe  [on]";
+const char MSG_FSENSOR_ON_CZ[] PROGMEM = "Filam. probe  [on]";
+const char MSG_FSENSOR_ON_IT[] PROGMEM = "Filam. probe  [on]";
+const char MSG_FSENSOR_ON_ES[] PROGMEM = "Filam. probe  [on]";
+const char MSG_FSENSOR_ON_PL[] PROGMEM = "Filam. probe  [on]";
+const char MSG_FSENSOR_ON_DE[] PROGMEM = "Filam. probe  [on]";
+const char * const MSG_FSENSOR_ON_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_FSENSOR_ON_EN,
+	MSG_FSENSOR_ON_CZ,
+	MSG_FSENSOR_ON_IT,
+	MSG_FSENSOR_ON_ES,
+	MSG_FSENSOR_ON_PL,
+	MSG_FSENSOR_ON_DE
+};
 
 const char MSG_SILENT_MODE_OFF_EN[] PROGMEM = "Mode [high power]";
 const char MSG_SILENT_MODE_OFF_CZ[] PROGMEM = "Mod  [vys. vykon]";
diff --git a/Firmware/language_all.h b/Firmware/language_all.h
index c95805d6..85bbb75f 100644
--- a/Firmware/language_all.h
+++ b/Firmware/language_all.h
@@ -220,6 +220,8 @@ extern const char* const MSG_FIL_ADJUSTING_LANG_TABLE[LANG_NUM];
 #define MSG_FIL_ADJUSTING LANG_TABLE_SELECT(MSG_FIL_ADJUSTING_LANG_TABLE)
 extern const char* const MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION_LANG_TABLE[LANG_NUM];
 #define MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION LANG_TABLE_SELECT(MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION_LANG_TABLE)
+extern const char* const MSG_CALIBRATE_Z_AUTO_LANG_TABLE[LANG_NUM];
+#define MSG_CALIBRATE_Z_AUTO LANG_TABLE_SELECT(MSG_CALIBRATE_Z_AUTO_LANG_TABLE)
 extern const char* const MSG_FIND_BED_OFFSET_AND_SKEW_LINE1_LANG_TABLE[LANG_NUM];
 #define MSG_FIND_BED_OFFSET_AND_SKEW_LINE1 LANG_TABLE_SELECT(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1_LANG_TABLE)
 extern const char* const MSG_FIND_BED_OFFSET_AND_SKEW_LINE2_LANG_TABLE[LANG_NUM];
@@ -546,12 +548,12 @@ extern const char* const MSG_SET_TEMPERATURE_LANG_TABLE[LANG_NUM];
 #define MSG_SET_TEMPERATURE LANG_TABLE_SELECT(MSG_SET_TEMPERATURE_LANG_TABLE)
 extern const char* const MSG_SHOW_END_STOPS_LANG_TABLE[LANG_NUM];
 #define MSG_SHOW_END_STOPS LANG_TABLE_SELECT(MSG_SHOW_END_STOPS_LANG_TABLE)
-
-extern const char* const MSG_FSENSOR_OFF_LANG_TABLE[LANG_NUM];
-#define MSG_FSENSOR_OFF LANG_TABLE_SELECT(MSG_FSENSOR_OFF_LANG_TABLE)
-extern const char* const MSG_FSENSOR_ON_LANG_TABLE[LANG_NUM];
-#define MSG_FSENSOR_ON LANG_TABLE_SELECT(MSG_FSENSOR_ON_LANG_TABLE)
-
+
+extern const char* const MSG_FSENSOR_OFF_LANG_TABLE[LANG_NUM];
+#define MSG_FSENSOR_OFF LANG_TABLE_SELECT(MSG_FSENSOR_OFF_LANG_TABLE)
+extern const char* const MSG_FSENSOR_ON_LANG_TABLE[LANG_NUM];
+#define MSG_FSENSOR_ON LANG_TABLE_SELECT(MSG_FSENSOR_ON_LANG_TABLE)
+
 extern const char* const MSG_SILENT_MODE_OFF_LANG_TABLE[LANG_NUM];
 #define MSG_SILENT_MODE_OFF LANG_TABLE_SELECT(MSG_SILENT_MODE_OFF_LANG_TABLE)
 extern const char* const MSG_SILENT_MODE_ON_LANG_TABLE[LANG_NUM];
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index b5fc90de..d3f6d94f 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -45,6 +45,8 @@ bool x_min_endstop = false;
 bool x_max_endstop = false;
 bool y_min_endstop = false;
 bool y_max_endstop = false;
+bool z_min_endstop = false;
+bool z_max_endstop = false;
 //===========================================================================
 //=============================private variables ============================
 //===========================================================================
@@ -85,11 +87,7 @@ static bool old_y_max_endstop=false;
 static bool old_z_min_endstop=false;
 static bool old_z_max_endstop=false;
 
-#ifdef TMC2130_SG_HOMING_SW
-static bool check_endstops = false;
-#else
 static bool check_endstops = true;
-#endif
 
 static bool check_z_endstop = false;
 
@@ -432,11 +430,11 @@ void isr() {
       {
         {
           #if defined(X_MIN_PIN) && (X_MIN_PIN > -1) && !defined(DEBUG_DISABLE_XMINLIMIT)
-			#ifndef TMC2130_SG_HOMING_SW
+			#ifndef TMC2130_SG_HOMING_SW_XY
 				x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
-			#else //TMC2130_SG_HOMING_SW
+			#else //TMC2130_SG_HOMING_SW_XY
 				x_min_endstop = tmc2130_axis_stalled[X_AXIS];
-			#endif //TMC2130_SG_HOMING_SW
+			#endif //TMC2130_SG_HOMING_SW_XY
             if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
               endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
               endstop_x_hit=true;
@@ -452,11 +450,11 @@ void isr() {
       {
         {
           #if defined(X_MAX_PIN) && (X_MAX_PIN > -1) && !defined(DEBUG_DISABLE_XMAXLIMIT)
-			#ifndef TMC2130_SG_HOMING_SW
+			#ifndef TMC2130_SG_HOMING_SW_XY
 				x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
-			#else //TMC2130_SG_HOMING_SW
+			#else //TMC2130_SG_HOMING_SW_XY
 				x_max_endstop = tmc2130_axis_stalled[X_AXIS];
-			#endif //TMC2130_SG_HOMING_SW
+			#endif //TMC2130_SG_HOMING_SW_XY
             if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
               endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
               endstop_x_hit=true;
@@ -476,11 +474,11 @@ void isr() {
       CHECK_ENDSTOPS
       {
         #if defined(Y_MIN_PIN) && (Y_MIN_PIN > -1) && !defined(DEBUG_DISABLE_YMINLIMIT)
-			#ifndef TMC2130_SG_HOMING_SW
+			#ifndef TMC2130_SG_HOMING_SW_XY
 				y_min_endstop=(READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
-			#else //TMC2130_SG_HOMING_SW
+			#else //TMC2130_SG_HOMING_SW_XY
 				y_min_endstop = tmc2130_axis_stalled[Y_AXIS];
-			#endif //TMC2130_SG_HOMING_SW
+			#endif //TMC2130_SG_HOMING_SW_XY
           if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
             endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
             endstop_y_hit=true;
@@ -494,11 +492,11 @@ void isr() {
       CHECK_ENDSTOPS
       {
         #if defined(Y_MAX_PIN) && (Y_MAX_PIN > -1) && !defined(DEBUG_DISABLE_YMAXLIMIT)
-			#ifndef TMC2130_SG_HOMING_SW
+			#ifndef TMC2130_SG_HOMING_SW_XY
 				y_max_endstop=(READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
-			#else //TMC2130_SG_HOMING_SW
+			#else //TMC2130_SG_HOMING_SW_XY
 				y_max_endstop = tmc2130_axis_stalled[Y_AXIS];
-			#endif //TMC2130_SG_HOMING_SW
+			#endif //TMC2130_SG_HOMING_SW_XY
           if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
             endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
             endstop_y_hit=true;
@@ -520,7 +518,7 @@ void isr() {
       if(check_endstops && ! check_z_endstop)
       {
         #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
-          bool z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
+          z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
           if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) {
             endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
             endstop_z_hit=true;
@@ -541,7 +539,11 @@ void isr() {
       CHECK_ENDSTOPS
       {
         #if defined(Z_MAX_PIN) && (Z_MAX_PIN > -1) && !defined(DEBUG_DISABLE_ZMAXLIMIT)
-          bool z_max_endstop=(READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
+			#ifndef TMC2130_SG_HOMING_SW_Z
+				z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
+			#else //TMC2130_SG_HOMING_SW_Z
+				z_max_endstop = tmc2130_axis_stalled[Z_AXIS];
+			#endif //TMC2130_SG_HOMING_SW_Z
           if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) {
             endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
             endstop_z_hit=true;
@@ -557,7 +559,7 @@ void isr() {
     if(check_z_endstop) {
         // Check the Z min end-stop no matter what.
         // Good for searching for the center of an induction target.
-        bool z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
+        z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
         if(z_min_endstop && old_z_min_endstop) {
           endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
           endstop_z_hit=true;
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index 3abce421..d111ab94 100644
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -25,7 +25,7 @@ uint8_t tmc2130_current_h[4] = TMC2130_CURRENTS_H;
 //running currents
 uint8_t tmc2130_current_r[4] = TMC2130_CURRENTS_R;
 //axis stalled flags
-uint8_t tmc2130_axis_stalled[2] = {0, 0};
+uint8_t tmc2130_axis_stalled[3] = {0, 0, 0};
 
 //pwm_ampl
 uint8_t tmc2130_pwm_ampl[2] = {TMC2130_PWM_AMPL_XY, TMC2130_PWM_AMPL_XY};
@@ -37,11 +37,11 @@ uint8_t tmc2130_pwm_auto[2] = {TMC2130_PWM_AUTO_XY, TMC2130_PWM_AUTO_XY};
 uint8_t tmc2130_pwm_freq[2] = {TMC2130_PWM_FREQ_XY, TMC2130_PWM_FREQ_XY};
 
 
-uint32_t tmc2131_axis_sg_pos[2] = {0, 0};
+uint8_t tmc2131_axis_sg_thr[3] = {TMC2130_SG_THRS_X, TMC2130_SG_THRS_Y, TMC2130_SG_THRS_Z};
+
+uint32_t tmc2131_axis_sg_pos[3] = {0, 0, 0};
 
 uint8_t sg_homing_axes_mask = 0x00;
-uint8_t sg_thrs_x = TMC2130_SG_THRS_X;
-uint8_t sg_thrs_y = TMC2130_SG_THRS_Y;
 
 bool skip_debug_msg = false;
 
@@ -81,7 +81,7 @@ bool skip_debug_msg = false;
 
 uint16_t tmc2130_rd_TSTEP(uint8_t cs);
 uint16_t tmc2130_rd_MSCNT(uint8_t cs);
-uint16_t tmc2130_rd_DRV_STATUS(uint8_t chipselect);
+uint16_t tmc2130_rd_DRV_STATUS(uint8_t cs);
 
 void tmc2130_wr_CHOPCONF(uint8_t cs, uint8_t toff = 3, uint8_t hstrt = 4, uint8_t hend = 1, uint8_t fd3 = 0, uint8_t disfdcc = 0, uint8_t rndtf = 0, uint8_t chm = 0, uint8_t tbl = 2, uint8_t vsense = 0, uint8_t vhighfs = 0, uint8_t vhighchm = 0, uint8_t sync = 0, uint8_t mres = 0b0100, uint8_t intpol = 1, uint8_t dedge = 0, uint8_t diss2g = 0);
 void tmc2130_wr_PWMCONF(uint8_t cs, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel);
@@ -139,7 +139,7 @@ void tmc2130_init()
 			tmc2130_wr(tmc2130_cs[i], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((tmc2130_current_r[i] >> 1) & 0x1f) << 8) | ((tmc2130_current_h[i] >> 1) & 0x1f));
 		}
 		tmc2130_wr(tmc2130_cs[i], TMC2130_REG_TPOWERDOWN, 0x00000000);
-		tmc2130_wr(tmc2130_cs[i], TMC2130_REG_GCONF, 0x00000000);
+		tmc2130_wr(tmc2130_cs[i], TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
 	}
 	for (int i = 3; i < 4; i++) // E axis
 	{
@@ -151,9 +151,35 @@ void tmc2130_init()
 	}
 }
 
+void tmc2130_update_sg_axis(uint8_t axis)
+{
+	if (!tmc2130_axis_stalled[axis])
+	{
+		uint8_t cs = tmc2130_cs[axis];
+		uint16_t tstep = tmc2130_rd_TSTEP(cs);
+		if (tstep < TMC2130_TCOOLTHRS)
+		{
+			long pos = st_get_position(axis);
+			if (abs(pos - tmc2131_axis_sg_pos[axis]) > TMC2130_SG_DELTA)
+			{
+				uint16_t sg = tmc2130_rd_DRV_STATUS(cs) & 0x3ff;
+				if (sg == 0)
+					tmc2130_axis_stalled[axis] = true;
+			}
+		}
+	}
+}
+
 bool tmc2130_update_sg()
 {
-#if (defined(TMC2130_SG_HOMING) && defined(TMC2130_SG_HOMING_SW))
+#ifdef TMC2130_SG_HOMING_SW_XY
+	if (sg_homing_axes_mask & X_AXIS_MASK) tmc2130_update_sg_axis(X_AXIS);
+	if (sg_homing_axes_mask & Y_AXIS_MASK) tmc2130_update_sg_axis(Y_AXIS);
+#endif //TMC2130_SG_HOMING_SW_XY
+#ifdef TMC2130_SG_HOMING_SW_Z
+	if (sg_homing_axes_mask & Z_AXIS_MASK) tmc2130_update_sg_axis(Z_AXIS);
+#endif //TMC2130_SG_HOMING_SW_Z
+#if (defined(TMC2130_SG_HOMING) && defined(TMC2130_SG_HOMING_SW_XY))
 	if (sg_homing_axes_mask == 0) return false;
 #ifdef TMC2130_DEBUG
 	MYSERIAL.print("tmc2130_update_sg mask=0x");
@@ -193,6 +219,7 @@ bool tmc2130_update_sg()
 	}
 	return true;
 #endif
+	return false;
 }
 
 void tmc2130_home_enter(uint8_t axes_mask)
@@ -202,22 +229,23 @@ void tmc2130_home_enter(uint8_t axes_mask)
 	MYSERIAL.println((int)axes_mask, 16);
 #endif //TMC2130_DEBUG
 #ifdef TMC2130_SG_HOMING
-	for (uint8_t axis = X_AXIS; axis <= Y_AXIS; axis++) //only X and Y axes
+	for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) //X Y and Z axes
 	{
 		uint8_t mask = (X_AXIS_MASK << axis);
+		uint8_t cs = tmc2130_cs[axis];
 		if (axes_mask & mask)
 		{
-			uint8_t cs = tmc2130_cs[axis];
 			sg_homing_axes_mask |= mask;
 			tmc2131_axis_sg_pos[axis] = st_get_position(axis);
 			tmc2130_axis_stalled[axis] = false;
 			//Configuration to spreadCycle
 			tmc2130_wr(cs, TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
-			tmc2130_wr(cs, TMC2130_REG_COOLCONF, ((axis == X_AXIS)?sg_thrs_x:sg_thrs_y) << 16);
+			tmc2130_wr(cs, TMC2130_REG_COOLCONF, ((unsigned long)tmc2131_axis_sg_thr[axis]) << 16);
 			tmc2130_wr(cs, TMC2130_REG_TCOOLTHRS, TMC2130_TCOOLTHRS);
-#ifndef TMC2130_SG_HOMING_SW
-			tmc2130_wr(cs, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS); //stallguard output DIAG1, DIAG1 = pushpull
-#endif //TMC2130_SG_HOMING_SW
+#ifndef TMC2130_SG_HOMING_SW_XY
+			if (mask & (X_AXIS_MASK | Y_AXIS_MASK))
+				tmc2130_wr(cs, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS); //stallguard output DIAG1, DIAG1 = pushpull
+#endif //TMC2130_SG_HOMING_SW_XY
 		}
 	}
 #endif //TMC2130_SG_HOMING
@@ -232,19 +260,19 @@ void tmc2130_home_exit()
 #ifdef TMC2130_SG_HOMING
 	if (sg_homing_axes_mask)
 	{
-		for (uint8_t axis = X_AXIS; axis <= Y_AXIS; axis++) //only X and Y axes
+		for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) //X Y and Z axes
 		{
 			uint8_t mask = (X_AXIS_MASK << axis);
-			if (sg_homing_axes_mask & mask)
+			if (sg_homing_axes_mask & mask & (X_AXIS_MASK | Y_AXIS_MASK))
 			{
 				if (tmc2130_mode == TMC2130_MODE_SILENT)
 					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SILENT); // Configuration back to stealthChop
 				else
-#ifdef TMC2130_SG_HOMING_SW
+#ifdef TMC2130_SG_HOMING_SW_XY
 					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
-#else //TMC2130_SG_HOMING_SW
+#else //TMC2130_SG_HOMING_SW_XY
 					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
-#endif //TMC2130_SG_HOMING_SW
+#endif //TMC2130_SG_HOMING_SW_XY
 			}
 			tmc2130_axis_stalled[axis] = false;
 		}
@@ -415,7 +443,7 @@ void tmc2130_wr_THIGH(uint8_t cs, uint32_t val32)
 	tmc2130_wr(cs, TMC2130_REG_THIGH, val32);
 }
 
-#ifdef TMC2130_DEBUG
+#if defined(TMC2130_DEBUG_RD) || defined(TMC2130_DEBUG_WR)
 uint8_t tmc2130_axis_by_cs(uint8_t cs)
 {
 	switch (cs)
diff --git a/Firmware/tmc2130.h b/Firmware/tmc2130.h
index c2901613..06c0f7cf 100644
--- a/Firmware/tmc2130.h
+++ b/Firmware/tmc2130.h
@@ -9,10 +9,9 @@ extern uint8_t tmc2130_mode;
 extern uint8_t tmc2130_current_h[4];
 extern uint8_t tmc2130_current_r[4];
 //flags for axis stall detection
-extern uint8_t tmc2130_axis_stalled[2];
+extern uint8_t tmc2130_axis_stalled[3];
 
-extern uint8_t sg_thrs_x;
-extern uint8_t sg_thrs_y;
+extern uint8_t tmc2131_axis_sg_thr[3];
 
 #define TMC2130_MODE_NORMAL 0
 #define TMC2130_MODE_SILENT 1
diff --git a/Firmware/variants/1_75mm_MK3-EINY03-E3Dv6full.h b/Firmware/variants/1_75mm_MK3-EINY03-E3Dv6full.h
index d7f6b66a..a60c0ab2 100644
--- a/Firmware/variants/1_75mm_MK3-EINY03-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK3-EINY03-E3Dv6full.h
@@ -134,11 +134,13 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
 #define TMC2130_TCOOLTHRS 239       // TCOOLTHRS - coolstep treshold
 
-#define TMC2130_SG_HOMING     1     // stallguard homing
-//#define TMC2130_SG_HOMING_SW  1     // stallguard "software" homing
-#define TMC2130_SG_THRS_X    30     // stallguard sensitivity for X axis
-#define TMC2130_SG_THRS_Y    30     // stallguard sensitivity for Y axis
-#define TMC2130_SG_DELTA     128    // stallguard delta [usteps] (minimum usteps before stallguard readed - SW homing)
+#define TMC2130_SG_HOMING       1     // stallguard homing
+//#define TMC2130_SG_HOMING_SW_XY  1    // stallguard "software" homing for XY axes
+#define TMC2130_SG_HOMING_SW_Z  1     // stallguard "software" homing for Z axis
+#define TMC2130_SG_THRS_X       0     // stallguard sensitivity for X axis
+#define TMC2130_SG_THRS_Y       0     // stallguard sensitivity for Y axis
+#define TMC2130_SG_THRS_Z       2     // stallguard sensitivity for Z axis
+#define TMC2130_SG_DELTA      128    // stallguard delta [usteps] (minimum usteps before stallguard readed - SW homing)
 
 //new settings is possible for vsense = 1, running current value > 31 set vsense to zero and shift both currents by 1 bit right (Z axis only)
 #define TMC2130_CURRENTS_H {3, 3, 5, 8}  // default holding currents for all axes
diff --git a/Firmware/variants/1_75mm_MK3-EINY04-E3Dv6full.h b/Firmware/variants/1_75mm_MK3-EINY04-E3Dv6full.h
index 6b6c861e..c1168f99 100644
--- a/Firmware/variants/1_75mm_MK3-EINY04-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK3-EINY04-E3Dv6full.h
@@ -134,11 +134,13 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
 #define TMC2130_TCOOLTHRS 239       // TCOOLTHRS - coolstep treshold
 
-#define TMC2130_SG_HOMING     1     // stallguard homing
-//#define TMC2130_SG_HOMING_SW  1     // stallguard "software" homing
-#define TMC2130_SG_THRS_X    30     // stallguard sensitivity for X axis
-#define TMC2130_SG_THRS_Y    30     // stallguard sensitivity for Y axis
-#define TMC2130_SG_DELTA     128    // stallguard delta [usteps] (minimum usteps before stallguard readed - SW homing)
+#define TMC2130_SG_HOMING       1     // stallguard homing
+//#define TMC2130_SG_HOMING_SW_XY  1    // stallguard "software" homing for XY axes
+#define TMC2130_SG_HOMING_SW_Z  1     // stallguard "software" homing for Z axis
+#define TMC2130_SG_THRS_X       0     // stallguard sensitivity for X axis
+#define TMC2130_SG_THRS_Y       0     // stallguard sensitivity for Y axis
+#define TMC2130_SG_THRS_Z       2     // stallguard sensitivity for Z axis
+#define TMC2130_SG_DELTA      128    // stallguard delta [usteps] (minimum usteps before stallguard readed - SW homing)
 
 //new settings is possible for vsense = 1, running current value > 31 set vsense to zero and shift both currents by 1 bit right (Z axis only)
 #define TMC2130_CURRENTS_H {3, 3, 5, 8}  // default holding currents for all axes