diff --git a/Firmware/ConfigurationStore.cpp b/Firmware/ConfigurationStore.cpp
index ea9d0bca6..c1ef8576f 100644
--- a/Firmware/ConfigurationStore.cpp
+++ b/Firmware/ConfigurationStore.cpp
@@ -313,6 +313,20 @@ bool Config_RetrieveSettings(uint16_t offset, uint8_t level)
         EEPROM_READ_VAR(i,max_feedrate_silent);  
         EEPROM_READ_VAR(i,max_acceleration_units_per_sq_second_silent);
 
+#ifdef TMC2130
+		for (uint8_t j = X_AXIS; j <= Y_AXIS; j++)
+		{
+			if (max_feedrate_normal[j] > NORMAL_MAX_FEEDRATE_XY)
+				max_feedrate_normal[j] = NORMAL_MAX_FEEDRATE_XY;
+			if (max_feedrate_silent[j] > SILENT_MAX_FEEDRATE_XY)
+				max_feedrate_silent[j] = SILENT_MAX_FEEDRATE_XY;
+			if (max_acceleration_units_per_sq_second_normal[j] > NORMAL_MAX_ACCEL_XY)
+				max_acceleration_units_per_sq_second_normal[j] = NORMAL_MAX_ACCEL_XY;
+			if (max_acceleration_units_per_sq_second_silent[j] > SILENT_MAX_ACCEL_XY)
+				max_acceleration_units_per_sq_second_silent[j] = SILENT_MAX_ACCEL_XY;
+		}
+#endif //TMC2130
+
 		reset_acceleration_rates();
 
 		// Call updatePID (similar to when we have processed M301)
diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index beba98123..80dcc2d09 100644
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -411,9 +411,6 @@ extern void print_world_coordinates();
 extern void print_physical_coordinates();
 extern void print_mesh_bed_leveling_table();
 
-#ifdef PAT9125
-extern void fsensor_init();
-#endif //PAT9125
 
 //estimated time to end of the print
 extern uint16_t print_time_remaining();
@@ -465,9 +462,9 @@ void gcode_M701();
 
 void proc_commands();
 
-bool mmu_get_reponse();
+bool mmu_get_reponse(bool timeout);
 void mmu_not_responding();
 void mmu_load_to_nozzle();
-void M600_load_filament(bool fsensor_enabled);
+void M600_load_filament();
 void mmu_M600_load_filament();
 void M600_load_filament_movements();
\ No newline at end of file
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index d2219f4b1..b5d8a2ab2 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -91,10 +91,9 @@
 #include "swi2c.h"
 #endif //SWI2C
 
-#ifdef PAT9125
-#include "pat9125.h"
+#ifdef FILAMENT_SENSOR
 #include "fsensor.h"
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 
 #ifdef TMC2130
 #include "tmc2130.h"
@@ -639,7 +638,6 @@ void stop_and_save_print_to_ram(float z_move, float e_move);
 void restore_print_from_ram_and_continue(float e_move);
 
 bool fans_check_enabled = true;
-bool filament_autoload_enabled = true;
 
 
 #ifdef TMC2130
@@ -688,12 +686,6 @@ void crashdet_stop_and_save_print2()
 
 void crashdet_detected(uint8_t mask)
 {
-//	printf("CRASH_DETECTED");
-/*	while (!is_buffer_empty())
-	{
-		process_commands();
-	    cmdqueue_pop_front();
-	}*/
 	st_synchronize();
 	static uint8_t crashDet_counter = 0;
 	bool automatic_recovery_after_crash = true;
@@ -856,8 +848,10 @@ void factory_reset(char level, bool quiet)
             eeprom_update_word((uint16_t *)EEPROM_FERROR_COUNT_TOT, 0);
             eeprom_update_word((uint16_t *)EEPROM_POWER_COUNT_TOT, 0);
 
-            fsensor_enable();
+#ifdef FILAMENT_SENSOR
+			fsensor_enable();
             fsensor_autoload_set(true);
+#endif //FILAMENT_SENSOR
                        
             WRITE(BEEPER, HIGH);
             _delay_ms(100);
@@ -1187,9 +1181,10 @@ void setup()
 		tmc2130_current_h[E_AXIS] = 36;
 		tmc2130_current_r[E_AXIS] = 36;
 #endif //TMC2130
+#ifdef FILAMENT_SENSOR
 		//disabled filament autoload (PFW360)
-		filament_autoload_enabled = false;
-		eeprom_update_byte((uint8_t*)EEPROM_FSENS_AUTOLOAD_ENABLED, 0);
+		fsensor_autoload_set(false);
+#endif //FILAMENT_SENSOR
 	}
 	MYSERIAL.begin(BAUDRATE);
 	fdev_setup_stream(uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE); //setup uart out stream
@@ -1441,9 +1436,9 @@ void setup()
 	// It does not make sense to load the correction matrix until the machine is homed.
 	world2machine_reset();
     
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
 	fsensor_init();
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 
 
 #if defined(CONTROLLERFAN_PIN) && (CONTROLLERFAN_PIN > -1)
@@ -1625,11 +1620,9 @@ void setup()
 	setup_fan_interrupt();
 #endif //DEBUG_DISABLE_FANCHECK
 
-#ifdef PAT9125
-#ifndef DEBUG_DISABLE_FSENSORCHECK
+#ifdef FILAMENT_SENSOR
 	fsensor_setup_interrupt();
-#endif //DEBUG_DISABLE_FSENSORCHECK
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 	for (int i = 0; i<4; i++) EEPROM_read_B(EEPROM_BOWDEN_LENGTH + i * 2, &bowden_length[i]); 
 	
 #ifndef DEBUG_DISABLE_STARTMSGS
@@ -1767,38 +1760,6 @@ void setup()
 #endif //WATCHDOG
 }
 
-#ifdef PAT9125
-void fsensor_init() {
-	int pat9125 = pat9125_init();
-	printf_P(_N("PAT9125_init:%d\n"), pat9125);
-	uint8_t fsensor = eeprom_read_byte((uint8_t*)EEPROM_FSENSOR);
-     filament_autoload_enabled=eeprom_read_byte((uint8_t*)EEPROM_FSENS_AUTOLOAD_ENABLED);
-	if (!pat9125)
-	{
-		fsensor = 0; //disable sensor
-		fsensor_not_responding = true;
-	}
-	else {
-		fsensor_not_responding = false;
-	}
-	puts_P(PSTR("FSensor "));
-	if (fsensor)
-	{
-		puts_P(PSTR("ENABLED\n"));
-		fsensor_enable();
-	}
-	else
-	{
-		puts_P(PSTR("DISABLED\n"));
-		fsensor_disable();
-	}
-#ifdef DEBUG_DISABLE_FSENSORCHECK
-	filament_autoload_enabled = false;
-	fsensor_disable();
-#endif //DEBUG_DISABLE_FSENSORCHECK
-}
-
-#endif //PAT9125
 
 void trace();
 
@@ -2013,9 +1974,9 @@ void loop()
   isPrintPaused ? manage_inactivity(true) : manage_inactivity(false);
   checkHitEndstops();
   lcd_update(0);
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
 	fsensor_update();
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 #ifdef TMC2130
 	tmc2130_check_overtemp();
 	if (tmc2130_sg_crash)
@@ -3111,18 +3072,14 @@ void gcode_M701()
 
 #if defined (SNMM) || defined (SNMM_V2)
 	extr_adj(snmm_extruder);//loads current extruder
-#else
+#else //defined (SNMM) || defined (SNMM_V2)
 	enable_z();
 	custom_message = true;
 	custom_message_type = 2;
 
-	bool old_watch_runout = fsensor_watch_runout;
-	fsensor_watch_runout = false;
-	fsensor_st_sum = 0;
-	fsensor_yd_sum = 0;
-	fsensor_er_sum = 0;
-	fsensor_yd_min = 255;
-	fsensor_yd_max = 0;
+#ifdef FILAMENT_SENSOR
+	fsensor_oq_meassure_start(40);
+#endif //FILAMENT_SENSOR
 
 	lcd_setstatuspgm(_T(MSG_LOADING_FILAMENT));
 	current_position[E_AXIS] += 40;
@@ -3163,13 +3120,19 @@ void gcode_M701()
 	custom_message = false;
 	custom_message_type = 0;
 
+#ifdef FILAMENT_SENSOR
+	fsensor_oq_meassure_stop();
 
