diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 77ab5b88..eb0810ec 100644
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -202,6 +202,12 @@ void manage_inactivity(bool ignore_stepper_queue=false);
 
 
 enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
+#define X_AXIS_MASK  1
+#define Y_AXIS_MASK  2
+#define Z_AXIS_MASK  4
+#define E_AXIS_MASK  8
+#define X_HEAD_MASK 16
+#define Y_HEAD_MASK 32
 
 
 void FlushSerialRequestResend();
@@ -302,9 +308,9 @@ extern unsigned int custom_message_state;
 extern char snmm_filaments_used;
 extern unsigned long PingTime;
 
-extern bool fan_state[2];
-extern int fan_edge_counter[2];
-extern int fan_speed[2];
+extern bool fan_state[2];
+extern int fan_edge_counter[2];
+extern int fan_speed[2];
 
 // Handling multiple extruders pins
 extern uint8_t active_extruder;
@@ -357,8 +363,8 @@ void uvlo_();
 void recover_print();
 void setup_uvlo_interrupt();
 
-extern void save_print_to_eeprom();
-extern void restore_print_from_eeprom();
+extern void save_print_to_eeprom();
+extern void restore_print_from_eeprom();
 extern void position_menu();
 
 
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 2781927f..d534ffca 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -55,6 +55,13 @@
 #include "math.h"
 #include "util.h"
 
+#include <avr/wdt.h>
+
+#ifdef HAVE_PAT9125_SENSOR
+#include "swspi.h"
+#include "pat9125.h"
+#endif //HAVE_PAT9125_SENSOR
+
 #ifdef HAVE_TMC2130_DRIVERS
 #include "tmc2130.h"
 #endif //HAVE_TMC2130_DRIVERS
@@ -449,7 +456,7 @@ static bool cmdbuffer_front_already_processed = false;
 
 // Enable debugging of the command buffer.
 // Debugging information will be sent to serial line.
-// #define CMDBUFFER_DEBUG
+//#define CMDBUFFER_DEBUG
 
 static int serial_count = 0;  //index of character read from serial line
 static boolean comment_mode = false;
@@ -902,6 +909,93 @@ void servo_init()
 static void lcd_language_menu();
 
 
+#ifdef HAVE_PAT9125_SENSOR
+
+bool fsensor_enabled = true;
+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_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
+
+void fsensor_enable()
+{
+	MYSERIAL.println("fsensor_enable");
+	pat9125_y = 0;
+	prev_pos_e = st_get_position(E_AXIS);
+	err_cnt = 0;
+	fsensor_enabled = true;
+	fsensor_ignore_error = true;
+	fsensor_M600 = false;
+}
+
+void fsensor_disable()
+{
+	MYSERIAL.println("fsensor_disable");
+	fsensor_enabled = false;
+}
+
+void fsensor_update()
+{
+	if (!fsensor_enabled) return;
+	long pos_e = st_get_position(E_AXIS); //current position
+	pat9125_update();
+	long del_e = pos_e - prev_pos_e; //delta
+	if (abs(del_e) < FSENS_MINDEL) return;
+	float de = ((float)del_e / FSENS_ESTEPS);
+	int cmin = de * FSENS_MINFAC;
+	int cmax = de * FSENS_MAXFAC;
+	int cnt = pat9125_y;
+	prev_pos_e = pos_e;
+	pat9125_y = 0;
+	bool err = false;
+	if ((del_e > 0) && ((cnt < cmin) || (cnt > cmax))) err = true;
+	if ((del_e < 0) && ((cnt > cmin) || (cnt < cmax))) err = true;
+	if (err)
+		err_cnt++;
+	else
+		err_cnt = 0;
+
+/*
+	MYSERIAL.print("de=");
+	MYSERIAL.print(de);
+	MYSERIAL.print(" cmin=");
+	MYSERIAL.print((int)cmin);
+	MYSERIAL.print(" cmax=");
+	MYSERIAL.print((int)cmax);
+	MYSERIAL.print(" cnt=");
+	MYSERIAL.print((int)cnt);
+	MYSERIAL.print(" err=");
+	MYSERIAL.println((int)err_cnt);*/
+
+	if (err_cnt > FSENS_MAXERR)
+	{
+		MYSERIAL.println("fsensor_update (err_cnt > FSENS_MAXERR)");
+		if (fsensor_ignore_error)
+		{
+			MYSERIAL.println("fsensor_update - error ignored)");
+			fsensor_ignore_error = false;
+		}
+		else
+		{
+			MYSERIAL.println("fsensor_update - ERROR!!!");
+			planner_abort_hard();
+//			planner_pause_and_save();
+			enquecommand_front_P((PSTR("M600")));
+			fsensor_M600 = true;
+			fsensor_enabled = false;
+		}
+	}
+}
+
+#endif //HAVE_PAT9125_SENSOR
+
+
 #ifdef MESH_BED_LEVELING
    enum MeshLevelingState { MeshReport, MeshStart, MeshNext, MeshSet };
 #endif
@@ -1069,6 +1163,9 @@ void setup()
 	tmc2130_mode = silentMode?TMC2130_MODE_SILENT:TMC2130_MODE_NORMAL;
 #endif //HAVE_TMC2130_DRIVERS
 
+#ifdef HAVE_PAT9125_SENSOR
+	pat9125_init(200, 200);
+#endif //HAVE_PAT9125_SENSOR
     
 	st_init();    // Initialize stepper, this enables interrupts!
     
@@ -1391,6 +1488,9 @@ void loop()
   isPrintPaused ? manage_inactivity(true) : manage_inactivity(false);
   checkHitEndstops();
   lcd_update();
+#ifdef HAVE_PAT9125_SENSOR
+	fsensor_update();
+#endif //HAVE_PAT9125_SENSOR
 #ifdef HAVE_TMC2130_DRIVERS
 	tmc2130_check_overtemp();
 #endif //HAVE_TMC2130_DRIVERS
