pat9125 filament sensor

sg homing - bug fix + homing multiple axes simultanously (AXIS_MASK)

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();

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
@@ -901,6 +908,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
@@ -1068,6 +1162,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!
     
@@ -1390,6 +1487,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
@@ -1839,52 +1939,6 @@ static float probe_pt(float x, float y, float z_before) {
 }
 
 #endif // #ifdef ENABLE_AUTO_BED_LEVELING
-/*
-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) \
@@ -1896,7 +1950,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;
@@ -1906,14 +1960,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;
@@ -1924,7 +1983,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];
@@ -3774,6 +3836,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.
@@ -3832,9 +3898,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;
     }
@@ -5169,6 +5240,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];
@@ -5475,6 +5547,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;
@@ -5804,6 +5883,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;

Firmware/language_all.cpp

@@ -2808,6 +2808,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]";

Firmware/language_all.h

@@ -530,6 +530,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];

Firmware/pat9125.cpp

@@ -16,6 +16,7 @@ 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)
 {
@@ -36,13 +37,17 @@ 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)
 		{
-			int iDX = pat9125_rd_reg(PAT9125_DELTA_XL);
+			unsigned char ucXL = pat9125_rd_reg(PAT9125_DELTA_XL);
-			int iDY = pat9125_rd_reg(PAT9125_DELTA_YL);
+			unsigned char ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
-			if (iDX >= 0x80) iDX = iDX - 256;
+			unsigned char ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
-			if (iDY >= 0x80) iDY = iDY - 256;
+			int iDX = ucXL | ((ucXYH << 4) & 0xf00);
-			pat9125_x += iDX;
+			int iDY = ucYL | ((ucXYH << 8) & 0xf00);
+			if (iDX & 0x800) iDX -= 4096;
+			if (iDY & 0x800) iDY -= 4096;
+//			pat9125_x += iDX;
 			pat9125_y += iDY;
 			return 1;
 		}

Firmware/pat9125.h

@@ -27,6 +27,7 @@ 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();

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

Firmware/swspi.h

@@ -23,7 +23,7 @@
 #define SWSPI_POL	1 //polarity
 #define SWSPI_PHA	0 //phase
 #define SWSPI_DOR	0 //data order
-#define SWSPI_DEL	100 //delay
+#define SWSPI_DEL	2 //delay
 
 
 void swspi_init();

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,9 +143,19 @@ 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];
+		uint8_t mask = (X_AXIS_MASK << axis);
+		if (sg_homing_axes_mask & mask)
+		{
+			if (!tmc2130_axis_stalled[axis])
+			{
+				uint8_t cs = tmc2130_cs[axis];
 				uint16_t tstep = tmc2130_rd_TSTEP(cs);
 				if (tstep < TMC2130_TCOOLTHRS)
 				{
@@ -156,24 +166,25 @@ bool tmc2130_update_sg()
 						uint16_t sg = tmc2130_rd_DRV_STATUS(cs) & 0x3ff;
 						if (sg==0)
 						{
-					tmc2130_axis_stalled[sg_homing_axis] = true;
+							tmc2130_axis_stalled[axis] = true;
 							tmc2130_LastHomingStalled = true;
 						}
-				else
+//						else
-					tmc2130_axis_stalled[sg_homing_axis] = false;
+//							tmc2130_axis_stalled[axis] = false;
+					}
+				}
+//				else
+//					tmc2130_axis_stalled[axis] = false;
+			}
 		}
 	}
-		else
-			tmc2130_axis_stalled[sg_homing_axis] = false;
 	return true;
-	}
+//	else
-	else
+//	{
-	{
+//		tmc2130_axis_stalled[X_AXIS] = false;
-		tmc2130_axis_stalled[X_AXIS] = false;
+//		tmc2130_axis_stalled[Y_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 ");
+#ifdef TMC2130_DEBUG
-	MYSERIAL.println((int)axis);
+	MYSERIAL.print("tmc2130_home_enter mask=0x");
+	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
+	{
+		uint8_t mask = (X_AXIS_MASK << axis);
+		if (axes_mask & mask)
+		{
 			uint8_t cs = tmc2130_cs[axis];
-	sg_homing_axis = axis;
+			sg_homing_axes_mask |= mask;
 			sg_homing_delay = 0;
-	tmc2130_axis_stalled[X_AXIS] = false;
+			tmc2130_axis_stalled[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);
 #ifndef TMC2130_SG_HOMING_SW
 			tmc2130_wr(cs, TMC2130_REG_GCONF, 0x00000080); //stallguard output to DIAG0
-#endif
+#endif //TMC2130_SG_HOMING_SW
-#endif
+		}
+	}
+#endif //TMC2130_SG_HOMING
 }
 
 void tmc2130_home_exit()
 {
-	MYSERIAL.println("tmc2130_home_exit ");
+#ifdef TMC2130_DEBUG
-	MYSERIAL.println((int)sg_homing_axis);
+	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)
+	{
+		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[sg_homing_axis], TMC2130_REG_GCONF, 0x00000004); // Configuration back to stealthChop
+					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, 0x00000004); // Configuration back to stealthChop
 				else
-			tmc2130_wr(tmc2130_cs[sg_homing_axis], TMC2130_REG_GCONF, 0x00000000);
+					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, 0x00000000);
-		sg_homing_axis = 0xff;
+			}
+		}
+		sg_homing_axes_mask = 0x00;
 	}
 #endif
 }

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 {

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);