-	fsensor_err_cnt = 0;
-	fsensor_watch_runout = old_watch_runout;
-	printf_P(_N("\nFSENSOR st_sum=%lu yd_sum=%lu er_sum=%lu\n"), fsensor_st_sum, fsensor_yd_sum, fsensor_er_sum);
-	printf_P(_N("\nFSENSOR yd_min=%hhu yd_max=%hhu yd_avg=%hhu\n"), fsensor_yd_min, fsensor_yd_max, fsensor_yd_sum * FSENSOR_CHUNK_LEN / fsensor_st_sum);
-	printf_P(PSTR("gcode_M701 end\n"));
-#endif
+	if (!fsensor_oq_result())
+	{
+		bool disable = lcd_show_fullscreen_message_yes_no_and_wait_P(_i("Fil. sensor response is poor, disable it?"), false, true);
+		lcd_update_enable(true);
+		lcd_update(2);
+		if (disable)
+			fsensor_disable();
+	}
+#endif //FILAMENT_SENSOR
+#endif //defined (SNMM) || defined (SNMM_V2)
 }
 /**
  * @brief Get serial number from 32U2 processor
@@ -3226,6 +3189,9 @@ extern uint8_t st_backlash_x;
 extern uint8_t st_backlash_y;
 #endif //BACKLASH_Y
 
+uint16_t gcode_in_progress = 0;
+uint16_t mcode_in_progress = 0;
+
 void process_commands()
 {
 	if (!buflen) return; //empty command
@@ -3380,8 +3346,6 @@ void process_commands()
 		}	
 		else if (code_seen("MMURES")) {
 			fprintf_P(uart2io, PSTR("X0"));
-			bool response = mmu_get_reponse();
-			if (!response) mmu_not_responding();
 		}
 		else if (code_seen("RESET")) {
             // careful!
@@ -3453,7 +3417,9 @@ void process_commands()
     // nothing, this is a version line
   } else if(code_seen('G'))
   {
-    switch((int)code_value())
+	gcode_in_progress = (int)code_value();
+//	printf_P(_N("BEGIN G-CODE=%u\n"), gcode_in_progress);
+    switch (gcode_in_progress)
     {
     case 0: // G0 -> G1
     case 1: // G1
@@ -4681,10 +4647,13 @@ void process_commands()
 	default:
 		printf_P(PSTR("Unknown G code: %s \n"), cmdbuffer + bufindr + CMDHDRSIZE);
     }
+//	printf_P(_N("END G-CODE=%u\n"), gcode_in_progress);
+	gcode_in_progress = 0;
   } // end if(code_seen('G'))
 
   else if(code_seen('M'))
   {
+
 	  int index;
 	  for (index = 1; *(strchr_pointer + index) == ' ' || *(strchr_pointer + index) == '\t'; index++);
 	   
@@ -4693,7 +4662,11 @@ void process_commands()
 		  printf_P(PSTR("Invalid M code: %s \n"), cmdbuffer + bufindr + CMDHDRSIZE);
 
 	  } else
-    switch((int)code_value())
+	  {
+	  mcode_in_progress = (int)code_value();
+//	printf_P(_N("BEGIN M-CODE=%u\n"), mcode_in_progress);
+
+    switch(mcode_in_progress)
     {
 
     case 0: // M0 - Unconditional stop - Wait for user button press on LCD
@@ -5782,18 +5755,19 @@ Sigma_Exit:
 			{
 				int val = code_value();
 #ifdef TMC2130
+				int val_silent = val;
 				if ((i == X_AXIS) || (i == Y_AXIS))
 				{
-					int max_val = 0;
-					if (tmc2130_mode == TMC2130_MODE_NORMAL)
-						max_val = NORMAL_MAX_ACCEL_XY;
-					else if (tmc2130_mode == TMC2130_MODE_SILENT)
-						max_val = SILENT_MAX_ACCEL_XY;
-					if (val > max_val)
-						val = max_val;
+					if (val > NORMAL_MAX_ACCEL_XY)
+						val = NORMAL_MAX_ACCEL_XY;
+					if (val_silent > SILENT_MAX_ACCEL_XY)
+						val_silent = SILENT_MAX_ACCEL_XY;
 				}
-#endif
+				max_acceleration_units_per_sq_second_normal[i] = val;
+				max_acceleration_units_per_sq_second_silent[i] = val_silent;
+#else //TMC2130
 				max_acceleration_units_per_sq_second[i] = val;
+#endif //TMC2130
 			}
 		}
 		// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
@@ -5813,18 +5787,19 @@ Sigma_Exit:
 			{
 				float val = code_value();
 #ifdef TMC2130
+				float val_silent = val;
 				if ((i == X_AXIS) || (i == Y_AXIS))
 				{
-					float max_val = 0;
-					if (tmc2130_mode == TMC2130_MODE_NORMAL)
-						max_val = NORMAL_MAX_FEEDRATE_XY;
-					else if (tmc2130_mode == TMC2130_MODE_SILENT)
-						max_val = SILENT_MAX_FEEDRATE_XY;
-					if (val > max_val)
-						val = max_val;
+					if (val > NORMAL_MAX_FEEDRATE_XY)
+						val = NORMAL_MAX_FEEDRATE_XY;
+					if (val_silent > SILENT_MAX_FEEDRATE_XY)
+						val_silent = SILENT_MAX_FEEDRATE_XY;
 				}
-#endif //TMC2130
+				max_feedrate_normal[i] = val;
+				max_feedrate_silent[i] = val_silent;
+#else //TMC2130
 				max_feedrate[i] = val;
+#endif //TMC2130
 			}
 		}
 		break;
@@ -6265,10 +6240,6 @@ Sigma_Exit:
     #ifdef FILAMENTCHANGEENABLE
     case 600: //Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
     {
-#ifdef PAT9125
-		bool old_fsensor_enabled = fsensor_enabled;
-//		fsensor_enabled = false; //temporary solution for unexpected restarting
-#endif //PAT9125
 
 		st_synchronize();
 		float lastpos[4];
@@ -6513,7 +6484,7 @@ Sigma_Exit:
 			fprintf_P(uart2io, PSTR("U0\n"));
 
 			// get response
-			bool response = mmu_get_reponse();
+			bool response = mmu_get_reponse(false);
 			if (!response) mmu_not_responding();
 #else
 			lcd_display_message_fullscreen_P(_T(MSG_PULL_OUT_FILAMENT));
@@ -6537,11 +6508,11 @@ Sigma_Exit:
 			//lcd_return_to_status();
 			lcd_update_enable(true);
 		
-        //Wait for user to insert filament and load filament to nozzle
+
 #ifdef SNMM_V2
 		mmu_M600_load_filament();
 #else		
-		M600_load_filament(old_fsensor_enabled);
+		M600_load_filament();
 #endif    
 
 		//Wait for user to check the state
@@ -6947,10 +6918,6 @@ Sigma_Exit:
 			  extr_unload_all(); //unload all filaments
 		}
 #else
-#ifdef PAT9125
-		bool old_fsensor_enabled = fsensor_enabled;
-//		fsensor_enabled = false;
-#endif //PAT9125
 		custom_message = true;
 		custom_message_type = 2;
 		lcd_setstatuspgm(_T(MSG_UNLOADING_FILAMENT)); 
@@ -6992,9 +6959,6 @@ Sigma_Exit:
 		lcd_setstatuspgm(_T(WELCOME_MSG));
 		custom_message = false;
 		custom_message_type = 0;
-#ifdef PAT9125
-//		fsensor_enabled = old_fsensor_enabled;
-#endif //PAT9125
 #endif	
 	}
 	break;
@@ -7008,7 +6972,9 @@ Sigma_Exit:
 	default: 
 		printf_P(PSTR("Unknown M code: %s \n"), cmdbuffer + bufindr + CMDHDRSIZE);
     }
-	
+//	printf_P(_N("END M-CODE=%u\n"), mcode_in_progress);
+	mcode_in_progress = 0;
+	}
   } // end if(code_seen('M')) (end of M codes)
 
   else if(code_seen('T'))
@@ -7033,7 +6999,7 @@ Sigma_Exit:
 		  printf_P(PSTR("T code: %d \n"), tmp_extruder);
 		  fprintf_P(uart2io, PSTR("T%d\n"), tmp_extruder);
 
-		  bool response = mmu_get_reponse();
+		  bool response = mmu_get_reponse(false);
 		  if (!response) mmu_not_responding();
 
     	  snmm_extruder = tmp_extruder; //filament change is finished
@@ -7174,10 +7140,10 @@ Sigma_Exit:
 		dcode_2130(); break;
 #endif //TMC2130
 
-#ifdef PAT9125
-	case 9125: // D9125 - PAT9125
+#ifdef FILAMENT_SENSOR
+	case 9125: // D9125 - FILAMENT_SENSOR
 		dcode_9125(); break;
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 
 	}
   }
@@ -7524,45 +7490,40 @@ static void handleSafetyTimer()
 
 void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument set in Marlin.h
 {
-#ifdef PAT9125
-	if (fsensor_enabled && filament_autoload_enabled && fsensor_watch_runout && !moves_planned() && !IS_SD_PRINTING && !is_usb_printing && (lcd_commands_type != LCD_COMMAND_V2_CAL))
+#ifdef FILAMENT_SENSOR
+	if (mcode_in_progress != 600) //M600 not in progress
 	{
-		if (fsensor_autoload_enabled)
+		if (!moves_planned() && !IS_SD_PRINTING && !is_usb_printing && (lcd_commands_type != LCD_COMMAND_V2_CAL))
 		{
 			if (fsensor_check_autoload())
 			{
-                
-                if (degHotend0() > EXTRUDE_MINTEMP)
-                {
-                    fsensor_autoload_check_stop();
-                    tone(BEEPER, 1000);
-                    delay_keep_alive(50);
-                    noTone(BEEPER);
-                    loading_flag = true;
-                    enquecommand_front_P((PSTR("M701")));
-                }
-                else
-                {
-                    lcd_update_enable(false);
-                    lcd_clear();
-                    lcd_set_cursor(0, 0);
-                    lcd_puts_P(_T(MSG_ERROR));
-                    lcd_set_cursor(0, 2);
-                    lcd_puts_P(_T(MSG_PREHEAT_NOZZLE));
-                    delay(2000);
-                    lcd_clear();
-                    lcd_update_enable(true);
-                }
-                
+				fsensor_autoload_check_stop();
+				if (degHotend0() > EXTRUDE_MINTEMP)
+				{
+					tone(BEEPER, 1000);
+					delay_keep_alive(50);
+					noTone(BEEPER);
+					loading_flag = true;
+					enquecommand_front_P((PSTR("M701")));
+				}
+				else
+				{
+					lcd_update_enable(false);
+					lcd_clear();
+					lcd_set_cursor(0, 0);
+					lcd_puts_P(_T(MSG_ERROR));
+					lcd_set_cursor(0, 2);
+					lcd_puts_P(_T(MSG_PREHEAT_NOZZLE));
+					delay(2000);
+					lcd_clear();
+					lcd_update_enable(true);
+				}
 			}
 		}
 		else
-			fsensor_autoload_check_start();
-	}
-	else
-		if (fsensor_autoload_enabled)
 			fsensor_autoload_check_stop();
-#endif //PAT9125
+	}
+#endif //FILAMENT_SENSOR
 
 #ifdef SAFETYTIMER
 	handleSafetyTimer();
@@ -9034,7 +8995,7 @@ static void print_time_remaining_init() {
 	print_percent_done_silent = PRINT_PERCENT_DONE_INIT;
 }
 
-bool mmu_get_reponse() {
+bool mmu_get_reponse(bool timeout) {
 	bool response = true;
 	LongTimer mmu_get_reponse_timeout;
     uart2_rx_clr();
@@ -9043,7 +9004,7 @@ bool mmu_get_reponse() {
 	while (!uart2_rx_ok())
     {
       delay_keep_alive(100);
-	  if (mmu_get_reponse_timeout.expired(180 * 1000ul)) { //3 minutes timeout
+	  if (timeout && mmu_get_reponse_timeout.expired(180 * 1000ul)) { //3 minutes timeout
 			response = false;
 			break;
 	  }
@@ -9106,7 +9067,7 @@ void mmu_M600_load_filament() {
 		  snmm_filaments_used |= (1 << tmp_extruder); //for stop print
 		  printf_P(PSTR("T code: %d \n"), tmp_extruder);
 		  fprintf_P(uart2io, PSTR("T%d\n"), tmp_extruder);
-		  response = mmu_get_reponse();
+		  response = mmu_get_reponse(false);
 		  if (!response) mmu_not_responding();
 
     	  snmm_extruder = tmp_extruder; //filament change is finished
@@ -9144,41 +9105,57 @@ void M600_load_filament_movements() {
                      lcd_loading_filament();
 }
 
-void M600_load_filament(bool fsensor_enabled) {
+void M600_load_filament()
+{
+	lcd_wait_interact();
 
-		lcd_wait_interact();
+	//load_filament_time = millis();
+	KEEPALIVE_STATE(PAUSED_FOR_USER);
 
-		//load_filament_time = millis();
-		KEEPALIVE_STATE(PAUSED_FOR_USER);
-
-#ifdef PAT9125
-		if (filament_autoload_enabled && (fsensor_enabled || fsensor_watch_runout)) fsensor_autoload_check_start();
-#endif //PAT9125
-        while(!lcd_clicked())
+#ifdef FILAMENT_SENSOR
+	fsensor_autoload_check_start();
+#endif //FILAMENT_SENSOR
+	while(!lcd_clicked())
+	{
+		manage_heater();
+		manage_inactivity(true);
+#ifdef FILAMENT_SENSOR
+		if (fsensor_check_autoload())
 		{
-          manage_heater();
-          manage_inactivity(true);
-#ifdef PAT9125
-		  if (filament_autoload_enabled && (fsensor_enabled || fsensor_watch_runout) && fsensor_check_autoload())
-		  {
 			tone(BEEPER, 1000);
 			delay_keep_alive(50);
 			noTone(BEEPER);
-			  break;
-		  }
-#endif //PAT9125
+			break;
+		}
+#endif //FILAMENT_SENSOR
+	}
+#ifdef FILAMENT_SENSOR
+	fsensor_autoload_check_stop();
+#endif //FILAMENT_SENSOR
+	KEEPALIVE_STATE(IN_HANDLER);
 
-        }
-#ifdef PAT9125
-		if (filament_autoload_enabled && (fsensor_enabled || fsensor_watch_runout)) fsensor_autoload_check_stop();
-#endif //PAT9125
-		KEEPALIVE_STATE(IN_HANDLER);
+#ifdef FILAMENT_SENSOR
+	fsensor_oq_meassure_start(70);
+#endif //FILAMENT_SENSOR
 
-		M600_load_filament_movements();
+	M600_load_filament_movements();
 
-		tone(BEEPER, 500);
-		delay_keep_alive(50);
-		noTone(BEEPER);
+	tone(BEEPER, 500);
+	delay_keep_alive(50);
+	noTone(BEEPER);
+
+#ifdef FILAMENT_SENSOR
+	fsensor_oq_meassure_stop();
+
+	if (!fsensor_oq_result())
+	{
+		bool disable = lcd_show_fullscreen_message_yes_no_and_wait_P(_i("Fil. sensor response is poor, disable it?"), false, true);
+		lcd_update_enable(true);
+		lcd_update(2);
+		if (disable)
+			fsensor_disable();
+	}
+#endif //FILAMENT_SENSOR
 
 }
 
diff --git a/Firmware/config.h b/Firmware/config.h
index 6ee4dd8d1..5ba843c53 100644
--- a/Firmware/config.h
+++ b/Firmware/config.h
@@ -22,7 +22,7 @@
 //#define PAT9125_I2C_ADDR  0x79  //ID=HI
 //#define PAT9125_I2C_ADDR  0x73  //ID=NC
 #define PAT9125_XRES      0
-#define PAT9125_YRES      200
+#define PAT9125_YRES      240
 
 //SM4 configuration
 #define SM4_DEFDELAY      500       //default step delay [us]
diff --git a/Firmware/fsensor.cpp b/Firmware/fsensor.cpp
index c2291235a..a280eda19 100644
--- a/Firmware/fsensor.cpp
+++ b/Firmware/fsensor.cpp
@@ -1,19 +1,29 @@
 #include "Marlin.h"
 
-#ifdef PAT9125
-
 #include "fsensor.h"
+#include <avr/pgmspace.h>
 #include "pat9125.h"
 #include "stepper.h"
 #include "planner.h"
 #include "fastio.h"
 #include "cmdqueue.h"
 
+//Basic params
+#define FSENSOR_CHUNK_LEN      180  //filament sensor chunk length in steps - 0.64mm
+#define FSENSOR_ERR_MAX         10  //filament sensor maximum error count for runout detection
+
+//Optical quality meassurement params
+#define FSENSOR_OQ_MAX_ES      6    //maximum error sum while loading (length ~64mm = 100chunks)
+#define FSENSOR_OQ_MAX_EM      2    //maximum error counter value while loading
+#define FSENSOR_OQ_MIN_YD      2    //minimum yd per chunk (applied to avg value)
+#define FSENSOR_OQ_MAX_YD      200  //maximum yd per chunk (applied to avg value)
+#define FSENSOR_OQ_MAX_PD      4    //maximum positive deviation (= yd_max/yd_avg)
+#define FSENSOR_OQ_MAX_ND      5    //maximum negative deviation (= yd_avg/yd_min)
+#define FSENSOR_OQ_MAX_SH      13   //maximum shutter value
+
 
 const char ERRMSG_PAT9125_NOT_RESP[] PROGMEM = "PAT9125 not responding (%d)!\n";
 
-//#define FSENSOR_ERR_MAX          5  //filament sensor max error count
-#define FSENSOR_ERR_MAX         10  //filament sensor max error count
 #define FSENSOR_INT_PIN         63  //filament sensor interrupt pin PK1
 #define FSENSOR_INT_PIN_MSK   0x02  //filament sensor interrupt pin mask (bit1)
 
@@ -21,12 +31,12 @@ extern void stop_and_save_print_to_ram(float z_move, float e_move);
 extern void restore_print_from_ram_and_continue(float e_move);
 extern int8_t FSensorStateMenu;
 
-void fsensor_stop_and_save_print()
+void fsensor_stop_and_save_print(void)
 {
 	stop_and_save_print_to_ram(0, 0); //XYZE - no change	
 }
 
-void fsensor_restore_print_and_continue()
+void fsensor_restore_print_and_continue(void)
 {
 	restore_print_from_ram_and_continue(0); //XYZ = orig, E - no change
 }
@@ -35,55 +45,109 @@ void fsensor_restore_print_and_continue()
 uint8_t fsensor_int_pin_old = 0;
 int16_t fsensor_chunk_len = FSENSOR_CHUNK_LEN;
 
+//enabled = initialized and sampled every chunk event
 bool fsensor_enabled = true;
+//runout watching is done in fsensor_update (called from main loop)
 bool fsensor_watch_runout = true;
+//not responding - is set if any communication error occured durring initialization or readout
 bool fsensor_not_responding = false;
+//printing saved
+bool fsensor_printing_saved = false;
 
+//number of errors, updated in ISR
 uint8_t fsensor_err_cnt = 0;
+//variable for accumolating step count (updated callbacks from stepper and ISR)
 int16_t fsensor_st_cnt = 0;
+//last dy value from pat9125 sensor (used in ISR)
+uint8_t fsensor_dy_old = 0;
+
+//log flag: 0=log disabled, 1=log enabled
+uint8_t fsensor_log = 0;
+
+////////////////////////////////////////////////////////////////////////////////
+//filament autoload variables
+
+//autoload feature enabled
+bool fsensor_autoload_enabled = true;
+
+//autoload watching enable/disable flag
+bool fsensor_watch_autoload = false;
+//
+uint16_t fsensor_autoload_y;
+//
+uint8_t fsensor_autoload_c;
+//
+uint32_t fsensor_autoload_last_millis;
+//
+uint8_t fsensor_autoload_sum;
+
+////////////////////////////////////////////////////////////////////////////////
+//filament optical quality meassurement variables
+
+//meassurement enable/disable flag
+bool fsensor_oq_meassure = false;
+//skip-chunk counter, for accurate meassurement is necesary to skip first chunk...
+uint8_t  fsensor_oq_skipchunk;
+//number of samples from start of meassurement
+uint8_t fsensor_oq_samples;
+//sum of steps in positive direction movements
+uint16_t fsensor_oq_st_sum;
+//sum of deltas in positive direction movements
+uint16_t fsensor_oq_yd_sum;
+//sum of errors durring meassurement
+uint16_t fsensor_oq_er_sum;
+//max error counter value durring meassurement
+uint8_t  fsensor_oq_er_max;
+//minimum delta value
+uint16_t fsensor_oq_yd_min;
+//maximum delta value
+uint16_t fsensor_oq_yd_max;
+//sum of shutter value
+uint16_t fsensor_oq_sh_sum;
 
 
-uint8_t fsensor_log = 1;
-
-//autoload enable/disable flag
-bool fsensor_autoload_enabled = false;
-uint16_t fsensor_autoload_y = 0;
-uint8_t fsensor_autoload_c = 0;
-uint32_t fsensor_autoload_last_millis = 0;
-uint8_t fsensor_autoload_sum = 0;
-
-uint32_t fsensor_st_sum = 0;
-uint32_t fsensor_yd_sum = 0;
-uint32_t fsensor_er_sum = 0;
-uint8_t fsensor_yd_min = 255;
-uint8_t fsensor_yd_max = 0;
-
-bool fsensor_enable()
+void fsensor_init(void)
 {
-//	puts_P(PSTR("fsensor_enable\n"));
 	int pat9125 = pat9125_init();
-    printf_P(PSTR("PAT9125_init:%d\n"), pat9125);
+	printf_P(_N("PAT9125_init:%d\n"), pat9125);
+	uint8_t fsensor = eeprom_read_byte((uint8_t*)EEPROM_FSENSOR);
+	fsensor_autoload_enabled=eeprom_read_byte((uint8_t*)EEPROM_FSENS_AUTOLOAD_ENABLED);
+	if (!pat9125)
+	{
+		fsensor = 0; //disable sensor
+		fsensor_not_responding = true;
+	}
+	else
+		fsensor_not_responding = false;
+	if (fsensor)
+		fsensor_enable();
+	else
+		fsensor_disable();
+	printf_P(PSTR("FSensor %S\n"), (fsensor_enabled?PSTR("ENABLED"):PSTR("DISABLED\n")));
+}
+
+bool fsensor_enable(void)
+{
+	uint8_t pat9125 = pat9125_init();
+    printf_P(PSTR("PAT9125_init:%hhu\n"), pat9125);
 	if (pat9125)
 		fsensor_not_responding = false;
 	else
 		fsensor_not_responding = true;
 	fsensor_enabled = pat9125?true:false;
 	fsensor_watch_runout = true;
+	fsensor_oq_meassure = false;
 	fsensor_err_cnt = 0;
+	fsensor_dy_old = 0;
 	eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, fsensor_enabled?0x01:0x00); 
 	FSensorStateMenu = fsensor_enabled?1:0;
-//	printf_P(PSTR("fsensor_enable - end %d\n"), fsensor_enabled?1:0);
 
-	fsensor_st_sum = 0;
-	fsensor_yd_sum = 0;
-	fsensor_er_sum = 0;
 
 	return fsensor_enabled;
 }
 
-void fsensor_disable()
+void fsensor_disable(void)
 {
-//	puts_P(PSTR("fsensor_disable\n"));
 	fsensor_enabled = false;
 	eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, 0x00); 
 	FSensorStateMenu = 0;
@@ -91,8 +155,8 @@ void fsensor_disable()
 
 void fsensor_autoload_set(bool State)
 {
-	filament_autoload_enabled = State;
-	eeprom_update_byte((unsigned char *)EEPROM_FSENS_AUTOLOAD_ENABLED, filament_autoload_enabled);
+	fsensor_autoload_enabled = State;
+	eeprom_update_byte((unsigned char *)EEPROM_FSENS_AUTOLOAD_ENABLED, fsensor_autoload_enabled);
 }
 
 void pciSetup(byte pin)
@@ -102,64 +166,70 @@ void pciSetup(byte pin)
 	PCICR |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group 
 }
 
-void fsensor_setup_interrupt()
-{
-//	uint8_t fsensor_int_pin = FSENSOR_INT_PIN;
-//	uint8_t fsensor_int_pcmsk = digitalPinToPCMSKbit(pin);
-//	uint8_t fsensor_int_pcicr = digitalPinToPCICRbit(pin);
-
-	pinMode(FSENSOR_INT_PIN, OUTPUT);
-	digitalWrite(FSENSOR_INT_PIN, LOW);
-	fsensor_int_pin_old = 0;
-
-	pciSetup(FSENSOR_INT_PIN);
-}
-
 void fsensor_autoload_check_start(void)
 {
-//	puts_P(PSTR("fsensor_autoload_check_start\n"));
+//	puts_P(_N("fsensor_autoload_check_start\n"));
+	if (!fsensor_enabled) return;
+	if (!fsensor_autoload_enabled) return;
+	if (fsensor_watch_autoload) return;
 	if (!pat9125_update_y()) //update sensor
 	{
-		printf_P(ERRMSG_PAT9125_NOT_RESP, 3);
 		fsensor_disable();
 		fsensor_not_responding = true;
-		fsensor_autoload_enabled = false;
+		fsensor_watch_autoload = false;
+		printf_P(ERRMSG_PAT9125_NOT_RESP, 3);
 		return;
 	}
+	puts_P(_N("fsensor_autoload_check_start - autoload ENABLED\n"));
 	fsensor_autoload_y = pat9125_y; //save current y value
 	fsensor_autoload_c = 0; //reset number of changes counter
 	fsensor_autoload_sum = 0;
 	fsensor_autoload_last_millis = millis();
-	fsensor_autoload_enabled = true;
+	fsensor_watch_runout = false;
+	fsensor_watch_autoload = true;
 	fsensor_err_cnt = 0;
 }
 
 void fsensor_autoload_check_stop(void)
 {
-//	puts_P(PSTR("fsensor_autoload_check_stop\n"));
+//	puts_P(_N("fsensor_autoload_check_stop\n"));
+	if (!fsensor_enabled) return;
+//	puts_P(_N("fsensor_autoload_check_stop 1\n"));
+	if (!fsensor_autoload_enabled) return;
+//	puts_P(_N("fsensor_autoload_check_stop 2\n"));
+	if (!fsensor_watch_autoload) return;
+	puts_P(_N("fsensor_autoload_check_stop - autoload DISABLED\n"));
 	fsensor_autoload_sum = 0;
-	fsensor_autoload_enabled = false;
+	fsensor_watch_autoload = false;
+	fsensor_watch_runout = true;
 	fsensor_err_cnt = 0;
 }
 
 bool fsensor_check_autoload(void)
 {
+	if (!fsensor_enabled) return false;
+	if (!fsensor_autoload_enabled) return false;
+	if (!fsensor_watch_autoload)
+	{
+		fsensor_autoload_check_start();
+		return false;
+	}
 	uint8_t fsensor_autoload_c_old = fsensor_autoload_c;
 	if ((millis() - fsensor_autoload_last_millis) < 25) return false;
 	fsensor_autoload_last_millis = millis();
-	if (!pat9125_update_y())
+	if (!pat9125_update_y()) //update sensor
 	{
-		printf_P(ERRMSG_PAT9125_NOT_RESP, 2);
 		fsensor_disable();
 		fsensor_not_responding = true;
-		return false; //update sensor
+		printf_P(ERRMSG_PAT9125_NOT_RESP, 2);
+		return false;
 	}
-	int16_t dy = fsensor_autoload_y - pat9125_y;
-	if (dy) //? y value is different
+	int16_t dy = pat9125_y - fsensor_autoload_y;
+	if (dy) //? dy value is nonzero
 	{
-		if (dy < 0) //? delta-y value is positive (inserting)
+		if (dy > 0) //? delta-y value is positive (inserting)
 		{
-			fsensor_autoload_sum -= dy;
+			fsensor_autoload_sum += dy;
 			fsensor_autoload_c += 3; //increment change counter by 3
 		}
 		else if (fsensor_autoload_c > 1)
@@ -169,13 +239,71 @@ bool fsensor_check_autoload(void)
 	else if (fsensor_autoload_c > 0)
 		fsensor_autoload_c--;
 	if (fsensor_autoload_c == 0) fsensor_autoload_sum = 0;
-//	if (fsensor_autoload_c != fsensor_autoload_c_old)
-//		printf_P(PSTR("fsensor_check_autoload dy=%d c=%d sum=%d\n"), dy, fsensor_autoload_c, fsensor_autoload_sum);
-	if ((fsensor_autoload_c >= 15) && (fsensor_autoload_sum > 30))
+//	puts_P(_N("fsensor_check_autoload\n"));
+	if (fsensor_autoload_c != fsensor_autoload_c_old)
+		printf_P(PSTR("fsensor_check_autoload dy=%d c=%d sum=%d\n"), dy, fsensor_autoload_c, fsensor_autoload_sum);
+//	if ((fsensor_autoload_c >= 15) && (fsensor_autoload_sum > 30))
+	if ((fsensor_autoload_c >= 10) && (fsensor_autoload_sum > 15))
+	{
+		puts_P(_N("fsensor_check_autoload = true !!!\n"));
 		return true;
+	}
 	return false;
 }
 
+void fsensor_oq_meassure_start(uint8_t skip)
+{
+	printf_P(PSTR("fsensor_oq_meassure_start\n"));
+	fsensor_oq_skipchunk = skip;
+	fsensor_oq_samples = 0;
+	fsensor_oq_st_sum = 0;
+	fsensor_oq_yd_sum = 0;
+	fsensor_oq_er_sum = 0;
+	fsensor_oq_er_max = 0;
+	fsensor_oq_yd_min = FSENSOR_OQ_MAX_YD;
+	fsensor_oq_yd_max = 0;
+	fsensor_oq_sh_sum = 0;
+	pat9125_update();
+	pat9125_y = 0;
+	fsensor_watch_runout = false;
+	fsensor_oq_meassure = true;
+}
+
+void fsensor_oq_meassure_stop(void)
+{
+	printf_P(PSTR("fsensor_oq_meassure_stop, %hhu samples\n"), fsensor_oq_samples);
+	printf_P(_N(" st_sum=%u yd_sum=%u er_sum=%u er_max=%hhu\n"), fsensor_oq_st_sum, fsensor_oq_yd_sum, fsensor_oq_er_sum, fsensor_oq_er_max);
+	printf_P(_N(" yd_min=%u yd_max=%u yd_avg=%u sh_avg=%u\n"), fsensor_oq_yd_min, fsensor_oq_yd_max, (uint16_t)((uint32_t)fsensor_oq_yd_sum * FSENSOR_CHUNK_LEN / fsensor_oq_st_sum), (uint16_t)(fsensor_oq_sh_sum / fsensor_oq_samples));
+	fsensor_oq_meassure = false;
+	fsensor_watch_runout = true;
+	fsensor_err_cnt = 0;
+}
+
+const char _OK[] PROGMEM = "OK";
+const char _NG[] PROGMEM = "NG!";
+
+bool fsensor_oq_result(void)
+{
+	printf_P(_N("fsensor_oq_result\n"));
+	bool res_er_sum = (fsensor_oq_er_sum <= FSENSOR_OQ_MAX_ES);
+	printf_P(_N(" er_sum = %u %S\n"), fsensor_oq_er_sum, (res_er_sum?_OK:_NG));
+	bool res_er_max = (fsensor_oq_er_max <= FSENSOR_OQ_MAX_EM);
+	printf_P(_N(" er_max = %hhu %S\n"), fsensor_oq_er_max, (res_er_max?_OK:_NG));
+	uint8_t yd_avg = ((uint32_t)fsensor_oq_yd_sum * FSENSOR_CHUNK_LEN / fsensor_oq_st_sum);
+	bool res_yd_avg = (yd_avg >= FSENSOR_OQ_MIN_YD) && (yd_avg <= FSENSOR_OQ_MAX_YD);
+	printf_P(_N(" yd_avg = %hhu %S\n"), yd_avg, (res_yd_avg?_OK:_NG));
+	bool res_yd_max = (fsensor_oq_yd_max <= (yd_avg * FSENSOR_OQ_MAX_PD));
+	printf_P(_N(" yd_max = %u %S\n"), fsensor_oq_yd_max, (res_yd_max?_OK:_NG));
+	bool res_yd_min = (fsensor_oq_yd_min >= (yd_avg / FSENSOR_OQ_MAX_ND));
+	printf_P(_N(" yd_min = %u %S\n"), fsensor_oq_yd_min, (res_yd_min?_OK:_NG));
+	uint8_t sh_avg = (fsensor_oq_sh_sum / fsensor_oq_samples);
+	bool res_sh_avg = (sh_avg <= FSENSOR_OQ_MAX_SH);
+	printf_P(_N(" sh_avg = %hhu %S\n"), sh_avg, (res_sh_avg?_OK:_NG));
+	bool res = res_er_sum && res_er_max && res_yd_avg && res_yd_max && res_yd_min && res_sh_avg;
+	printf_P(_N("fsensor_oq_result %S\n"), (res?_OK:_NG));
+	return res;
+}
+
 ISR(PCINT2_vect)
 {
 	if (!((fsensor_int_pin_old ^ PINK) & FSENSOR_INT_PIN_MSK)) return;
@@ -187,33 +315,51 @@ ISR(PCINT2_vect)
 	fsensor_st_cnt = 0;
 	sei();
 	uint8_t old_err_cnt = fsensor_err_cnt;
-	if (!pat9125_update_y())
+	uint8_t pat9125_res = fsensor_oq_meassure?pat9125_update():pat9125_update_y();
+	if (!pat9125_res)
 	{
-		printf_P(ERRMSG_PAT9125_NOT_RESP, 1);
 		fsensor_disable();
 		fsensor_not_responding = true;
+		printf_P(ERRMSG_PAT9125_NOT_RESP, 1);
 	}
 	if (st_cnt != 0)
 	{ //movement
 		if (st_cnt > 0) //positive movement
 		{
-			if (pat9125_y <= 0)
-			{
+			if (pat9125_y < 0)
 				fsensor_err_cnt++;
-				fsensor_er_sum++;
-			}
-			else
+			else if (pat9125_y > 0)
 			{
 				if (fsensor_err_cnt)
 					fsensor_err_cnt--;
-				if (st_cnt == FSENSOR_CHUNK_LEN)
+			}
+			else //(pat9125_y == 0)
+				if (fsensor_dy_old <= 0)
+					fsensor_err_cnt++;
+			if (fsensor_oq_meassure)
+			{
+				if (fsensor_oq_skipchunk)
 				{
-					if (fsensor_yd_min > pat9125_y) fsensor_yd_min = pat9125_y;
-					if (fsensor_yd_max < pat9125_y) fsensor_yd_max = pat9125_y;
+					fsensor_oq_skipchunk--;
+					fsensor_err_cnt = 0;
+				}
+				else
+				{
+					if (st_cnt == FSENSOR_CHUNK_LEN)
+					{
+						if (pat9125_y > 0) if (fsensor_oq_yd_min > pat9125_y) fsensor_oq_yd_min = (fsensor_oq_yd_min + pat9125_y) / 2;
+						if (pat9125_y >= 0) if (fsensor_oq_yd_max < pat9125_y) fsensor_oq_yd_max = (fsensor_oq_yd_max + pat9125_y) / 2;
+					}
+					fsensor_oq_samples++;
+					fsensor_oq_st_sum += st_cnt;
+					fsensor_oq_yd_sum += pat9125_y;
+					if (fsensor_err_cnt > old_err_cnt)
+						fsensor_oq_er_sum += (fsensor_err_cnt - old_err_cnt);
+					if (fsensor_oq_er_max < fsensor_err_cnt)
+						fsensor_oq_er_max = fsensor_err_cnt;
+					fsensor_oq_sh_sum += pat9125_s;
 				}
 			}
-			fsensor_st_sum += st_cnt;
-			fsensor_yd_sum += pat9125_y;
 		}
 		else //negative movement
 		{
@@ -226,12 +372,14 @@ ISR(PCINT2_vect)
 #ifdef DEBUG_FSENSOR_LOG
 	if (fsensor_log)
 	{
-		printf_P(_N("FSENSOR cnt=%d dy=%d err=%d %S\n"), st_cnt, pat9125_y, fsensor_err_cnt, (fsensor_err_cnt > old_err_cnt)?_N("NG!"):_N("OK"));
-		printf_P(_N("FSENSOR st_sum=%lu yd_sum=%lu er_sum=%lu\n"), fsensor_st_sum, fsensor_yd_sum, fsensor_er_sum);
+		printf_P(_N("FSENSOR cnt=%d dy=%d err=%hhu %S\n"), st_cnt, pat9125_y, fsensor_err_cnt, (fsensor_err_cnt > old_err_cnt)?_N("NG!"):_N("OK"));
+		if (fsensor_oq_meassure) printf_P(_N("FSENSOR st_sum=%u yd_sum=%u er_sum=%u er_max=%hhu yd_max=%u\n"), fsensor_oq_st_sum, fsensor_oq_yd_sum, fsensor_oq_er_sum, fsensor_oq_er_max, fsensor_oq_yd_max);
 	}
 #endif //DEBUG_FSENSOR_LOG
 
+	fsensor_dy_old = pat9125_y;
 	pat9125_y = 0;
+
 	_lock = false;
 	return;
 }
@@ -245,8 +393,6 @@ void fsensor_st_block_begin(block_t* bl)
 		if (_READ(63)) _WRITE(63, LOW);
 		else _WRITE(63, HIGH);
 	}
-//		PINK |= FSENSOR_INT_PIN_MSK; //toggle pin
-//		_WRITE(fsensor_int_pin, LOW);
 }
 
 void fsensor_st_block_chunk(block_t* bl, int cnt)
@@ -258,16 +404,23 @@ void fsensor_st_block_chunk(block_t* bl, int cnt)
 		if (_READ(63)) _WRITE(63, LOW);
 		else _WRITE(63, HIGH);
 	}
-//		PINK |= FSENSOR_INT_PIN_MSK; //toggle pin
-//		_WRITE(fsensor_int_pin, LOW);
 }
 
-void fsensor_update()
+void fsensor_update(void)
 {
-	if (fsensor_enabled && fsensor_watch_runout)
-		if (fsensor_err_cnt > FSENSOR_ERR_MAX)
+	if (fsensor_enabled)
+	{
+		if (fsensor_printing_saved)
+		{
+			fsensor_printing_saved = false;
+			fsensor_watch_runout = true;
+			fsensor_err_cnt = 0;
+			fsensor_restore_print_and_continue();
+		}
+		else if (fsensor_watch_runout && (fsensor_err_cnt > FSENSOR_ERR_MAX))
 		{
 			fsensor_stop_and_save_print();
+			fsensor_printing_saved = true;
 
 			fsensor_err_cnt = 0;
 
@@ -293,6 +446,15 @@ void fsensor_update()
 				fsensor_watch_runout = false;
 			}
 		}
+	}
 }
 
-#endif //PAT9125
+void fsensor_setup_interrupt(void)
+{
+
+	pinMode(FSENSOR_INT_PIN, OUTPUT);
+	digitalWrite(FSENSOR_INT_PIN, LOW);
+	fsensor_int_pin_old = 0;
+
+	pciSetup(FSENSOR_INT_PIN);
+}
diff --git a/Firmware/fsensor.h b/Firmware/fsensor.h
index eda8d43fe..df58def28 100644
--- a/Firmware/fsensor.h
+++ b/Firmware/fsensor.h
@@ -1,60 +1,52 @@
 #ifndef FSENSOR_H
 #define FSENSOR_H
 
-#include "planner.h"
+#include <inttypes.h>
 
-//#define FSENSOR_CHUNK_LEN      280  //filament sensor chunk length in steps - 1mm
-#define FSENSOR_CHUNK_LEN      180  //filament sensor chunk length in steps - 0.64mm
-
-//save restore printing
-extern void fsensor_stop_and_save_print();
-extern void fsensor_restore_print_and_continue();
-
-//enable/disable
-extern bool fsensor_enable();
-extern void fsensor_disable();
-
-extern bool filament_autoload_enabled;
-extern void fsensor_autoload_set(bool State);
-
-//update (perform M600 on filament runout)
-extern void fsensor_update();
-
-//setup pin-change interrupt
-extern void fsensor_setup_interrupt();
-
-//
-extern void fsensor_autoload_check_start(void);
-
-//
-extern void fsensor_autoload_check_stop(void);
-
-//
-extern bool fsensor_check_autoload(void);
-
-//callbacks from stepper
-extern void fsensor_st_block_begin(block_t* bl);
-extern void fsensor_st_block_chunk(block_t* bl, int cnt);
 
 //minimum meassured chunk length in steps
 extern int16_t fsensor_chunk_len;
 //enable/disable flag
 extern bool fsensor_enabled;
-//watch runout flag
-extern bool fsensor_watch_runout;
 //not responding flag
 extern bool fsensor_not_responding;
 
-extern uint8_t fsensor_err_cnt;
 
-//autoload enable/disable flag
+//save restore printing
+extern void fsensor_stop_and_save_print(void);
+extern void fsensor_restore_print_and_continue(void);
+
+//initialize
+extern void fsensor_init(void);
+
+//enable/disable
+extern bool fsensor_enable(void);
+extern void fsensor_disable(void);
+
+//autoload feature enabled
 extern bool fsensor_autoload_enabled;
+extern void fsensor_autoload_set(bool State);
 
+//update (perform M600 on filament runout)
+extern void fsensor_update(void);
+
+//setup pin-change interrupt
+extern void fsensor_setup_interrupt(void);
+
+//autoload support
+extern void fsensor_autoload_check_start(void);
+extern void fsensor_autoload_check_stop(void);
+extern bool fsensor_check_autoload(void);
+
+//optical quality meassurement support
+extern void fsensor_oq_meassure_start(uint8_t skip);
+extern void fsensor_oq_meassure_stop(void);
+extern bool fsensor_oq_result(void);
+
+//callbacks from stepper
+#include "planner.h"
+extern void fsensor_st_block_begin(block_t* bl);
+extern void fsensor_st_block_chunk(block_t* bl, int cnt);
 
-extern uint32_t fsensor_st_sum;
-extern uint32_t fsensor_yd_sum;
-extern uint32_t fsensor_er_sum;
-extern uint8_t fsensor_yd_min;
-extern uint8_t fsensor_yd_max;
 
 #endif //FSENSOR_H
diff --git a/Firmware/pat9125.c b/Firmware/pat9125.c
index c800f4a4c..e917c45cf 100644
--- a/Firmware/pat9125.c
+++ b/Firmware/pat9125.c
@@ -99,6 +99,9 @@ uint8_t pat9125_rd_reg(uint8_t addr);
 void pat9125_wr_reg(uint8_t addr, uint8_t data);
 uint8_t pat9125_wr_reg_verify(uint8_t addr, uint8_t data);
 
+extern FILE _uartout;
+#define uartout (&_uartout)
+
 
 uint8_t pat9125_init(void)
 {
@@ -162,8 +165,11 @@ uint8_t pat9125_init(void)
 	pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
 	pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
 #endif //PAT9125_NEW_INIT
-	pat9125_wr_reg(PAT9125_RES_X, 0);
-	pat9125_wr_reg(PAT9125_RES_Y, 200);
+
+	pat9125_wr_reg(PAT9125_RES_X, PAT9125_XRES);
+	pat9125_wr_reg(PAT9125_RES_Y, PAT9125_YRES);
+	fprintf_P(uartout, PSTR("PAT9125_RES_X=%hhu\n"), pat9125_rd_reg(PAT9125_RES_X));
+	fprintf_P(uartout, PSTR("PAT9125_RES_Y=%hhu\n"), pat9125_rd_reg(PAT9125_RES_Y));
 	return 1;
 }
 
diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index 9b08ea67f..e89cbef8d 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -109,6 +109,8 @@ static float previous_speed[NUM_AXIS]; // Speed of previous path line segment
 static float previous_nominal_speed; // Nominal speed of previous path line segment
 static float previous_safe_speed; // Exit speed limited by a jerk to full halt of a previous last segment.
 
+uint8_t maxlimit_status;
+
 #ifdef AUTOTEMP
 float autotemp_max=250;
 float autotemp_min=210;
@@ -961,11 +963,15 @@ Having the real displacement of the head, we can calculate the total movement le
   // Calculate and limit speed in mm/sec for each axis
   float current_speed[4];
   float speed_factor = 1.0; //factor <=1 do decrease speed
+//  maxlimit_status &= ~0xf;
   for(int i=0; i < 4; i++)
   {
     current_speed[i] = delta_mm[i] * inverse_second;
 	if(fabs(current_speed[i]) > max_feedrate[i])
+	{
       speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
+	  maxlimit_status |= (1 << i);
+	}
   }
 
   // Correct the speed  
@@ -993,13 +999,13 @@ Having the real displacement of the head, we can calculate the total movement le
     // Limit acceleration per axis
     //FIXME Vojtech: One shall rather limit a projection of the acceleration vector instead of using the limit.
     if(((float)block->acceleration_st * (float)block->steps_x.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[X_AXIS])
-      block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
+	{  block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; maxlimit_status |= (X_AXIS_MASK << 4); }
     if(((float)block->acceleration_st * (float)block->steps_y.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[Y_AXIS])
-      block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
+	{  block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; maxlimit_status |= (Y_AXIS_MASK << 4); }
     if(((float)block->acceleration_st * (float)block->steps_e.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[E_AXIS])
-      block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
+	{  block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; maxlimit_status |= (Z_AXIS_MASK << 4); }
     if(((float)block->acceleration_st * (float)block->steps_z.wide / (float)block->step_event_count.wide ) > axis_steps_per_sqr_second[Z_AXIS])
-      block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
+	{  block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; maxlimit_status |= (E_AXIS_MASK << 4); }
   }
   // Acceleration of the segment, in mm/sec^2
   block->acceleration = block->acceleration_st / steps_per_mm;
diff --git a/Firmware/planner.h b/Firmware/planner.h
index 779fc79b1..8d648c8e8 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -182,6 +182,8 @@ extern float mintravelfeedrate;
 extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];
 
 extern long position[NUM_AXIS];
+extern uint8_t maxlimit_status;
+
 
 #ifdef AUTOTEMP
     extern bool autotemp_enabled;
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index cd14c5018..b2d71e2a6 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -36,10 +36,10 @@
 #include "tmc2130.h"
 #endif //TMC2130
 
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
 #include "fsensor.h"
 int fsensor_counter = 0; //counter for e-steps
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 
 #ifdef DEBUG_STACK_MONITOR
 uint16_t SP_min = 0x21FF;
@@ -469,10 +469,10 @@ FORCE_INLINE void stepper_next_block()
 	}
 #endif
 
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
     fsensor_counter = 0;
     fsensor_st_block_begin(current_block);
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
     // The busy flag is set by the plan_get_current_block() call.
     // current_block->busy = true;
     // Initializes the trapezoid generator from the current block. Called whenever a new
@@ -760,9 +760,9 @@ FORCE_INLINE void stepper_tick_lowres()
 #ifdef LIN_ADVANCE
       ++ e_steps;
 #else
-  #ifdef PAT9125
+  #ifdef FILAMENT_SENSOR
       ++ fsensor_counter;
-  #endif //PAT9125
+  #endif //FILAMENT_SENSOR
       WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
 #endif
     }
@@ -825,9 +825,9 @@ FORCE_INLINE void stepper_tick_highres()
 #ifdef LIN_ADVANCE
       ++ e_steps;
 #else
-  #ifdef PAT9125
+  #ifdef FILAMENT_SENSOR
       ++ fsensor_counter;
-  #endif //PAT9125
+  #endif //FILAMENT_SENSOR
       WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
 #endif
     }
@@ -900,9 +900,9 @@ FORCE_INLINE void isr() {
         estep_loops = (e_steps & 0x0ff00) ? 4 : e_steps;
         if (step_loops < estep_loops)
           estep_loops = step_loops;
-    #ifdef PAT9125
+    #ifdef FILAMENT_SENSOR
         fsensor_counter += estep_loops;
-    #endif //PAT9125
+    #endif //FILAMENT_SENSOR
         do {
           WRITE_NC(E0_STEP_PIN, !INVERT_E_STEP_PIN);
           -- e_steps;
@@ -1026,9 +1026,9 @@ FORCE_INLINE void isr() {
       if (eISR_Rate == 0) {
         // There is not enough time to fit even a single additional tick.
         // Tick all the extruder ticks now.
-    #ifdef PAT9125
+    #ifdef FILAMENT_SENSOR
         fsensor_counter += e_steps;
-    #endif //PAT9125
+    #endif //FILAMENT_SENSOR
         MSerial.checkRx(); // Check for serial chars.
         do {
           WRITE_NC(E0_STEP_PIN, !INVERT_E_STEP_PIN);
@@ -1048,21 +1048,21 @@ FORCE_INLINE void isr() {
 
     // If current block is finished, reset pointer
     if (step_events_completed.wide >= current_block->step_event_count.wide) {
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
       fsensor_st_block_chunk(current_block, fsensor_counter);
 	    fsensor_counter = 0;
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 
       current_block = NULL;
       plan_discard_current_block();
     }
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
   	else if (fsensor_counter >= fsensor_chunk_len)
   	{
       fsensor_st_block_chunk(current_block, fsensor_counter);
   	  fsensor_counter = 0;
   	}
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
   }
 
 #ifdef TMC2130
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index fe9a636f7..4b47562ed 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -23,9 +23,10 @@
 
 #include "SdFatUtil.h"
 
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
 #include "pat9125.h"
-#endif //PAT9125
+#include "fsensor.h"
+#endif //FILAMENT_SENSOR
 
 #ifdef TMC2130
 #include "tmc2130.h"
@@ -38,12 +39,7 @@
 
 extern int lcd_change_fil_state;
 extern bool fans_check_enabled;
-extern bool filament_autoload_enabled;
 
-#ifdef PAT9125
-extern bool fsensor_not_responding;
-extern bool fsensor_enabled;
-#endif //PAT9125
 
 int scrollstuff = 0;
 char longFilenameOLD[LONG_FILENAME_LENGTH];
@@ -224,9 +220,9 @@ static void lcd_menu_extruder_info();
 static void lcd_menu_xyz_y_min();
 static void lcd_menu_xyz_skew();
 static void lcd_menu_xyz_offset();
-#if defined(TMC2130) || defined(PAT9125)
+#if defined(TMC2130) || defined(FILAMENT_SENSOR)
 static void lcd_menu_fails_stats();
-#endif //TMC2130 or PAT9125
+#endif //TMC2130 or FILAMENT_SENSOR
 
 static void lcd_selftest_v();
 static bool lcd_selfcheck_endstops();
@@ -702,7 +698,13 @@ static void lcd_implementation_status_screen()
     //Print Feedrate
     lcd_set_cursor(LCD_WIDTH - 8-2, 1);
     lcd_puts_P(PSTR("  "));
-    lcd_print(LCD_STR_FEEDRATE[0]);
+	if (maxlimit_status)
+	{
+		maxlimit_status = 0;
+		lcd_print('!');
+	}
+	else
+		lcd_print(LCD_STR_FEEDRATE[0]);
     lcd_print(itostr3(feedmultiply));
     lcd_puts_P(PSTR("%     "));
 #endif /* PLANNER_DIAGNOSTICS */