@@ -1850,6 +1950,7 @@ static float probe_pt(float x, float y, float z_before) {
 
 #endif // #ifdef ENABLE_AUTO_BED_LEVELING
 
+
 #ifdef LIN_ADVANCE
    /**
     * M900: Set and/or Get advance K factor and WH/D ratio
@@ -1883,53 +1984,6 @@ inline void gcode_M900() {
     }
 #endif // LIN_ADVANCE
 
-/*
-void homeaxis(int axis) {
-#define HOMEAXIS_DO(LETTER) \
-  ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
-
-  if (axis==X_AXIS ? HOMEAXIS_DO(X) :
-      axis==Y_AXIS ? HOMEAXIS_DO(Y) :
-      axis==Z_AXIS ? HOMEAXIS_DO(Z) :
-      0) {
-    int axis_home_dir = home_dir(axis);
-#ifdef HAVE_TMC2130_DRIVERS
-	if ((axis == X_AXIS) || (axis == Y_AXIS))
-		tmc2130_home_enter(axis);
-#endif //HAVE_TMC2130_DRIVERS
-
-    current_position[axis] = 0;
-    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-
-    destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
-    feedrate = homing_feedrate[axis];
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-    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]);
-    destination[axis] = -home_retract_mm(axis) * axis_home_dir;
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-    st_synchronize();
-
-    destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
-//    feedrate = homing_feedrate[axis]/2 ;
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-    st_synchronize();
-    axis_is_at_home(axis);
-    destination[axis] = current_position[axis];
-    feedrate = 0.0;
-    endstops_hit_on_purpose();
-    axis_known_position[axis] = true;
-
-#ifdef HAVE_TMC2130_DRIVERS
-	if ((axis == X_AXIS) || (axis == Y_AXIS))
-		tmc2130_home_exit();
-#endif //HAVE_TMC2130_DRIVERS
-  }
-}
-/**/
-
 void homeaxis(int axis) {
 #define HOMEAXIS_DO(LETTER) \
 ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
@@ -1940,7 +1994,7 @@ void homeaxis(int axis) {
         int axis_home_dir = home_dir(axis);
         
 #ifdef HAVE_TMC2130_DRIVERS
-		tmc2130_home_enter(axis);
+		tmc2130_home_enter(X_AXIS_MASK << axis);
 #endif
         
         current_position[axis] = 0;
@@ -1950,14 +2004,19 @@ void homeaxis(int axis) {
         feedrate = homing_feedrate[axis];
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
 
-		sg_homing_delay = 0;
+#ifdef HAVE_TMC2130_DRIVERS
+		tmc2130_axis_stalled[axis] = false;
+#endif
         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]);
         destination[axis] = -home_retract_mm(axis) * axis_home_dir;
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-		sg_homing_delay = 0;
+
+#ifdef HAVE_TMC2130_DRIVERS
+		tmc2130_axis_stalled[axis] = false;
+#endif
         st_synchronize();
         
         destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
@@ -1968,7 +2027,10 @@ void homeaxis(int axis) {
 #endif
 		feedrate = homing_feedrate[axis] / 2;
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-		sg_homing_delay = 0;
+
+#ifdef HAVE_TMC2130_DRIVERS
+		tmc2130_axis_stalled[axis] = false;
+#endif
         st_synchronize();
         axis_is_at_home(axis);
         destination[axis] = current_position[axis];
@@ -3783,7 +3845,7 @@ void process_commands()
 			setTargetHotend(0, 2);
 			adjust_bed_reset(); //reset bed level correction
 		}
-		
+
         // Disable the default update procedure of the display. We will do a modal dialog.
         lcd_update_enable(false);
         // Let the planner use the uncorrected coordinates.
@@ -3818,6 +3880,10 @@ void process_commands()
             setup_for_endstop_move();
             home_xy();
 
+#ifdef HAVE_TMC2130_DRIVERS
+		tmc2130_home_enter(X_AXIS_MASK | Y_AXIS_MASK);
+#endif
+
             int8_t verbosity_level = 0;
             if (code_seen('V')) {
                 // Just 'V' without a number counts as V1.
@@ -3876,9 +3942,14 @@ void process_commands()
                     lcd_show_fullscreen_message_and_wait_P(MSG_BABYSTEP_Z_NOT_SET);
                 }
             }
+#ifdef HAVE_TMC2130_DRIVERS
+		tmc2130_home_exit();
+#endif
         } else {
             // Timeouted.
         }
+
+
         lcd_update_enable(true);
         break;
     }
@@ -5213,6 +5284,7 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
     #ifdef FILAMENTCHANGEENABLE
     case 600: //Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
     {
+		MYSERIAL.println("!!!!M600!!!!");
 
 		st_synchronize();
 		float target[4];
@@ -5519,6 +5591,13 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 	  lcd_setstatuspgm(WELCOME_MSG);
 	  custom_message = false;
 	  custom_message_type = 0;
+#ifdef HAVE_PAT9125_SENSOR
+	  if (fsensor_M600)
+	  {
+		cmdqueue_pop_front(); //hack because M600 repeated 2x when enqueued to front
+		fsensor_enable();
+	  }
+#endif //HAVE_PAT9125_SENSOR
         
     }
     break;
@@ -5860,6 +5939,67 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 	  }
   } // end if(code_seen('T')) (end of T codes)
 
