diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 3bde8ba44..53988194f 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -911,14 +911,14 @@ static void lcd_language_menu();
 
 #ifdef HAVE_PAT9125_SENSOR
 
-bool fsensor_enabled = true;
+bool fsensor_enabled = false;
 bool fsensor_ignore_error = true;
 bool fsensor_M600 = false;
 long prev_pos_e = 0;
 long err_cnt = 0;
 
-#define FSENS_ESTEPS 140  //extruder resolution [steps/mm]
-#define FSENS_MINDEL 280  //filament sensor min delta [steps] (3mm)
+#define FSENS_ESTEPS 280  //extruder resolution [steps/mm]
+#define FSENS_MINDEL 560  //filament sensor min delta [steps] (3mm)
 #define FSENS_MINFAC 3    //filament sensor minimum factor [count/mm]
 #define FSENS_MAXFAC 50   //filament sensor maximum factor [count/mm]
 #define FSENS_MAXERR 2    //filament sensor max error count
@@ -5982,11 +5982,15 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 		break;
 	case 3:
 		MYSERIAL.print("fsensor_enable()");
+#ifdef HAVE_PAT9125_SENSOR
 		fsensor_enable();
+#endif
 		break;
 	case 4:
 		MYSERIAL.print("fsensor_disable()");
+#ifdef HAVE_PAT9125_SENSOR
 		fsensor_disable();