@@ -2049,12 +2051,20 @@ static void lcd_menu_extruder_info()
 //|Fil. Xd:    Yd:     |
 //|Int:      Shut:     |
 //----------------------
-    int fan_speed_RPM[2];
-    // Display Nozzle fan RPM
+	int fan_speed_RPM[2];
+	// Display Nozzle fan RPM
 	fan_speed_RPM[0] = 60*fan_speed[0];
-    fan_speed_RPM[1] = 60*fan_speed[1];
-    
-#ifdef PAT9125
+	fan_speed_RPM[1] = 60*fan_speed[1];
+	lcd_printf_P(_N(
+	  ESC_H(0,0)
+	  "Nozzle FAN: %4d RPM\n"
+	  "Print FAN:  %4d RPM\n"
+	 ),
+	 fan_speed_RPM[0],
+	 fan_speed_RPM[1]
+	);
+
+#ifdef FILAMENT_SENSOR
 	// Display X and Y difference from Filament sensor    
     // Display Light intensity from Filament sensor
     //  Frame_Avg register represents the average brightness of all pixels within a frame (324 pixels). This
@@ -2063,34 +2073,27 @@ static void lcd_menu_extruder_info()
     //  Shutter register is an index of LASER shutter time. It is automatically controlled by the chip's internal
     //  auto-exposure algorithm. When the chip is tracking on a good reflection surface, the Shutter is small.
     //  When the chip is tracking on a poor reflection surface, the Shutter is large. Value ranges from 0 to 46.