+  else if (code_seen('D')) // D codes (debug)
+  {
+    switch((int)code_value())
+    {
+	case 0: // D0 - Reset
+		MYSERIAL.println("D0 - Reset");
+		cli(); //disable interrupts
+		wdt_reset(); //reset watchdog
+		WDTCSR = (1<<WDCE) | (1<<WDE); //enable watchdog
+		WDTCSR = (1<<WDE) | (1<<WDP0); //30ms prescaler
+		while(1); //wait for reset
+		break;
+	case 1: // D1 - Clear EEPROM
+		{
+			MYSERIAL.println("D1 - Clear EEPROM");
+			cli();
+			for (int i = 0; i < 4096; i++)
+				eeprom_write_byte((unsigned char*)i, (unsigned char)0);
+			sei();
+		}
+		break;
+	case 2: // D2 - read/write PIN
+		{
+			if (code_seen('P')) // Pin (0-255)
+			{
+				int pin = (int)code_value();
+				if ((pin >= 0) && (pin <= 255))
+				{
+					if (code_seen('F')) // Function in/out (0/1)
+					{
+						int fnc = (int)code_value();
+						if (fnc == 0) pinMode(pin, INPUT);
+						else if (fnc == 1) pinMode(pin, OUTPUT);
+					}
+					if (code_seen('V')) // Value (0/1)
+					{
+						int val = (int)code_value();
+						if (val == 0) digitalWrite(pin, LOW);
+						else if (val == 1) digitalWrite(pin, HIGH);
+					}
+					else
+					{
+						int val = (digitalRead(pin) != LOW)?1:0;
+						MYSERIAL.print("PIN");
+						MYSERIAL.print(pin);
+						MYSERIAL.print("=");
+						MYSERIAL.println(val);
+					}
+				}
+			}
+		}
+		break;
+	case 3:
+		fsensor_enable();
+		break;
+	case 4:
+		fsensor_disable();
+		break;
+	}
+  }
+
   else
   {
     SERIAL_ECHO_START;
@@ -6979,11 +7119,17 @@ void recover_print() {
 
 	enquecommand_P(PSTR("G28 X"));
 	enquecommand_P(PSTR("G28 Y"));
-
+	sprintf_P(cmd, PSTR("M109 S%d"), target_temperature[active_extruder]);
+	enquecommand(cmd);
+	sprintf_P(cmd, PSTR("M190 S%d"), target_temperature_bed);
+	enquecommand(cmd);
+	enquecommand_P(PSTR("M83")); //E axis relative mode
+	enquecommand_P(PSTR("G1 E5 F120")); //Extrude some filament to stabilize pessure
+	enquecommand_P(PSTR("G1 E"  STRINGIFY(-DEFAULT_RETRACTION)" F480"));
 	eeprom_update_byte((uint8_t*)EEPROM_UVLO, 0);
-	while ((abs(degHotend(0)- target_temperature[0])>5) || (abs(degBed() -target_temperature_bed)>3)) { //wait for heater and bed to reach target temp
+	/*while ((abs(degHotend(0)- target_temperature[0])>5) || (abs(degBed() -target_temperature_bed)>3)) { //wait for heater and bed to reach target temp
 		delay_keep_alive(1000);
-	}
+	}*/
 	SERIAL_ECHOPGM("After waiting for temp:");
 	SERIAL_ECHOPGM("Current position X_AXIS:");
 	MYSERIAL.println(current_position[X_AXIS]);
@@ -7034,17 +7180,17 @@ void restore_print_from_eeprom() {
 	strcat(cmd, ftostr32(x_rec));
 	strcat(cmd, " Y");
 	strcat(cmd, ftostr32(y_rec));
+	strcat(cmd, " F2000");
 	enquecommand(cmd);
 	strcpy(cmd, "G1 Z");
 	strcat(cmd, ftostr32(z_pos));
 	enquecommand(cmd);
 	
 	enquecommand_P(PSTR("G1 E"  STRINGIFY(DEFAULT_RETRACTION)" F480"));
-	enquecommand_P(PSTR("G1 E0.5"));
+	//enquecommand_P(PSTR("G1 E0.5"));
 	sprintf_P(cmd, PSTR("G1 F%d"), feedrate_rec);
 	enquecommand(cmd);
 	strcpy(cmd, "M106 S");
 	strcat(cmd, itostr3(int(fan_speed_rec)));
-	enquecommand(cmd);
-	
+	enquecommand(cmd);	
 }
diff --git a/Firmware/language_all.cpp b/Firmware/language_all.cpp
index 4288ad51..66703176 100644
--- a/Firmware/language_all.cpp
+++ b/Firmware/language_all.cpp
@@ -2823,6 +2823,36 @@ 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_SILENT_MODE_OFF_EN[] PROGMEM = "Mode [high power]";
 const char MSG_SILENT_MODE_OFF_CZ[] PROGMEM = "Mod  [vys. vykon]";
 const char MSG_SILENT_MODE_OFF_IT[] PROGMEM = "Mode      [forte]";
diff --git a/Firmware/language_all.h b/Firmware/language_all.h
index 5aee1e70..fcaac7a8 100644
--- a/Firmware/language_all.h
+++ b/Firmware/language_all.h
@@ -536,6 +536,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_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/pat9125.cpp b/Firmware/pat9125.cpp
index 50a4aea8..fc5e7df0 100755
--- a/Firmware/pat9125.cpp
+++ b/Firmware/pat9125.cpp
@@ -1,74 +1,79 @@
-#include "pat9125.h"
-#include "swspi.h"
-
-
-#ifdef SWSPI_RPI
-//	#include <bcm2835.h>
-	#define DELAY(delay) usleep(delay)
-#endif //SWSPI_RPI
-
-#ifdef SWSPI_AVR
-	#include "Arduino.h"
-	#define DELAY(delay) delayMicroseconds(delay)
-#endif //SWSPI_AVR
-
-unsigned char ucPID1 = 0;
-unsigned char ucPID2 = 0;
-int pat9125_x = 0;
-int pat9125_y = 0;
-
-int pat9125_init(unsigned char xres, unsigned char yres)
-{
-	swspi_init();
-	ucPID1 = pat9125_rd_reg(PAT9125_PID1);
-	ucPID2 = pat9125_rd_reg(PAT9125_PID2);
-	if ((ucPID1 != 0x31) || (ucPID2 != 0x91))
-	{
-		return 0;
-	}
-    pat9125_wr_reg(PAT9125_RES_X, xres);
-    pat9125_wr_reg(PAT9125_RES_Y, yres);
-	return 1;
-}
-
-int pat9125_update()
-{
-	if ((ucPID1 == 0x31) && (ucPID2 == 0x91))
-	{
-		unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
-		if (ucMotion & 0x80)
-		{
-			int iDX = pat9125_rd_reg(PAT9125_DELTA_XL);
-			int iDY = pat9125_rd_reg(PAT9125_DELTA_YL);
-			if (iDX >= 0x80) iDX = iDX - 256;
-			if (iDY >= 0x80) iDY = iDY - 256;
-			pat9125_x += iDX;
-			pat9125_y += iDY;
-			return 1;
-		}
-	}
-	return 0;
-}
-
-unsigned char pat9125_rd_reg(unsigned char addr)
-{
-	swspi_start();
-	DELAY(100);
-	swspi_tx(addr & 0x7f);
-	DELAY(100);
-	unsigned char data = swspi_rx();
-	swspi_stop();
-	DELAY(100);
-	return data;
-}
-
-void pat9125_wr_reg(unsigned char addr, unsigned char data)
-{
-	swspi_start();
-	DELAY(100);
-	swspi_tx(addr | 0x80);
-	DELAY(100);
-	swspi_tx(data);
-	swspi_stop();
-	DELAY(100);
-}
+#include "pat9125.h"
+#include "swspi.h"
+
+
+#ifdef SWSPI_RPI
+//	#include <bcm2835.h>
+	#define DELAY(delay) usleep(delay)
+#endif //SWSPI_RPI
+
+#ifdef SWSPI_AVR
+	#include "Arduino.h"
+	#define DELAY(delay) delayMicroseconds(delay)
+#endif //SWSPI_AVR
+
+unsigned char ucPID1 = 0;
+unsigned char ucPID2 = 0;
+int pat9125_x = 0;
+int pat9125_y = 0;
+int pat9125_b = 0;
+
+int pat9125_init(unsigned char xres, unsigned char yres)
+{
+	swspi_init();
+	ucPID1 = pat9125_rd_reg(PAT9125_PID1);
+	ucPID2 = pat9125_rd_reg(PAT9125_PID2);
+	if ((ucPID1 != 0x31) || (ucPID2 != 0x91))
+	{
+		return 0;
+	}
+    pat9125_wr_reg(PAT9125_RES_X, xres);
+    pat9125_wr_reg(PAT9125_RES_Y, yres);
+	return 1;
+}
+
+int pat9125_update()
+{
+	if ((ucPID1 == 0x31) && (ucPID2 == 0x91))
+	{
+		unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
+		pat9125_b = pat9125_rd_reg(PAT9125_FRAME);
+		if (ucMotion & 0x80)
+		{
+			unsigned char ucXL = pat9125_rd_reg(PAT9125_DELTA_XL);
+			unsigned char ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
+			unsigned char ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
+			int iDX = ucXL | ((ucXYH << 4) & 0xf00);
+			int iDY = ucYL | ((ucXYH << 8) & 0xf00);
+			if (iDX & 0x800) iDX -= 4096;
+			if (iDY & 0x800) iDY -= 4096;
+//			pat9125_x += iDX;
+			pat9125_y += iDY;
+			return 1;
+		}
+	}
+	return 0;
+}
+
+unsigned char pat9125_rd_reg(unsigned char addr)
+{
+	swspi_start();
+	DELAY(100);
+	swspi_tx(addr & 0x7f);
+	DELAY(100);
+	unsigned char data = swspi_rx();
+	swspi_stop();
+	DELAY(100);
+	return data;
+}
+
+void pat9125_wr_reg(unsigned char addr, unsigned char data)
+{
+	swspi_start();
+	DELAY(100);
+	swspi_tx(addr | 0x80);
+	DELAY(100);
+	swspi_tx(data);
+	swspi_stop();
+	DELAY(100);
+}
diff --git a/Firmware/pat9125.h b/Firmware/pat9125.h
index eecd77a5..aa3d9d6d 100755
--- a/Firmware/pat9125.h
+++ b/Firmware/pat9125.h
@@ -1,38 +1,39 @@
-#ifndef PAT9125_H
-#define PAT9125_H
-
-//#define PAT9125_RPI
-#define PAT9125_AVR
-
-//PAT9125 registers
-#define PAT9125_PID1			0x00
-#define PAT9125_PID2			0x01
-#define PAT9125_MOTION			0x02
-#define PAT9125_DELTA_XL		0x03
-#define PAT9125_DELTA_YL		0x04
-#define PAT9125_MODE			0x05
-#define PAT9125_CONFIG			0x06
-#define PAT9125_WP				0x09
-#define PAT9125_SLEEP1			0x0a
-#define PAT9125_SLEEP2			0x0b
-#define PAT9125_RES_X			0x0d
-#define PAT9125_RES_Y			0x0e
-#define PAT9125_DELTA_XYH		0x12
-#define PAT9125_SHUTTER			0x14
-#define PAT9125_FRAME			0x17
-#define PAT9125_ORIENTATION		0x19
-
-extern unsigned char ucPID1;
-extern unsigned char ucPID2;
-
-extern int pat9125_x;
-extern int pat9125_y;
-
-int pat9125_init(unsigned char xres, unsigned char yres);
-int pat9125_update();
-
-unsigned char pat9125_rd_reg(unsigned char addr);
-void pat9125_wr_reg(unsigned char addr, unsigned char data);
-
-
-#endif //PAT9125_H
+#ifndef PAT9125_H
+#define PAT9125_H
+
+//#define PAT9125_RPI
+#define PAT9125_AVR
+
+//PAT9125 registers
+#define PAT9125_PID1			0x00
+#define PAT9125_PID2			0x01
+#define PAT9125_MOTION			0x02
+#define PAT9125_DELTA_XL		0x03
+#define PAT9125_DELTA_YL		0x04
+#define PAT9125_MODE			0x05
+#define PAT9125_CONFIG			0x06
+#define PAT9125_WP				0x09
+#define PAT9125_SLEEP1			0x0a
+#define PAT9125_SLEEP2			0x0b
+#define PAT9125_RES_X			0x0d
+#define PAT9125_RES_Y			0x0e
+#define PAT9125_DELTA_XYH		0x12
+#define PAT9125_SHUTTER			0x14
+#define PAT9125_FRAME			0x17
+#define PAT9125_ORIENTATION		0x19
+
+extern unsigned char ucPID1;
+extern unsigned char ucPID2;
+
+extern int pat9125_x;
+extern int pat9125_y;
+extern int pat9125_b;
+
+int pat9125_init(unsigned char xres, unsigned char yres);
+int pat9125_update();
+
+unsigned char pat9125_rd_reg(unsigned char addr);
+void pat9125_wr_reg(unsigned char addr, unsigned char data);
+
+
+#endif //PAT9125_H
diff --git a/Firmware/pins.h b/Firmware/pins.h
index 885f03f3..d5b2c36a 100644
--- a/Firmware/pins.h
+++ b/Firmware/pins.h
@@ -327,6 +327,7 @@
 
 #define LARGE_FLASH         true
 #define HAVE_TMC2130_DRIVERS
+#define HAVE_PAT9125_SENSOR
 
 #define X_STEP_PIN          37
 #define X_DIR_PIN           49
@@ -437,6 +438,7 @@
 
 #define LARGE_FLASH         true
 #define HAVE_TMC2130_DRIVERS
+#define HAVE_PAT9125_SENSOR
 
 #define X_STEP_PIN          37
 #define X_DIR_PIN           49
diff --git a/Firmware/swspi.cpp b/Firmware/swspi.cpp
index 1eac73ce..ca681e90 100755
--- a/Firmware/swspi.cpp
+++ b/Firmware/swspi.cpp
@@ -1,107 +1,107 @@
-#include "swspi.h"
-
-
-#ifdef SWSPI_RPI
-	#include <bcm2835.h>
-	#define GPIO_INP(gpio) bcm2835_gpio_fsel(gpio, BCM2835_GPIO_FSEL_INPT)
-	#define GPIO_OUT(gpio) bcm2835_gpio_fsel(gpio, BCM2835_GPIO_FSEL_OUTP)
-	#define GPIO_SET(gpio) bcm2835_gpio_write(gpio, HIGH)
-	#define GPIO_CLR(gpio) bcm2835_gpio_write(gpio, LOW)
-	#define GPIO_GET(gpio) (bcm2835_gpio_lev(gpio) != LOW)
-	#define DELAY(delay) usleep(delay)
-#endif //SWSPI_RPI
-
-#ifdef SWSPI_AVR
-	#include "Arduino.h"
-	#define GPIO_INP(gpio) pinMode(gpio, INPUT)
-	#define GPIO_OUT(gpio) pinMode(gpio, OUTPUT)
-	#define GPIO_SET(gpio) digitalWrite(gpio, HIGH)
-	#define GPIO_CLR(gpio) digitalWrite(gpio, LOW)
-	#define GPIO_GET(gpio) (digitalRead(gpio) != LOW)
-	#define DELAY(delay) delayMicroseconds(delay)
-#endif //SWSPI_AVR
-
-#if (SWSPI_POL != 0)
-	#define SWSPI_SCK_UP GPIO_CLR(SWSPI_SCK)
-	#define SWSPI_SCK_DN GPIO_SET(SWSPI_SCK)
-#else
-	#define SWSPI_SCK_UP GPIO_SET(SWSPI_SCK)
-	#define SWSPI_SCK_DN GPIO_CLR(SWSPI_SCK)
-#endif
-
-
-void swspi_init()
-{
-	GPIO_INP(SWSPI_MISO);
-	GPIO_OUT(SWSPI_MOSI);
-	GPIO_OUT(SWSPI_SCK);
-	GPIO_OUT(SWSPI_CS);
-	GPIO_CLR(SWSPI_MOSI);
-	SWSPI_SCK_DN;
-	GPIO_SET(SWSPI_CS);
-}
-
-#if (SWSPI_MOSI == SWSPI_MISO)
-
-void swspi_tx(unsigned char tx)
-{
-	GPIO_OUT(SWSPI_MOSI);
-	unsigned char i = 0; for (; i < 8; i++)
-	{
-		if (tx & 0x80) GPIO_SET(SWSPI_MOSI);
-		else GPIO_CLR(SWSPI_MOSI);
-	    DELAY(SWSPI_DEL);
-		SWSPI_SCK_UP;
-	    DELAY(SWSPI_DEL);
-		SWSPI_SCK_DN;
-		tx <<= 1;
-	}
-}
-
-unsigned char swspi_rx()
-{
-	GPIO_INP(SWSPI_MISO);
-	unsigned char rx = 0;
-	unsigned char i = 0; for (; i < 8; i++)
-	{
-		rx <<= 1;
-	    DELAY(SWSPI_DEL);
-		SWSPI_SCK_UP;
-	    DELAY(SWSPI_DEL);
-		rx |= GPIO_GET(SWSPI_MISO)?1:0;
-		SWSPI_SCK_DN;
-	}
-	return rx;
-}
-
-#else //(SWSPI_MOSI == SWSPI_MISO)
-
-unsigned char swspi_txrx(unsigned char tx)
-{
-	unsigned char rx = 0;
-	unsigned char i = 0; for (; i < 8; i++)
-	{
-		rx <<= 1;
-		if (tx & 0x80) GPIO_SET(SWSPI_MOSI);
-		else GPIO_CLR(SWSPI_MOSI);
-	    DELAY(SWSPI_DEL);
-		SWSPI_SCK_UP;
-	    DELAY(SWSPI_DEL);
-		rx |= GPIO_GET(SWSPI_MISO)?1:0;
-		SWSPI_SCK_DN;
-		tx <<= 1;
-	}
-	return rx;
-}
-
-#endif //(SWSPI_MOSI == SWSPI_MISO)
-
-void swspi_start()
-{
-	GPIO_CLR(SWSPI_CS);
-}
-
-void swspi_stop()
-{
-	GPIO_SET(SWSPI_CS);
-}
+#include "swspi.h"
+
+
+#ifdef SWSPI_RPI
+	#include <bcm2835.h>
+	#define GPIO_INP(gpio) bcm2835_gpio_fsel(gpio, BCM2835_GPIO_FSEL_INPT)
+	#define GPIO_OUT(gpio) bcm2835_gpio_fsel(gpio, BCM2835_GPIO_FSEL_OUTP)
+	#define GPIO_SET(gpio) bcm2835_gpio_write(gpio, HIGH)
+	#define GPIO_CLR(gpio) bcm2835_gpio_write(gpio, LOW)
+	#define GPIO_GET(gpio) (bcm2835_gpio_lev(gpio) != LOW)
+	#define DELAY(delay) usleep(delay)
+#endif //SWSPI_RPI
+
+#ifdef SWSPI_AVR
+	#include "Arduino.h"
+	#define GPIO_INP(gpio) pinMode(gpio, INPUT)
+	#define GPIO_OUT(gpio) pinMode(gpio, OUTPUT)
+	#define GPIO_SET(gpio) digitalWrite(gpio, HIGH)
+	#define GPIO_CLR(gpio) digitalWrite(gpio, LOW)
+	#define GPIO_GET(gpio) (digitalRead(gpio) != LOW)
+	#define DELAY(delay) delayMicroseconds(delay)
+#endif //SWSPI_AVR
+
+#if (SWSPI_POL != 0)
+	#define SWSPI_SCK_UP GPIO_CLR(SWSPI_SCK)
+	#define SWSPI_SCK_DN GPIO_SET(SWSPI_SCK)
+#else
+	#define SWSPI_SCK_UP GPIO_SET(SWSPI_SCK)
+	#define SWSPI_SCK_DN GPIO_CLR(SWSPI_SCK)
+#endif
+
+
+void swspi_init()
+{
+	GPIO_INP(SWSPI_MISO);
+	GPIO_OUT(SWSPI_MOSI);
+	GPIO_OUT(SWSPI_SCK);
+	GPIO_OUT(SWSPI_CS);
+	GPIO_CLR(SWSPI_MOSI);
+	SWSPI_SCK_DN;
+	GPIO_SET(SWSPI_CS);
+}
+
+#if (SWSPI_MOSI == SWSPI_MISO)
+
+void swspi_tx(unsigned char tx)
+{
+	GPIO_OUT(SWSPI_MOSI);
+	unsigned char i = 0; for (; i < 8; i++)
+	{
+		if (tx & 0x80) GPIO_SET(SWSPI_MOSI);
+		else GPIO_CLR(SWSPI_MOSI);
+	    DELAY(SWSPI_DEL);
+		SWSPI_SCK_UP;
+	    DELAY(SWSPI_DEL);
+		SWSPI_SCK_DN;
+		tx <<= 1;
+	}
+}
+
+unsigned char swspi_rx()
+{
+	GPIO_INP(SWSPI_MISO);
+	unsigned char rx = 0;
+	unsigned char i = 0; for (; i < 8; i++)
+	{
+		rx <<= 1;
+	    DELAY(SWSPI_DEL);
+		SWSPI_SCK_UP;
+	    DELAY(SWSPI_DEL);
+		rx |= GPIO_GET(SWSPI_MISO)?1:0;
+		SWSPI_SCK_DN;
+	}
+	return rx;
+}
+
+#else //(SWSPI_MOSI == SWSPI_MISO)
+
+unsigned char swspi_txrx(unsigned char tx)
+{
+	unsigned char rx = 0;
+	unsigned char i = 0; for (; i < 8; i++)
+	{
+		rx <<= 1;
+		if (tx & 0x80) GPIO_SET(SWSPI_MOSI);
+		else GPIO_CLR(SWSPI_MOSI);
+	    DELAY(SWSPI_DEL);
+		SWSPI_SCK_UP;
+	    DELAY(SWSPI_DEL);
+		rx |= GPIO_GET(SWSPI_MISO)?1:0;
+		SWSPI_SCK_DN;
+		tx <<= 1;
+	}
+	return rx;
+}
+
+#endif //(SWSPI_MOSI == SWSPI_MISO)
+
+void swspi_start()
+{
+	GPIO_CLR(SWSPI_CS);
+}
+
+void swspi_stop()
+{
+	GPIO_SET(SWSPI_CS);
+}
diff --git a/Firmware/swspi.h b/Firmware/swspi.h
index ef93556c..772c20c4 100755
--- a/Firmware/swspi.h
+++ b/Firmware/swspi.h
@@ -1,48 +1,48 @@
-#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
-#define SWSPI_MISO	16
-#define SWSPI_MOSI	16
-#define SWSPI_SCK	17
-#define SWSPI_CS	20
-#endif //SWSPI_AVR
-
-#define SWSPI_POL	1 //polarity
-#define SWSPI_PHA	0 //phase
-#define SWSPI_DOR	0 //data order
-#define SWSPI_DEL	100 //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
+#define SWSPI_MISO	16
+#define SWSPI_MOSI	16
+#define SWSPI_SCK	17
+#define SWSPI_CS	20
+#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/temperature.cpp b/Firmware/temperature.cpp
index 1de4d92f..5598c795 100644
--- a/Firmware/temperature.cpp
+++ b/Firmware/temperature.cpp
@@ -405,55 +405,55 @@ void setExtruderAutoFanState(int pin, bool state)
   analogWrite(pin, newFanSpeed);
 }
 
-void countFanSpeed()
-{	
-	fan_speed[0] = (fan_edge_counter[0] * (float(250) / (millis() - extruder_autofan_last_check)));
-	fan_speed[1] = (fan_edge_counter[1] * (float(250) / (millis() - extruder_autofan_last_check)));
-
-	fan_edge_counter[0] = 0;
-	fan_edge_counter[1] = 0;
-}
-
-void checkFanSpeed()
-{
-	static unsigned char fan_speed_errors[2] = { 0,0 };
-
-	if (fan_speed[0] == 0 && current_temperature[0] > EXTRUDER_AUTO_FAN_TEMPERATURE) fan_speed_errors[0]++;
-	else fan_speed_errors[0] = 0;
-
-	if (fan_speed[1] == 0 && fanSpeed > MIN_PRINT_FAN_SPEED) fan_speed_errors[1]++;
-	else fan_speed_errors[1] = 0;
-
-	if (fan_speed_errors[0] > 5) fanSpeedError(0);
-	if (fan_speed_errors[1] > 15) fanSpeedError(1);
-}
-
-void fanSpeedError(unsigned char _fan) {
-
-	if (card.sdprinting) {
-		card.pauseSDPrint();
-	}
-
-	setTargetHotend0(0);
-	/*lcd_update();
-	WRITE(BEEPER, HIGH);
-	delayMicroseconds(500);
-	WRITE(BEEPER, LOW);
-	delayMicroseconds(100);*/
-
-
-	SERIAL_ERROR_START;
-	switch (_fan) {
-	case 0:
-		SERIAL_ERRORLNPGM("ERROR: Extruder fan speed is lower then expected");
-		LCD_ALERTMESSAGEPGM("Err: EXTR. FAN ERROR");
-		break;
-	case 1:
-		SERIAL_ERRORLNPGM("ERROR: Print fan speed is lower then expected");
-		LCD_ALERTMESSAGEPGM("Err: PRINT FAN ERROR");
-		break;
-	}
-}
+void countFanSpeed()
+{	
+	fan_speed[0] = (fan_edge_counter[0] * (float(250) / (millis() - extruder_autofan_last_check)));
+	fan_speed[1] = (fan_edge_counter[1] * (float(250) / (millis() - extruder_autofan_last_check)));
+
+	fan_edge_counter[0] = 0;
+	fan_edge_counter[1] = 0;
+}
+
+void checkFanSpeed()
+{
+	static unsigned char fan_speed_errors[2] = { 0,0 };
+
+	if (fan_speed[0] == 0 && current_temperature[0] > EXTRUDER_AUTO_FAN_TEMPERATURE) fan_speed_errors[0]++;
+	else fan_speed_errors[0] = 0;
+
+	if (fan_speed[1] == 0 && fanSpeed > MIN_PRINT_FAN_SPEED) fan_speed_errors[1]++;
+	else fan_speed_errors[1] = 0;
+
+	if (fan_speed_errors[0] > 5) fanSpeedError(0);
+	if (fan_speed_errors[1] > 15) fanSpeedError(1);
+}
+
+void fanSpeedError(unsigned char _fan) {
+
+	if (card.sdprinting) {
+		card.pauseSDPrint();
+	}
+
+	setTargetHotend0(0);
+	/*lcd_update();
+	WRITE(BEEPER, HIGH);
+	delayMicroseconds(500);
+	WRITE(BEEPER, LOW);
+	delayMicroseconds(100);*/
+
+
+	SERIAL_ERROR_START;
+	switch (_fan) {
+	case 0:
+		SERIAL_ERRORLNPGM("ERROR: Extruder fan speed is lower then expected");
+		LCD_ALERTMESSAGEPGM("Err: EXTR. FAN ERROR");
+		break;
+	case 1:
+		SERIAL_ERRORLNPGM("ERROR: Print fan speed is lower then expected");
+		LCD_ALERTMESSAGEPGM("Err: PRINT FAN ERROR");
+		break;
+	}
+}
 
 
 void checkExtruderAutoFans()
@@ -628,8 +628,8 @@ void manage_heater()
       (defined(EXTRUDER_2_AUTO_FAN_PIN) && EXTRUDER_2_AUTO_FAN_PIN > -1)
   if(millis() - extruder_autofan_last_check > 1000)  // only need to check fan state very infrequently
   {
-	countFanSpeed();
-	checkFanSpeed();
+	countFanSpeed();
+	checkFanSpeed();
     checkExtruderAutoFans();
     extruder_autofan_last_check = millis();
   }  
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index 5589b470..afcb8d60 100644
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -32,7 +32,7 @@ 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};
 
 
-uint8_t sg_homing_axis = 0xff;
+uint8_t sg_homing_axes_mask = 0x00;
 uint8_t sg_homing_delay = 0;
 uint8_t sg_thrs_x = TMC2130_SG_THRS_X;
 uint8_t sg_thrs_y = TMC2130_SG_THRS_Y;
@@ -143,37 +143,48 @@ void tmc2130_init()
 bool tmc2130_update_sg()
 {
 #if (defined(TMC2130_SG_HOMING) && defined(TMC2130_SG_HOMING_SW))
-	if ((sg_homing_axis == X_AXIS) || (sg_homing_axis == Y_AXIS))
+	if (sg_homing_axes_mask == 0) return false;
+#ifdef TMC2130_DEBUG
+	MYSERIAL.print("tmc2130_update_sg mask=0x");
+	MYSERIAL.println((int)sg_homing_axes_mask, 16);
+#endif //TMC2130_DEBUG
+	for (uint8_t axis = X_AXIS; axis <= Y_AXIS; axis++) //only X and Y axes
 	{
-		uint8_t cs = tmc2130_cs[sg_homing_axis];
-		uint16_t tstep = tmc2130_rd_TSTEP(cs);
-		if (tstep < TMC2130_TCOOLTHRS)
+		uint8_t mask = (X_AXIS_MASK << axis);
+		if (sg_homing_axes_mask & mask)
 		{
-			if(sg_homing_delay < TMC2130_SG_DELAY) // wait for a few tens microsteps until stallGuard is used //todo: read out microsteps directly, instead of delay counter
-				sg_homing_delay++;
-			else
+			if (!tmc2130_axis_stalled[axis])
 			{
-				uint16_t sg = tmc2130_rd_DRV_STATUS(cs) & 0x3ff;
-				if (sg==0)
+				uint8_t cs = tmc2130_cs[axis];
+				uint16_t tstep = tmc2130_rd_TSTEP(cs);
+				if (tstep < TMC2130_TCOOLTHRS)
 				{
-					tmc2130_axis_stalled[sg_homing_axis] = true;
-					tmc2130_LastHomingStalled = true;
+					if(sg_homing_delay < TMC2130_SG_DELAY) // wait for a few tens microsteps until stallGuard is used //todo: read out microsteps directly, instead of delay counter
+						sg_homing_delay++;
+					else
+					{
+						uint16_t sg = tmc2130_rd_DRV_STATUS(cs) & 0x3ff;
+						if (sg==0)
+						{
+							tmc2130_axis_stalled[axis] = true;
+							tmc2130_LastHomingStalled = true;
+						}
+//						else
+//							tmc2130_axis_stalled[axis] = false;
+					}
 				}
-				else
-					tmc2130_axis_stalled[sg_homing_axis] = false;
+//				else
+//					tmc2130_axis_stalled[axis] = false;
 			}
 		}
-		else
-			tmc2130_axis_stalled[sg_homing_axis] = false;
-		return true;
-	}
-	else
-	{
-		tmc2130_axis_stalled[X_AXIS] = false;
-		tmc2130_axis_stalled[Y_AXIS] = false;
 	}
+	return true;
+//	else
+//	{
+//		tmc2130_axis_stalled[X_AXIS] = false;
+//		tmc2130_axis_stalled[Y_AXIS] = false;
+//	}
 #endif
-	return false;
 }
 
 void tmc2130_check_overtemp()
@@ -208,38 +219,55 @@ void tmc2130_check_overtemp()
 	}
 }
 
