diff --git a/Firmware/Configuration_prusa.h b/Firmware/Configuration_prusa.h
index a74750b6..4c45b6c5 100644
--- a/Firmware/Configuration_prusa.h
+++ b/Firmware/Configuration_prusa.h
@@ -133,6 +133,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define MINTEMP_MINAMBIENT      25
 #define MINTEMP_MINAMBIENT_RAW  978
 
+#define DEBUG_DISABLE_FORCE_SELFTEST //disable force selftest
 //#define DEBUG_BUILD
 #ifdef DEBUG_BUILD
 //#define _NO_ASM
@@ -154,6 +155,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 //#define DEBUG_DISABLE_LCD_STATUS_LINE  //empty four lcd line
 //#define DEBUG_DISABLE_PREVENT_EXTRUDER //cold extrusion and long extrusion allowed
 //#define DEBUG_DISABLE_PRUSA_STATISTICS //disable prusa_statistics() mesages
+#define DEBUG_DISABLE_FORCE_SELFTEST //disable force selftest
 //#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_BLINK_ACTIVE
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index b9ef2d35..cc1237ea 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1332,11 +1332,14 @@ void setup()
 	  }
   }
 
+#ifndef DEBUG_DISABLE_FORCE_SELFTEST
   if (force_selftest_if_fw_version() && calibration_status() < CALIBRATION_STATUS_ASSEMBLED ) {
 	  lcd_show_fullscreen_message_and_wait_P(MSG_FORCE_SELFTEST);
 	  update_current_firmware_version_to_eeprom();
 	  lcd_selftest();
   }
+#endif //DEBUG_DISABLE_FORCE_SELFTEST
+
   KEEPALIVE_STATE(IN_PROCESS);
 #endif //DEBUG_DISABLE_STARTMSGS
   lcd_update_enable(true);
diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp
index 5d0aaacc..c6a1750a 100644
--- a/Firmware/xyzcal.cpp
+++ b/Firmware/xyzcal.cpp
@@ -19,8 +19,8 @@
 
 #define _PINDA ((READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)?1:0)
 
-//#define DBG(args...) printf_P(args)
-#define DBG(args...)
+#define DBG(args...) printf_P(args)
+//#define DBG(args...)
 #define _n PSTR
 
 #define _X ((int16_t)count_position[X_AXIS])
@@ -87,11 +87,11 @@ uint8_t xyzcal_dm = 0;
 
 void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
 {
-	DBG(_n("xyzcal_update_pos dx=%d dy=%d dz=%d dir=%02x\n"), dx, dy, dz, xyzcal_dm);
+//	DBG(_n("xyzcal_update_pos dx=%d dy=%d dz=%d dir=%02x\n"), dx, dy, dz, xyzcal_dm);
 	if (xyzcal_dm&1) count_position[0] -= dx; else count_position[0] += dx;
 	if (xyzcal_dm&2) count_position[1] -= dy; else count_position[1] += dy;
 	if (xyzcal_dm&4) count_position[2] -= dz; else count_position[2] += dz;
-	DBG(_n(" after xyzcal_update_pos x=%ld y=%ld z=%ld\n"), count_position[0], count_position[1], count_position[2]);
+//	DBG(_n(" after xyzcal_update_pos x=%ld y=%ld z=%ld\n"), count_position[0], count_position[1], count_position[2]);
 }
 
 uint16_t xyzcal_sm4_delay = 0;
@@ -142,7 +142,7 @@ uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd)
 
 bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda)
 {
-	DBG(_n("xyzcal_lineXYZ_to x=%d y=%d z=%d  check=%d\n"), x, y, z, check_pinda);
+//	DBG(_n("xyzcal_lineXYZ_to x=%d y=%d z=%d  check=%d\n"), x, y, z, check_pinda);
 	x -= (int16_t)count_position[0];
 	y -= (int16_t)count_position[1];
 	z -= (int16_t)count_position[2];
@@ -384,7 +384,7 @@ void xyzcal_adjust_pixels(uint8_t* pixels, uint16_t* histo)
 	for (l = 15; l > 8; l--)
 		if (histo[l] >= 10)
 			break;
-	uint8_t pix_min = (max_l + 3) << 4;
+	uint8_t pix_min = (max_l + 2) << 4;
 	uint8_t pix_max = l << 4;
 	uint8_t pix_dif = pix_max - pix_min;
 	DBG(_n(" min=%d max=%d dif=%d\n"), pix_min, pix_max, pix_dif);
@@ -425,7 +425,7 @@ void xyzcal_draw_pattern_12x12_in_32x32(uint8_t* pattern, uint32_t* pixels, int
 
 int16_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t c, uint8_t r)
 {
-	uint8_t thr = 64;
+	uint8_t thr = 32;
 	int16_t match = 0;
 	for (uint8_t i = 0; i < 12; i++)
 		for (uint8_t j = 0; j < 12; j++)
@@ -620,7 +620,7 @@ bool xyzcal_scan_and_process(void)
 	int16_t z = _Z;
 
 	uint8_t* pixels = (uint8_t*)block_buffer;
-	xyzcal_scan_pixels_32x32(x, y, z - 128, 2400, 200, pixels);
+	xyzcal_scan_pixels_32x32(x, y, z - 72, 2400, 200, pixels);
 
 	uint16_t* histo = (uint16_t*)(pixels + 32*32);
 	xyzcal_histo_pixels_32x32(pixels, histo);