-    pat9125_update();
-	lcd_printf_P(_N(
-	  ESC_H(0,0)
-	  "Nozzle FAN: %4d RPM\n"
-	  "Print FAN:  %4d RPM\n"
-	  "Fil. Xd:%3d Yd:%3d\n"
-	  "Int: %3d  Shut: %3d\n"
-	 ),
-	 fan_speed_RPM[0],
-	 fan_speed_RPM[1],
-	 pat9125_x, pat9125_y,
-	 pat9125_b, pat9125_s
-	);
-#else //PAT9125
-	printf_P(_N(
-	  ESC_H(0,0)
-	  "Nozzle FAN: %4d RPM\n"
-	  "Print FAN:  %4d RPM\n"
-	 ),
-	 fan_speed_RPM[0],
-	 fan_speed_RPM[1]
-	);
-#endif //PAT9125
+
+	if (!fsensor_enabled)
+		lcd_puts_P(_N("Filament sensor\n" "is disabled."));
+	else
+	{
+		if (!moves_planned() && !IS_SD_PRINTING && !is_usb_printing && (lcd_commands_type != LCD_COMMAND_V2_CAL))
+			pat9125_update();
+		lcd_printf_P(_N(
+		  "Fil. Xd:%3d Yd:%3d\n"
+		  "Int: %3d  Shut: %3d"
+		 ),
+		 pat9125_x, pat9125_y,
+		 pat9125_b, pat9125_s
+		);
+	}
+#endif //FILAMENT_SENSOR
     
     menu_back_if_clicked();
 }
 
