diff --git a/Firmware/fsensor.cpp b/Firmware/fsensor.cpp
index 0d6ec5fd..18fefb7f 100644
--- a/Firmware/fsensor.cpp
+++ b/Firmware/fsensor.cpp
@@ -35,6 +35,7 @@ bool fsensor_enabled = true;
 bool fsensor_M600 = false;
 uint8_t fsensor_err_cnt = 0;
 int16_t fsensor_st_cnt = 0;
+uint8_t fsensor_log = 0;
 
 
 void fsensor_enable()
@@ -86,35 +87,46 @@ ISR(PCINT2_vect)
 	if (st_cnt != 0)
 	{
 #ifdef DEBUG_FSENSOR_LOG
-		MYSERIAL.print("cnt=");
-		MYSERIAL.print(st_cnt, DEC);
-		MYSERIAL.print(" dy=");
-		MYSERIAL.print(pat9125_y, DEC);
+		if (fsensor_log)
+		{
+			MYSERIAL.print("cnt=");
+			MYSERIAL.print(st_cnt, DEC);
+			MYSERIAL.print(" dy=");
+			MYSERIAL.print(pat9125_y, DEC);
+		}
 #endif //DEBUG_FSENSOR_LOG
 		if (st_cnt != 0)
 		{
 			if( (pat9125_y == 0) || ((pat9125_y > 0) && (st_cnt < 0)) || ((pat9125_y < 0) && (st_cnt > 0)))
 			{ //invalid movement
-				fsensor_err_cnt++;
+				if (st_cnt > 0) //only positive movements
+					fsensor_err_cnt++;
 #ifdef DEBUG_FSENSOR_LOG
+			if (fsensor_log)
+			{
 				MYSERIAL.print("\tNG ! err=");
 				MYSERIAL.println(fsensor_err_cnt, DEC);
+			}
 #endif //DEBUG_FSENSOR_LOG
 			}
 			else
 			{ //propper movement
-//				if (fsensor_err_cnt > 0)
-//					fsensor_err_cnt--;
-					fsensor_err_cnt = 0;
+				if (fsensor_err_cnt > 0)
+					fsensor_err_cnt--;
+//					fsensor_err_cnt = 0;
 #ifdef DEBUG_FSENSOR_LOG
-				MYSERIAL.print("\tOK    err=");
-				MYSERIAL.println(fsensor_err_cnt, DEC);
+				if (fsensor_log)
+				{
+					MYSERIAL.print("\tOK    err=");
+					MYSERIAL.println(fsensor_err_cnt, DEC);
+				}
 #endif //DEBUG_FSENSOR_LOG
 			}
 		}
 		else
 		{ //no movement
 #ifdef DEBUG_FSENSOR_LOG
+		if (fsensor_log)
 			MYSERIAL.println("\tOK 0");
 #endif //DEBUG_FSENSOR_LOG
 		}
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index cd1d3685..e884c44c 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -3043,7 +3043,7 @@ static void prusa_stat_printinfo()
 	SERIAL_ECHO("]");
 }
 
-
+/*
 void lcd_pick_babystep(){
     int enc_dif = 0;
     int cursor_pos = 1;
@@ -3145,7 +3145,7 @@ void lcd_pick_babystep(){
     lcd_implementation_clear();
     lcd_return_to_status();
 }
-
+*/
 void lcd_move_menu_axis()
 {
 	START_MENU();
@@ -3664,6 +3664,11 @@ static void lcd_settings_menu()
 	END_MENU();
 }
 
+static void lcd_selftest_()
+{
+	lcd_selftest();
+}
+
 static void lcd_calibration_menu()
 {
   START_MENU();
@@ -3671,7 +3676,7 @@ static void lcd_calibration_menu()
   if (!isPrintPaused)
   {
 	MENU_ITEM(gcode, MSG_AUTO_HOME, PSTR("G28 W"));
-    MENU_ITEM(function, MSG_SELFTEST, lcd_selftest);
+    MENU_ITEM(function, MSG_SELFTEST, lcd_selftest_);
 #ifdef MK1BP
     // MK1
     // "Calibrate Z"