-void tmc2130_home_enter(uint8_t axis)
+void tmc2130_home_enter(uint8_t axes_mask)
 {
-	MYSERIAL.print("tmc2130_home_enter ");
-	MYSERIAL.println((int)axis);
+#ifdef TMC2130_DEBUG
+	MYSERIAL.print("tmc2130_home_enter mask=0x");
+	MYSERIAL.println((int)axes_mask, 16);
+#endif //TMC2130_DEBUG
 #ifdef TMC2130_SG_HOMING
-	uint8_t cs = tmc2130_cs[axis];
-	sg_homing_axis = axis;
-	sg_homing_delay = 0;
-	tmc2130_axis_stalled[X_AXIS] = false;
-	tmc2130_axis_stalled[Y_AXIS] = false;
-	//Configuration to spreadCycle
-	tmc2130_wr(cs, TMC2130_REG_GCONF, 0x00000000);
-	tmc2130_wr(cs, TMC2130_REG_COOLCONF, ((axis == X_AXIS)?sg_thrs_x:sg_thrs_y) << 16);
-	tmc2130_wr(cs, TMC2130_REG_TCOOLTHRS, TMC2130_TCOOLTHRS);
+	for (uint8_t axis = X_AXIS; axis <= Y_AXIS; axis++) //only X and Y axes
+	{
+		uint8_t mask = (X_AXIS_MASK << axis);
+		if (axes_mask & mask)
+		{
+			uint8_t cs = tmc2130_cs[axis];
+			sg_homing_axes_mask |= mask;
+			sg_homing_delay = 0;
+			tmc2130_axis_stalled[axis] = false;
+			//Configuration to spreadCycle
+			tmc2130_wr(cs, TMC2130_REG_GCONF, 0x00000000);
+			tmc2130_wr(cs, TMC2130_REG_COOLCONF, ((axis == X_AXIS)?sg_thrs_x:sg_thrs_y) << 16);
+			tmc2130_wr(cs, TMC2130_REG_TCOOLTHRS, TMC2130_TCOOLTHRS);
 #ifndef TMC2130_SG_HOMING_SW
-	tmc2130_wr(cs, TMC2130_REG_GCONF, 0x00000080); //stallguard output to DIAG0
-#endif
-#endif
+			tmc2130_wr(cs, TMC2130_REG_GCONF, 0x00000080); //stallguard output to DIAG0
+#endif //TMC2130_SG_HOMING_SW
+		}
+	}
+#endif //TMC2130_SG_HOMING
 }
 
 void tmc2130_home_exit()
 {
-	MYSERIAL.println("tmc2130_home_exit ");
-	MYSERIAL.println((int)sg_homing_axis);
+#ifdef TMC2130_DEBUG
+	MYSERIAL.print("tmc2130_home_exit mask=0x");
+	MYSERIAL.println((int)sg_homing_axes_mask, 16);
+#endif //TMC2130_DEBUG
 #ifdef TMC2130_SG_HOMING
-	if ((sg_homing_axis == X_AXIS) || (sg_homing_axis == Y_AXIS))
+	if (sg_homing_axes_mask)
 	{
-		if (tmc2130_mode == TMC2130_MODE_SILENT)
-			tmc2130_wr(tmc2130_cs[sg_homing_axis], TMC2130_REG_GCONF, 0x00000004); // Configuration back to stealthChop
-		else
-			tmc2130_wr(tmc2130_cs[sg_homing_axis], TMC2130_REG_GCONF, 0x00000000);
-		sg_homing_axis = 0xff;
+		for (uint8_t axis = X_AXIS; axis <= Y_AXIS; axis++) //only X and Y axes
+		{
+			uint8_t mask = (X_AXIS_MASK << axis);
+			if (sg_homing_axes_mask & mask)
+			{
+				if (tmc2130_mode == TMC2130_MODE_SILENT)
+					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, 0x00000004); // Configuration back to stealthChop
+				else
+					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, 0x00000000);
+			}
+		}
+		sg_homing_axes_mask = 0x00;
 	}
 #endif
 }
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index d2b0df4a..41f838ea 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -101,6 +101,9 @@ int8_t SDscrool = 0;
 
 int8_t SilentModeMenu = 0;
 