-#if defined(TMC2130) && defined(PAT9125)
+#if defined(TMC2130) && defined(FILAMENT_SENSOR)
 static void lcd_menu_fails_stats_total()
 {
 //01234567890123456789
@@ -2137,7 +2140,7 @@ static void lcd_menu_fails_stats()
 	MENU_ITEM_SUBMENU_P(PSTR("Total"), lcd_menu_fails_stats_total);
 	MENU_END();
 }
-#elif defined(PAT9125)
+#elif defined(FILAMENT_SENSOR)
 /**
  * @brief Print last print and total filament run outs
  *
@@ -2160,6 +2163,13 @@ static void lcd_menu_fails_stats()
     lcd_printf_P(PSTR(ESC_H(0,0) "Last print failures" ESC_H(1,1) "Filam. runouts  %-3d" ESC_H(0,2) "Total failures" ESC_H(1,3) "Filam. runouts  %-3d"), filamentLast, filamentTotal);
     menu_back_if_clicked();
 }
+#else
+static void lcd_menu_fails_stats()
+{
+	MENU_BEGIN();
+	MENU_ITEM_BACK_P(_T(MSG_MAIN));
+	MENU_END();
+}
 #endif //TMC2130
 
 
@@ -2215,6 +2225,7 @@ static void lcd_menu_belt_status()
 }
 #endif //TMC2130
 
+#ifdef RESUME_DEBUG 
 extern void stop_and_save_print_to_ram(float z_move, float e_move);
 extern void restore_print_from_ram_and_continue(float e_move);
 
@@ -2227,6 +2238,7 @@ static void lcd_menu_test_restore()
 {
 	restore_print_from_ram_and_continue(0.8);
 }
+#endif //RESUME_DEBUG 
 
 static void lcd_preheat_menu()
 {
@@ -2336,7 +2348,7 @@ void lcd_set_fan_check() {
 }
 
 void lcd_set_filament_autoload() {
-     fsensor_autoload_set(!filament_autoload_enabled);
+     fsensor_autoload_set(!fsensor_autoload_enabled);
 }
 
 void lcd_unLoadFilament()
@@ -2551,7 +2563,7 @@ void lcd_alright() {
 
 }
 
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
 static void lcd_menu_AutoLoadFilament()
 {
     if (degHotend0() > EXTRUDE_MINTEMP)
@@ -2571,7 +2583,7 @@ static void lcd_menu_AutoLoadFilament()
     }
     menu_back_if_clicked();
 }
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 
 static void lcd_LoadFilament()
 {
@@ -4069,7 +4081,7 @@ static void lcd_crash_mode_info2()
 }
 #endif //TMC2130
 
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
 static void lcd_filament_autoload_info()
 {
 uint8_t nlines;
@@ -4095,7 +4107,7 @@ uint8_t nlines;
 	}
     menu_back_if_clicked();
 }
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 
 
 static void lcd_silent_mode_set() {
@@ -4152,13 +4164,13 @@ static void lcd_crash_mode_set()
 #endif //TMC2130
  
 
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
 static void lcd_fsensor_state_set()
 {
 	FSensorStateMenu = !FSensorStateMenu; //set also from fsensor_enable() and fsensor_disable()
     if (!FSensorStateMenu) {
         fsensor_disable();
-        if (filament_autoload_enabled)
+        if (fsensor_autoload_enabled)
             menu_submenu(lcd_filament_autoload_info);
     }else{
         fsensor_enable();
@@ -4166,7 +4178,7 @@ static void lcd_fsensor_state_set()
             menu_submenu(lcd_fsensor_fail);
     }
 }
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 
 
 #if !SDSORT_USES_RAM
@@ -4626,130 +4638,120 @@ void lcd_settings_linearity_correction_menu(void)
 */
 static void lcd_settings_menu()
 {
-  EEPROM_read(EEPROM_SILENT, (uint8_t*)&SilentModeMenu, sizeof(SilentModeMenu));
-  MENU_BEGIN();
-  MENU_ITEM_BACK_P(_T(MSG_MAIN));
+	EEPROM_read(EEPROM_SILENT, (uint8_t*)&SilentModeMenu, sizeof(SilentModeMenu));
+	MENU_BEGIN();
+	MENU_ITEM_BACK_P(_T(MSG_MAIN));
 
+	MENU_ITEM_SUBMENU_P(_i("Temperature"), lcd_control_temperature_menu);////MSG_TEMPERATURE c=0 r=0
+	if (!homing_flag)
+	MENU_ITEM_SUBMENU_P(_i("Move axis"), lcd_move_menu_1mm);////MSG_MOVE_AXIS c=0 r=0
+	if (!isPrintPaused)
+	MENU_ITEM_GCODE_P(_i("Disable steppers"), PSTR("M84"));////MSG_DISABLE_STEPPERS c=0 r=0
 
-  MENU_ITEM_SUBMENU_P(_i("Temperature"), lcd_control_temperature_menu);////MSG_TEMPERATURE c=0 r=0
-  if (!homing_flag)
-  {
-	  MENU_ITEM_SUBMENU_P(_i("Move axis"), lcd_move_menu_1mm);////MSG_MOVE_AXIS c=0 r=0
-  }
-  if (!isPrintPaused)
-  {
-	  MENU_ITEM_GCODE_P(_i("Disable steppers"), PSTR("M84"));////MSG_DISABLE_STEPPERS c=0 r=0
-  }
 #ifndef TMC2130
-  if (!farm_mode) { //dont show in menu if we are in farm mode
-	  switch (SilentModeMenu) {
-	  case SILENT_MODE_POWER: MENU_ITEM_FUNCTION_P(_T(MSG_SILENT_MODE_OFF), lcd_silent_mode_set); break;
-	  case SILENT_MODE_SILENT: MENU_ITEM_FUNCTION_P(_T(MSG_SILENT_MODE_ON), lcd_silent_mode_set); break;
-	  case SILENT_MODE_AUTO: MENU_ITEM_FUNCTION_P(_T(MSG_AUTO_MODE_ON), lcd_silent_mode_set); break;
-	  default: MENU_ITEM_FUNCTION_P(_T(MSG_SILENT_MODE_OFF), lcd_silent_mode_set); break; // (probably) not needed
-	  }
-  }
+	if (!farm_mode)
+	{ //dont show in menu if we are in farm mode
+		switch (SilentModeMenu)
+		{
+		case SILENT_MODE_POWER: MENU_ITEM_FUNCTION_P(_T(MSG_SILENT_MODE_OFF), lcd_silent_mode_set); break;
+		case SILENT_MODE_SILENT: MENU_ITEM_FUNCTION_P(_T(MSG_SILENT_MODE_ON), lcd_silent_mode_set); break;
+		case SILENT_MODE_AUTO: MENU_ITEM_FUNCTION_P(_T(MSG_AUTO_MODE_ON), lcd_silent_mode_set); break;
+		default: MENU_ITEM_FUNCTION_P(_T(MSG_SILENT_MODE_OFF), lcd_silent_mode_set); break; // (probably) not needed
+		}
+	}
 #endif //TMC2130
 
-#ifdef PAT9125
-#ifndef DEBUG_DISABLE_FSENSORCHECK
-  if (FSensorStateMenu == 0) {
-      if (fsensor_not_responding){
-          // Filament sensor not working
-          MENU_ITEM_FUNCTION_P(_i("Fil. sensor [N/A]"), lcd_fsensor_state_set);////MSG_FSENSOR_NA c=0 r=0
-          MENU_ITEM_SUBMENU_P(_T(MSG_FSENS_AUTOLOAD_NA), lcd_fsensor_fail);
-      }
-      else{
-          // Filament sensor turned off, working, no problems
-          MENU_ITEM_FUNCTION_P(_T(MSG_FSENSOR_OFF), lcd_fsensor_state_set);
-          MENU_ITEM_SUBMENU_P(_T(MSG_FSENS_AUTOLOAD_NA), lcd_filament_autoload_info);
-      }
-  } else {
-      // Filament sensor turned on, working, no problems
-      MENU_ITEM_FUNCTION_P(_T(MSG_FSENSOR_ON), lcd_fsensor_state_set);
-     
+#ifdef FILAMENT_SENSOR
+	if (FSensorStateMenu == 0)
+	{
+		if (fsensor_not_responding)
+		{
+			// Filament sensor not working
+			MENU_ITEM_FUNCTION_P(_i("Fil. sensor [N/A]"), lcd_fsensor_state_set);////MSG_FSENSOR_NA c=0 r=0
+			MENU_ITEM_SUBMENU_P(_T(MSG_FSENS_AUTOLOAD_NA), lcd_fsensor_fail);
+		}
+		else
+		{
+			// Filament sensor turned off, working, no problems
+			MENU_ITEM_FUNCTION_P(_T(MSG_FSENSOR_OFF), lcd_fsensor_state_set);
+			MENU_ITEM_SUBMENU_P(_T(MSG_FSENS_AUTOLOAD_NA), lcd_filament_autoload_info);
+		}
+	}
+	else
+	{
+		// Filament sensor turned on, working, no problems
+		MENU_ITEM_FUNCTION_P(_T(MSG_FSENSOR_ON), lcd_fsensor_state_set);
+		if (fsensor_autoload_enabled)
+			MENU_ITEM_FUNCTION_P(_i("F. autoload  [on]"), lcd_set_filament_autoload);////MSG_FSENS_AUTOLOAD_ON c=17 r=1
+		else
+			MENU_ITEM_FUNCTION_P(_i("F. autoload [off]"), lcd_set_filament_autoload);////MSG_FSENS_AUTOLOAD_OFF c=17 r=1
+	}
+#endif //FILAMENT_SENSOR
 
-      if (filament_autoload_enabled) {
-          MENU_ITEM_FUNCTION_P(_i("F. autoload  [on]"), lcd_set_filament_autoload);////MSG_FSENS_AUTOLOAD_ON c=17 r=1
-      }
-      else {
-          MENU_ITEM_FUNCTION_P(_i("F. autoload [off]"), lcd_set_filament_autoload);////MSG_FSENS_AUTOLOAD_OFF c=17 r=1
-      }
-      
-  }
-#endif //DEBUG_DISABLE_FSENSORCHECK
-#endif //PAT9125
-
-  if (fans_check_enabled == true) {
-	  MENU_ITEM_FUNCTION_P(_i("Fans check   [on]"), lcd_set_fan_check);////MSG_FANS_CHECK_ON c=17 r=1
-  }
-  else {
-	  MENU_ITEM_FUNCTION_P(_i("Fans check  [off]"), lcd_set_fan_check);////MSG_FANS_CHECK_OFF c=17 r=1
-  }
+	if (fans_check_enabled == true)
+		MENU_ITEM_FUNCTION_P(_i("Fans check   [on]"), lcd_set_fan_check);////MSG_FANS_CHECK_ON c=17 r=1
+	else
+		MENU_ITEM_FUNCTION_P(_i("Fans check  [off]"), lcd_set_fan_check);////MSG_FANS_CHECK_OFF c=17 r=1
 
 #ifdef TMC2130
-  if(!farm_mode)
-  {
-	if (SilentModeMenu == SILENT_MODE_NORMAL) { MENU_ITEM_FUNCTION_P(_T(MSG_STEALTH_MODE_OFF), lcd_silent_mode_set); }
-    else MENU_ITEM_FUNCTION_P(_T(MSG_STEALTH_MODE_ON), lcd_silent_mode_set);
-    if (SilentModeMenu == SILENT_MODE_NORMAL)
-    {
-	  if (CrashDetectMenu == 0) { MENU_ITEM_FUNCTION_P(_T(MSG_CRASHDETECT_OFF), lcd_crash_mode_set); }
-      else MENU_ITEM_FUNCTION_P(_T(MSG_CRASHDETECT_ON), lcd_crash_mode_set);
-    }
-    else MENU_ITEM_SUBMENU_P(_T(MSG_CRASHDETECT_NA), lcd_crash_mode_info);
-  }
+	if(!farm_mode)
+	{
+		if (SilentModeMenu == SILENT_MODE_NORMAL) { MENU_ITEM_FUNCTION_P(_T(MSG_STEALTH_MODE_OFF), lcd_silent_mode_set); }
+		else MENU_ITEM_FUNCTION_P(_T(MSG_STEALTH_MODE_ON), lcd_silent_mode_set);
+		if (SilentModeMenu == SILENT_MODE_NORMAL)
+		{
+			if (CrashDetectMenu == 0) { MENU_ITEM_FUNCTION_P(_T(MSG_CRASHDETECT_OFF), lcd_crash_mode_set); }
+			else MENU_ITEM_FUNCTION_P(_T(MSG_CRASHDETECT_ON), lcd_crash_mode_set);
+		}
+		else MENU_ITEM_SUBMENU_P(_T(MSG_CRASHDETECT_NA), lcd_crash_mode_info);
+	}
 
 //  MENU_ITEM_SUBMENU_P(_i("Lin. correction"), lcd_settings_linearity_correction_menu);
 #endif //TMC2130
 
-  if (temp_cal_active == false) {
+  if (temp_cal_active == false)
 	  MENU_ITEM_FUNCTION_P(_i("Temp. cal.  [off]"), lcd_temp_calibration_set);////MSG_TEMP_CALIBRATION_OFF c=20 r=1
-  }
-  else {
+  else
 	  MENU_ITEM_FUNCTION_P(_i("Temp. cal.   [on]"), lcd_temp_calibration_set);////MSG_TEMP_CALIBRATION_ON c=20 r=1
-  }
+
 #ifdef HAS_SECOND_SERIAL_PORT
-  if (selectedSerialPort == 0) {
-	  MENU_ITEM_FUNCTION_P(_i("RPi port    [off]"), lcd_second_serial_set);////MSG_SECOND_SERIAL_OFF c=17 r=1
-  }
-  else {
-	  MENU_ITEM_FUNCTION_P(_i("RPi port     [on]"), lcd_second_serial_set);////MSG_SECOND_SERIAL_ON c=17 r=1
-  }
+	if (selectedSerialPort == 0)
+		MENU_ITEM_FUNCTION_P(_i("RPi port    [off]"), lcd_second_serial_set);////MSG_SECOND_SERIAL_OFF c=17 r=1
+	else
+		MENU_ITEM_FUNCTION_P(_i("RPi port     [on]"), lcd_second_serial_set);////MSG_SECOND_SERIAL_ON c=17 r=1
 #endif //HAS_SECOND_SERIAL
 
-  if (!isPrintPaused && !homing_flag)
-	{
+	if (!isPrintPaused && !homing_flag)
 		MENU_ITEM_SUBMENU_P(_T(MSG_BABYSTEP_Z), lcd_babystep_z);
-	}
 
 #if (LANG_MODE != 0)
 	MENU_ITEM_SUBMENU_P(_i("Select language"), lcd_language_menu);////MSG_LANGUAGE_SELECT c=0 r=0
 #endif //(LANG_MODE != 0)
 
-  if (card.ToshibaFlashAir_isEnabled()) {
-    MENU_ITEM_FUNCTION_P(_i("SD card [FlshAir]"), lcd_toshiba_flash_air_compatibility_toggle);////MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_ON c=19 r=1
-  } else {
-    MENU_ITEM_FUNCTION_P(_i("SD card  [normal]"), lcd_toshiba_flash_air_compatibility_toggle);////MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF c=19 r=1
-  }
+	if (card.ToshibaFlashAir_isEnabled())
+		MENU_ITEM_FUNCTION_P(_i("SD card [FlshAir]"), lcd_toshiba_flash_air_compatibility_toggle);////MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_ON c=19 r=1
+	else
+		MENU_ITEM_FUNCTION_P(_i("SD card  [normal]"), lcd_toshiba_flash_air_compatibility_toggle);////MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF c=19 r=1
 
-  #ifdef SDCARD_SORT_ALPHA
-	  if (!farm_mode) {
-	  uint8_t sdSort;
-	  EEPROM_read(EEPROM_SD_SORT, (uint8_t*)&sdSort, sizeof(sdSort));
-	  switch (sdSort) {
+#ifdef SDCARD_SORT_ALPHA
+	if (!farm_mode)
+	{
+		uint8_t sdSort;
+		EEPROM_read(EEPROM_SD_SORT, (uint8_t*)&sdSort, sizeof(sdSort));
+		switch (sdSort)
+		{
 		  case SD_SORT_TIME: MENU_ITEM_FUNCTION_P(_i("Sort:      [Time]"), lcd_sort_type_set); break;////MSG_SORT_TIME c=17 r=1
 		  case SD_SORT_ALPHA: MENU_ITEM_FUNCTION_P(_i("Sort:  [Alphabet]"), lcd_sort_type_set); break;////MSG_SORT_ALPHA c=17 r=1
 		  default: MENU_ITEM_FUNCTION_P(_i("Sort:      [None]"), lcd_sort_type_set);////MSG_SORT_NONE c=17 r=1
-	  }
-  }
-  #endif // SDCARD_SORT_ALPHA
+		}
+	}
+#endif // SDCARD_SORT_ALPHA
     
-    if (farm_mode)
-    {
-        MENU_ITEM_SUBMENU_P(PSTR("Farm number"), lcd_farm_no);
+	if (farm_mode)
+	{
+		MENU_ITEM_SUBMENU_P(PSTR("Farm number"), lcd_farm_no);
 		MENU_ITEM_FUNCTION_P(PSTR("Disable farm mode"), lcd_disable_farm_mode);
-    }
+	}
 
 	MENU_END();
 }