+#endif            
 		break;
 	case 5:
 		{
diff --git a/Firmware/swspi.h b/Firmware/swspi.h
index bee3c19df..c74232376 100755
--- a/Firmware/swspi.h
+++ b/Firmware/swspi.h
@@ -1,55 +1,60 @@
-#ifndef SWSPI_H
-#define SWSPI_H
-
-//#define SWSPI_RPI
-#define SWSPI_AVR
-
-#ifdef SWSPI_RPI
-//#define SWSPI_MISO	9
-#define SWSPI_MISO	10
-#define SWSPI_MOSI	10
-#define SWSPI_SCK	11
-#define SWSPI_CS	7
-#endif //SWSPI_RPI
-
-#ifdef SWSPI_AVR
-//#if MOTHERBOARD == BOARD_EINY_0_3a
-#define SWSPI_MISO	16 //RX2
-#define SWSPI_MOSI	16 //RX2
-#define SWSPI_SCK	17 //TX2
-#define SWSPI_CS	20 //SDA
-//#endif //(MOTHERBOARD == 299)
-/*#if MOTHERBOARD == BOARD_EINY_0_4a
-#define SWSPI_MISO	62 //PK0
-#define SWSPI_MOSI	62 //PK0
-#define SWSPI_SCK	21 //SCL
-#define SWSPI_CS	20 //SDA
-#endif //(MOTHERBOARD == BOARD_EINY_0_4a)*/
-#endif //SWSPI_AVR
-
-#define SWSPI_POL	1 //polarity
-#define SWSPI_PHA	0 //phase
-#define SWSPI_DOR	0 //data order
-#define SWSPI_DEL	2 //delay
-
-
-void swspi_init();
-
-#if (SWSPI_MOSI == SWSPI_MISO)
-
-void swspi_tx(unsigned char tx);
-unsigned char swspi_rx();
-
-#else //(SWSPI_MOSI == SWSPI_MISO)
-
-#define swspi_tx swspi_txrx
-#define swspi_rx swspi_txrx
-unsigned char swspi_txrx(unsigned char tx);
-
-#endif //(SWSPI_MOSI == SWSPI_MISO)
-
-void swspi_start();
-void swspi_stop();
-
-
-#endif //SWSPI_H
+#ifndef SWSPI_H
+#define SWSPI_H
+
+//#define SWSPI_RPI
+#define SWSPI_AVR
+
+#ifdef SWSPI_RPI
+//#define SWSPI_MISO	9
+#define SWSPI_MISO	10
+#define SWSPI_MOSI	10
+#define SWSPI_SCK	11
+#define SWSPI_CS	7
+#endif //SWSPI_RPI
+
+#ifdef SWSPI_AVR
+//#if MOTHERBOARD == BOARD_EINY_0_3a
+#define SWSPI_MISO	16 //RX2
+#define SWSPI_MOSI	16 //RX2
+#define SWSPI_SCK	17 //TX2
+#define SWSPI_CS	20 //SDA
+//#endif //(MOTHERBOARD == 299)
+/*#if MOTHERBOARD == BOARD_EINY_0_4a
+#define SWSPI_MISO	62 //PK0
+#define SWSPI_MOSI	62 //PK0
+#define SWSPI_SCK	21 //SCL
+#endif //(MOTHERBOARD == BOARD_EINY_0_3a)
+#if (MOTHERBOARD == BOARD_EINY_0_4a)
+#define SWSPI_MISO	21 //PK0
+#define SWSPI_MOSI	21 //PK0
+#define SWSPI_SCK	62 //SCL
+#define SWSPI_CS	20 //SDA
+#endif //(MOTHERBOARD == BOARD_EINY_0_4a)*/
+#endif //SWSPI_AVR
+
+#define SWSPI_POL	1 //polarity
+#define SWSPI_PHA	0 //phase
+#define SWSPI_DOR	0 //data order
+#define SWSPI_DEL	2 //delay
+
+
+void swspi_init();
+
+#if (SWSPI_MOSI == SWSPI_MISO)
+
+void swspi_tx(unsigned char tx);
+unsigned char swspi_rx();
+
+#else //(SWSPI_MOSI == SWSPI_MISO)
+
+#define swspi_tx swspi_txrx
+#define swspi_rx swspi_txrx
+unsigned char swspi_txrx(unsigned char tx);
+
+#endif //(SWSPI_MOSI == SWSPI_MISO)
+
+void swspi_start();
+void swspi_stop();
+
+
+#endif //SWSPI_H
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index a1e595645..ed7e5e838 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -102,6 +102,8 @@ int8_t SDscrool = 0;
 int8_t SilentModeMenu = 0;
 
 int8_t FSensorStateMenu = 0;
+extern void fsensor_enable();
+extern void fsensor_disable();
 
 
 #ifdef SNMM
@@ -2472,8 +2474,14 @@ void EEPROM_read(int pos, uint8_t* value, uint8_t size)
 
 static void lcd_fsensor_state_set()
 {
+    if (!FSensorStateMenu==0) {
+        fsensor_disable();
+    }else{
+        fsensor_enable();
+    }
 	FSensorStateMenu = !FSensorStateMenu;
 	lcd_goto_menu(lcd_settings_menu, 7);
+    
 }
 
 static void lcd_silent_mode_set() {
@@ -4365,10 +4373,10 @@ static void lcd_selftest()
 
 	if (_result)
 	{
-		//current_position[X_AXIS] = current_position[X_AXIS] + 14;
-		//current_position[Y_AXIS] = current_position[Y_AXIS] + 12;
 #ifdef HAVE_TMC2130_DRIVERS
 		tmc2130_home_exit();
+		sg_homing_delay = 0;
+		enable_endstops(false);
 #endif
 		current_position[X_AXIS] = current_position[X_AXIS] + 14;
 		current_position[Y_AXIS] = current_position[Y_AXIS] + 12;
@@ -4429,33 +4437,31 @@ static bool lcd_selfcheck_axis_sg(char axis) {
 	current_position[axis] = 0;
 	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 
+#ifdef HAVE_TMC2130_DRIVERS
+	tmc2130_home_exit();
+	sg_homing_delay = 0;
+	tmc2130_axis_stalled[axis] = false;
+	enable_endstops(true);
+#endif
+
 	for (char i = 0; i < 2; i++) {
 		/*SERIAL_ECHOPGM("i = ");
 		MYSERIAL.println(int(i));
 		SERIAL_ECHOPGM("Current position 2:");
 		MYSERIAL.println(current_position[axis]);*/
-		if (i == 0) {
-			current_position[axis] -= (axis_length + margin);
-			/*SERIAL_ECHOPGM("Current position 3:");
-			MYSERIAL.println(current_position[axis]);*/
-
-			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
-		}
-		else {
-			current_position[axis] -= margin;
-			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();
-			current_position[axis] -= axis_length;
-			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
-		}
-
 
 #ifdef HAVE_TMC2130_DRIVERS
-		tmc2130_home_enter(axis);
+		tmc2130_home_enter(X_AXIS_MASK << axis);
 #endif
+
+
+			current_position[axis] -= (axis_length + margin);
+			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 HAVE_TMC2130_DRIVERS
+		sg_homing_delay = 0;
 		tmc2130_home_exit();
 #endif
 		//current_position[axis] = st_get_position_mm(axis);
@@ -4463,17 +4469,18 @@ static bool lcd_selfcheck_axis_sg(char axis) {
 
 
 		current_position_init = st_get_position_mm(axis);
-		if (i == 0) {
+		if (i < 1) {
 			current_position[axis] += margin;
 			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();
 			current_position[axis] += axis_length;
 			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
 #ifdef HAVE_TMC2130_DRIVERS
-			tmc2130_home_enter(axis);
+			tmc2130_home_enter(X_AXIS_MASK << axis);
 #endif
 			st_synchronize();
 #ifdef HAVE_TMC2130_DRIVERS
+			sg_homing_delay = 0;
 			tmc2130_home_exit();
 #endif
 			//current_position[axis] = st_get_position_mm(axis);
@@ -4489,7 +4496,9 @@ static bool lcd_selfcheck_axis_sg(char axis) {
 		if (abs(measured_axis_length[i] - axis_length) > max_error_mm) {
 			//axis length
 #ifdef HAVE_TMC2130_DRIVERS
+			sg_homing_delay = 0;
 			tmc2130_home_exit();
+			enable_endstops(false);
 #endif
 			const char *_error_1;
 			const char *_error_2;
diff --git a/Firmware/variants/1_75mm_MK3-EINY04-E3Dv6full.h b/Firmware/variants/1_75mm_MK3-EINY04-E3Dv6full.h
index e06fd40c5..f16a2a2e4 100644
--- a/Firmware/variants/1_75mm_MK3-EINY04-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK3-EINY04-E3Dv6full.h
@@ -139,7 +139,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
 //#define TMC2130_DEBUG
 //#define TMC2130_DEBUG_WR
-//#define TMC2130_DEBUG_RD
+#define TMC2130_DEBUG_RD
 
 
 /*------------------------------------