+int8_t FSensorStateMenu = 0;
+
+
 #ifdef SNMM
 uint8_t snmm_extruder = 0;
 #endif
@@ -516,11 +519,16 @@ static void lcd_status_screen()
 	  lcd_printPGM(MSG_PRINTER_DISCONNECTED);
   }
 
+//#define FSENS_FACTOR (2580.8/50) //filament sensor factor [steps / encoder counts]
+//#define FSENS_FACTOR (2580.8/45.3) //filament sensor factor [steps / encoder counts]
+  lcd.setCursor(0, 3);
+  lcd_implementation_print("                    ");
   lcd.setCursor(0, 3);
   lcd_implementation_print(pat9125_x);
-  lcd.setCursor(10, 3);
+  lcd.setCursor(6, 3);
   lcd_implementation_print(pat9125_y);
-
+  lcd.setCursor(12, 3);
+  lcd_implementation_print(pat9125_b);
 }
 
 #ifdef ULTIPANEL
@@ -2052,8 +2060,9 @@ void lcd_diag_show_end_stops()
 
 
 void prusa_statistics(int _message) {
-	
-
+#ifdef DEBUG_DISABLE_PRUSA_STATISTICS
+	return;
+#endif //DEBUG_DISABLE_PRUSA_STATISTICS
 	switch (_message)
 	{
 
@@ -2400,7 +2409,11 @@ void EEPROM_read(int pos, uint8_t* value, uint8_t size)
   } while (--size);
 }
 