@@ -5284,7 +5286,7 @@ void extr_adj(int extruder) //loading filament for SNMM
 	lcd_print(snmm_extruder + 1);
 
 	// get response
-	bool response = mmu_get_reponse();
+	bool response = mmu_get_reponse(false);
 	if (!response) mmu_not_responding();
 	lcd_update_enable(true);
 	
@@ -5357,7 +5359,7 @@ void extr_unload() { //unloads filament
 		fprintf_P(uart2io, PSTR("U0\n"));
 
 		// get response
-		bool response = mmu_get_reponse();
+		bool response = mmu_get_reponse(false);
 		if (!response) mmu_not_responding();
 
 		lcd_update_enable(true);
@@ -6007,11 +6009,11 @@ static void lcd_main_menu()
     #endif
 
 	#else
-	  #ifdef PAT9125
-	if ( ((filament_autoload_enabled == true) && (fsensor_enabled == true)))
+	  #ifdef FILAMENT_SENSOR
+	if ( ((fsensor_autoload_enabled == true) && (fsensor_enabled == true)))
         MENU_ITEM_SUBMENU_P(_i("AutoLoad filament"), lcd_menu_AutoLoadFilament);////MSG_AUTOLOAD_FILAMENT c=17 r=0
 	else
-      #endif //PAT9125
+      #endif //FILAMENT_SENSOR
 		MENU_ITEM_FUNCTION_P(_T(MSG_LOAD_FILAMENT), lcd_LoadFilament);
 	MENU_ITEM_SUBMENU_P(_T(MSG_UNLOAD_FILAMENT), lcd_unLoadFilament);
     #endif
@@ -6026,7 +6028,7 @@ static void lcd_main_menu()
 	  MENU_ITEM_SUBMENU_P(_i("Statistics  "), lcd_menu_statistics);////MSG_STATISTICS c=0 r=0
   }
     
-#if defined(TMC2130) || defined(PAT9125)
+#if defined(TMC2130) || defined(FILAMENT_SENSOR)
   MENU_ITEM_SUBMENU_P(PSTR("Fail stats"), lcd_menu_fails_stats);
 #endif
 
@@ -6137,16 +6139,14 @@ static void lcd_tune_menu()
 	MENU_ITEM_FUNCTION_P(_T(MSG_FILAMENTCHANGE), lcd_colorprint_change);//7
 #endif
 
-#ifndef DEBUG_DISABLE_FSENSORCHECK
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
 	if (FSensorStateMenu == 0) {
 		MENU_ITEM_FUNCTION_P(_T(MSG_FSENSOR_OFF), lcd_fsensor_state_set);
 	}
 	else {
 		MENU_ITEM_FUNCTION_P(_T(MSG_FSENSOR_ON), lcd_fsensor_state_set);
 	}
-#endif //PAT9125
-#endif //DEBUG_DISABLE_FSENSORCHECK
+#endif //FILAMENT_SENSOR
 
 #ifdef TMC2130
      if(!farm_mode)
@@ -6499,16 +6499,16 @@ bool lcd_selftest()
 	if (_result)
 	{
 		_progress = lcd_selftest_screen(8, _progress, 3, true, 2000); //bed ok
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
 		_progress = lcd_selftest_screen(9, _progress, 3, true, 2000); //check filaments sensor
 		_result = lcd_selftest_fsensor();
-#endif // PAT9125
+#endif // FILAMENT_SENSOR
 	}
 	if (_result)
 	{
-#ifdef PAT9125
+#ifdef FILAMENT_SENSOR
 		_progress = lcd_selftest_screen(10, _progress, 3, true, 2000); //fil sensor OK
-#endif // PAT9125
+#endif // FILAMENT_SENSOR
 		_progress = lcd_selftest_screen(11, _progress, 3, true, 5000); //all correct
 	}
 	else