-
+static void lcd_fsensor_state_set()
+{
+	FSensorStateMenu = !FSensorStateMenu;
+	lcd_goto_menu(lcd_settings_menu, 7);
+}
 
 static void lcd_silent_mode_set() {
   SilentModeMenu = !SilentModeMenu;
@@ -2605,6 +2618,12 @@ static void lcd_settings_menu()
 	  MENU_ITEM(gcode, MSG_DISABLE_STEPPERS, PSTR("M84"));
   }
 
+  if (FSensorStateMenu == 0) {
+    MENU_ITEM(function, MSG_FSENSOR_OFF, lcd_fsensor_state_set);
+  } else {
+    MENU_ITEM(function, MSG_FSENSOR_ON, lcd_fsensor_state_set);
+  }
+
   if ((SilentModeMenu == 0) || (farm_mode) ) {
     MENU_ITEM(function, MSG_SILENT_MODE_OFF, lcd_silent_mode_set);
   } else {
@@ -4233,7 +4252,7 @@ static void lcd_selftest()
 
 	_progress = lcd_selftest_screen(-1, _progress, 3, true, 2000);
 	_result = lcd_selftest_fan_dialog(0);
-
+	
 	if (_result)
 	{
 		_progress = lcd_selftest_screen(0, _progress, 3, true, 2000);
@@ -4320,7 +4339,7 @@ static void lcd_selftest()
 	enquecommand_P(PSTR("M84"));
 	lcd_implementation_clear();
 	lcd_next_update_millis = millis() + LCD_UPDATE_INTERVAL;
-
+	
 	if (_result)
 	{
 		LCD_ALERTMESSAGERPGM(MSG_SELFTEST_OK);
@@ -4343,13 +4362,22 @@ static bool lcd_selfcheck_axis_sg(char axis) {
 	case 1: axis_length = Y_MAX_POS + 8; break;
 	default: axis_length = 210; break;
 	}
-	
+	/*SERIAL_ECHOPGM("Current position 1:");
+	MYSERIAL.println(current_position[axis]);*/
+
+	current_position[axis] = 0;
+	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 
 	for (char i = 0; i < 2; i++) {
-		SERIAL_ECHOPGM("Current position:");
-		MYSERIAL.println(current_position[axis]);
+		/*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 {
@@ -4365,14 +4393,14 @@ static bool lcd_selfcheck_axis_sg(char axis) {
 		tmc2130_home_enter(axis);
 #endif
 		st_synchronize();
+
 #ifdef HAVE_TMC2130_DRIVERS
 		tmc2130_home_exit();
 #endif
 		//current_position[axis] = st_get_position_mm(axis);
 		//plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 
-		SERIAL_ECHOPGM("Current position:");
-		MYSERIAL.println(current_position[axis]);
+
 		current_position_init = st_get_position_mm(axis);
 		if (i == 0) {
 			current_position[axis] += margin;
@@ -4389,8 +4417,7 @@ static bool lcd_selfcheck_axis_sg(char axis) {
 #endif
 			//current_position[axis] = st_get_position_mm(axis);
 			//plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-			SERIAL_ECHOPGM("Current position:");
-			MYSERIAL.println(current_position[axis]);
+
 			current_position_final = st_get_position_mm(axis);
 		}
 		measured_axis_length[i] = abs(current_position_final - current_position_init);
diff --git a/Firmware/ultralcd_implementation_hitachi_HD44780.h b/Firmware/ultralcd_implementation_hitachi_HD44780.h
index ae0bb09f..c50d8342 100644
--- a/Firmware/ultralcd_implementation_hitachi_HD44780.h
+++ b/Firmware/ultralcd_implementation_hitachi_HD44780.h
@@ -809,6 +809,9 @@ static void lcd_implementation_status_screen()
     }
     lcd_printPGM(PSTR("  "));
 
+#ifdef DEBUG_DISABLE_LCD_STATUS_LINE
+	return;
+#endif //DEBUG_DISABLE_LCD_STATUS_LINE
 
     //Print status line
     lcd.setCursor(0, 3);