@@ -7057,17 +7057,18 @@ static void lcd_selftest_error(int _error_no, const char *_error_1, const char *
 
 }
 
-#ifdef PAT9125
-static bool lcd_selftest_fsensor() {
+#ifdef FILAMENT_SENSOR
+static bool lcd_selftest_fsensor(void)
+{
 	fsensor_init();
 	if (fsensor_not_responding)
 	{
 		const char *_err;
 		lcd_selftest_error(11, _err, _err);
 	}
-	return(!fsensor_not_responding);
+	return (!fsensor_not_responding);
 }
-#endif //PAT9125
+#endif //FILAMENT_SENSOR
 
 static bool lcd_selftest_manual_fan_check(int _fan, bool check_opposite)
 {
diff --git a/Firmware/variants/1_75mm_MK25-RAMBo10a-E3Dv6full.h b/Firmware/variants/1_75mm_MK25-RAMBo10a-E3Dv6full.h
index f08e80620..03232779e 100644
--- a/Firmware/variants/1_75mm_MK25-RAMBo10a-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK25-RAMBo10a-E3Dv6full.h
@@ -110,7 +110,7 @@
 
 // Filament sensor
 #define PAT9125
-
+#define FILAMENT_SENSOR
 
 
 //#define DEBUG_BUILD
@@ -137,7 +137,6 @@
 //#define DEBUG_XSTEP_DUP_PIN 21   //duplicate x-step output to pin 21 (SCL on P3)
 //#define DEBUG_YSTEP_DUP_PIN 21   //duplicate y-step output to pin 21 (SCL on P3)
 //#define DEBUG_DISABLE_FANCHECK     //disable fan check (no ISR INT7, check disabled)
-//#define DEBUG_DISABLE_FSENSORCHECK //disable fsensor check (no ISR INT7, check disabled)
 //#define DEBUG_DUMP_TO_2ND_SERIAL   //dump received characters to 2nd serial line
 //#define DEBUG_STEPPER_TIMER_MISSED // Stop on stepper timer overflow, beep and display a message.
 //#define PLANNER_DIAGNOSTICS // Show the planner queue status on printer display.
@@ -265,13 +264,6 @@
 #define Z_HIGH_POWER 200
 #endif
 
-/*------------------------------------
- PAT9125 SETTINGS
- *------------------------------------*/
-
-#define PAT9125_XRES			0
-#define PAT9125_YRES			255
-
 /*------------------------------------
  BED SETTINGS
  *------------------------------------*/
diff --git a/Firmware/variants/1_75mm_MK25-RAMBo13a-E3Dv6full.h b/Firmware/variants/1_75mm_MK25-RAMBo13a-E3Dv6full.h
index 1a32cefee..01ab8667a 100644
--- a/Firmware/variants/1_75mm_MK25-RAMBo13a-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK25-RAMBo13a-E3Dv6full.h
@@ -110,7 +110,7 @@
 
 // Filament sensor
 #define PAT9125
-
+#define FILAMENT_SENSOR
 
 
 //#define DEBUG_BUILD
@@ -137,7 +137,6 @@
 //#define DEBUG_XSTEP_DUP_PIN 21   //duplicate x-step output to pin 21 (SCL on P3)
 //#define DEBUG_YSTEP_DUP_PIN 21   //duplicate y-step output to pin 21 (SCL on P3)
 //#define DEBUG_DISABLE_FANCHECK     //disable fan check (no ISR INT7, check disabled)
-//#define DEBUG_DISABLE_FSENSORCHECK //disable fsensor check (no ISR INT7, check disabled)
 //#define DEBUG_DUMP_TO_2ND_SERIAL   //dump received characters to 2nd serial line
 //#define DEBUG_STEPPER_TIMER_MISSED // Stop on stepper timer overflow, beep and display a message.
 //#define PLANNER_DIAGNOSTICS // Show the planner queue status on printer display.
@@ -265,13 +264,6 @@
 #define Z_HIGH_POWER 200
 #endif
 
-/*------------------------------------
- PAT9125 SETTINGS
- *------------------------------------*/
-
-#define PAT9125_XRES			0
-#define PAT9125_YRES			255
-
 /*------------------------------------
  BED SETTINGS
  *------------------------------------*/
diff --git a/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h b/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h
index 4fdbd1b0e..56b8b3334 100644
--- a/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h
@@ -133,6 +133,7 @@
 
 // Filament sensor
 #define PAT9125
+#define FILAMENT_SENSOR
 
 // Backlash - 
 //#define BACKLASH_X
@@ -378,13 +379,6 @@
 #define DEFAULT_PWM_MOTOR_CURRENT_LOUD  {400, 750, 750} // {XY,Z,E}
 #endif
 
-/*------------------------------------
- PAT9125 SETTINGS
- *------------------------------------*/
-
-#define PAT9125_XRES			0
-#define PAT9125_YRES			255
-
 /*------------------------------------
  BED SETTINGS
  *------------------------------------*/