Merge branch 'M221_fix' into MK3_no_limitsv3.1.2-RC2

14 files changed, 218 insertions, 226 deletions

diff --git a/Firmware/Configuration.h b/Firmware/Configuration.h
index a0d8091a7..4eb4270c4 100644
--- a/Firmware/Configuration.h
+++ b/Firmware/Configuration.h
@@ -7,8 +7,8 @@
 #define STR(x) STR_HELPER(x)
 
 // Firmware version
-#define FW_VERSION "3.1.1"
-#define FW_COMMIT_NR   201
+#define FW_VERSION "3.1.2"
+#define FW_COMMIT_NR   231
 // FW_VERSION_UNKNOWN means this is an unofficial build.
 // The firmware should only be checked into github with this symbol.
 #define FW_DEV_VERSION FW_VERSION_UNKNOWN
diff --git a/Firmware/Configuration_prusa.h b/Firmware/Configuration_prusa.h
index 023f1c52c..87a85dd0b 100644
--- a/Firmware/Configuration_prusa.h
+++ b/Firmware/Configuration_prusa.h
@@ -138,6 +138,11 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define PLANNER_DIAGNOSTICS // Show the planner queue status on printer display.
 #endif /* DEBUG_BUILD */
 
+//#define EXPERIMENTAL_FEATURES
+//#define TMC2130_LINEARITY_CORRECTION
+//#define TMC2130_VARIABLE_RESOLUTION
+
+
 
 /*------------------------------------
  TMC2130 default settings
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index bc4d6e772..60afe10a5 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1033,6 +1033,7 @@ void setup()
 	    MYSERIAL.println("CrashDetect DISABLED");
 	}
 
+#ifdef TMC2130_LINEARITY_CORRECTION
 	tmc2130_wave_fac[X_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_X_FAC);
 	tmc2130_wave_fac[Y_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_Y_FAC);
 	tmc2130_wave_fac[Z_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_Z_FAC);
@@ -1041,7 +1042,9 @@ void setup()
 	if (tmc2130_wave_fac[Y_AXIS] == 0xff) tmc2130_wave_fac[Y_AXIS] = 0;
 	if (tmc2130_wave_fac[Z_AXIS] == 0xff) tmc2130_wave_fac[Z_AXIS] = 0;
 	if (tmc2130_wave_fac[E_AXIS] == 0xff) tmc2130_wave_fac[E_AXIS] = 0;
+#endif //TMC2130_LINEARITY_CORRECTION
 
+#ifdef TMC2130_VARIABLE_RESOLUTION
 	tmc2130_mres[X_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_X_MRES);
 	tmc2130_mres[Y_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_Y_MRES);
 	tmc2130_mres[Z_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_Z_MRES);
@@ -1054,6 +1057,12 @@ void setup()
 	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_Y_MRES, tmc2130_mres[Y_AXIS]);
 	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_Z_MRES, tmc2130_mres[Z_AXIS]);
 	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_E_MRES, tmc2130_mres[E_AXIS]);
+#else //TMC2130_VARIABLE_RESOLUTION
+	tmc2130_mres[X_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
+	tmc2130_mres[Y_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
+	tmc2130_mres[Z_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_Z);
+	tmc2130_mres[E_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_E);
+#endif //TMC2130_VARIABLE_RESOLUTION
 
 #endif //TMC2130
 
@@ -1128,6 +1137,51 @@ void setup()
 	// Force SD card update. Otherwise the SD card update is done from loop() on card.checkautostart(false), 
 	// but this times out if a blocking dialog is shown in setup().
 	card.initsd();
+#ifdef DEBUG_SD_SPEED_TEST
+	if (card.cardOK)
+	{
+		uint8_t* buff = (uint8_t*)block_buffer;
+		uint32_t block = 0;
+		uint32_t sumr = 0;
+		uint32_t sumw = 0;
+		for (int i = 0; i < 1024; i++)
+		{
+			uint32_t u = micros();
+			bool res = card.card.readBlock(i, buff);
+			u = micros() - u;
+			if (res)
+			{
+				printf_P(PSTR("readBlock %4d 512 bytes %lu us\n"), i, u);
+				sumr += u;
+				u = micros();
+				res = card.card.writeBlock(i, buff);
+				u = micros() - u;
+				if (res)
+				{
+					printf_P(PSTR("writeBlock %4d 512 bytes %lu us\n"), i, u);
+					sumw += u;
+				}
+				else
+				{
+					printf_P(PSTR("writeBlock %4d error\n"), i);
+					break;
+				}
+			}
+			else
+			{
+				printf_P(PSTR("readBlock %4d error\n"), i);
+				break;
+			}
+		}
+		uint32_t avg_rspeed = (1024 * 1000000) / (sumr / 512);
+		uint32_t avg_wspeed = (1024 * 1000000) / (sumw / 512);
+		printf_P(PSTR("avg read speed %lu bytes/s\n"), avg_rspeed);
+		printf_P(PSTR("avg write speed %lu bytes/s\n"), avg_wspeed);
+	}
+	else
+		printf_P(PSTR("Card NG!\n"));
+#endif DEBUG_SD_SPEED_TEST
+
 	if (eeprom_read_byte((uint8_t*)EEPROM_POWER_COUNT) == 0xff) eeprom_write_byte((uint8_t*)EEPROM_POWER_COUNT, 0);
 	if (eeprom_read_byte((uint8_t*)EEPROM_CRASH_COUNT_X) == 0xff) eeprom_write_byte((uint8_t*)EEPROM_CRASH_COUNT_X, 0);
 	if (eeprom_read_byte((uint8_t*)EEPROM_CRASH_COUNT_Y) == 0xff) eeprom_write_byte((uint8_t*)EEPROM_CRASH_COUNT_Y, 0);
@@ -1219,6 +1273,12 @@ void setup()
 		  lcd_show_fullscreen_message_and_wait_P(MSG_FOLLOW_CALIBRATION_FLOW);
 	  }
   }
+
+  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();
+  }
   KEEPALIVE_STATE(IN_PROCESS);
 #endif //DEBUG_DISABLE_STARTMSGS
   lcd_update_enable(true);
@@ -1833,6 +1893,7 @@ void homeaxis(int axis, uint8_t cnt, uint8_t* pstep)
         // for the stall guard to work.
         current_position[axis] = 0;
         plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+		set_destination_to_current();
 //        destination[axis] = 11.f;
         destination[axis] = 3.f;
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
@@ -6457,6 +6518,11 @@ void get_coordinates()
 		if (next_feedrate > MAX_SILENT_FEEDRATE) next_feedrate = MAX_SILENT_FEEDRATE;
 #endif //MAX_SILENT_FEEDRATE
     if(next_feedrate > 0.0) feedrate = next_feedrate;
+	if (!seen[0] && !seen[1] && !seen[2] && seen[3])
+	{
+//		float e_max_speed = 
+//		printf_P(PSTR("E MOVE speed %7.3f\n"), feedrate / 60)
+	}
   }
 }
 
@@ -6960,11 +7026,14 @@ void save_statistics(unsigned long _total_filament_used, unsigned long _total_pr
 }
 
 float calculate_extruder_multiplier(float diameter) {
-  bool  enabled = volumetric_enabled && diameter > 0;
-  float area    = enabled ? (M_PI * pow(diameter * .5, 2)) : 0;
-	return (extrudemultiply == 100) ? 
-    (enabled ? (1.f / area) : 1.f) :
-    (enabled ? ((float(extrudemultiply) * 0.01f) / area) : 1.f);
+  float out = 1.f;
+  if (volumetric_enabled && diameter > 0.f) {
+    float area = M_PI * diameter * diameter * 0.25;
+    out = 1.f / area;
+  }
+  if (extrudemultiply != 100)
+    out *= float(extrudemultiply) * 0.01f;
+  return out;
 }
 
 void calculate_extruder_multipliers() {
diff --git a/Firmware/cardreader.h b/Firmware/cardreader.h
index f9d30a769..4d1367a99 100644
--- a/Firmware/cardreader.h
+++ b/Firmware/cardreader.h
@@ -131,7 +131,12 @@ private:
 
 #endif // SDCARD_SORT_ALPHA
 
+#ifdef DEBUG_SD_SPEED_TEST
+public:
+#endif DEBUG_SD_SPEED_TEST
   Sd2Card card;
+
+private:
   SdVolume volume;
   SdFile file;
   #define SD_PROCEDURE_DEPTH 1
diff --git a/Firmware/language_all.cpp b/Firmware/language_all.cpp
index 64f459ff9..b72de3611 100644
--- a/Firmware/language_all.cpp
+++ b/Firmware/language_all.cpp
@@ -747,6 +747,13 @@ const char * const MSG_FOLLOW_CALIBRATION_FLOW_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_FOLLOW_CALIBRATION_FLOW_CZ
 };
 
+const char MSG_FORCE_SELFTEST_EN[] PROGMEM = "Selftest will be run to calibrate accurate sensorless rehoming.";
+const char MSG_FORCE_SELFTEST_CZ[] PROGMEM = "Pro kalibraci presneho rehomovani bude nyni spusten selftest.";
+const char * const MSG_FORCE_SELFTEST_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_FORCE_SELFTEST_EN,
+	MSG_FORCE_SELFTEST_CZ
+};
+
 const char MSG_FREE_MEMORY_EN[] PROGMEM = " Free Memory: ";
 const char * const MSG_FREE_MEMORY_LANG_TABLE[1] PROGMEM = {
 	MSG_FREE_MEMORY_EN
diff --git a/Firmware/language_all.h b/Firmware/language_all.h
index f191ddf99..12412e4c8 100644
--- a/Firmware/language_all.h
+++ b/Firmware/language_all.h
@@ -270,6 +270,8 @@ extern const char* const MSG_FLOW2_LANG_TABLE[1];
 #define MSG_FLOW2 LANG_TABLE_SELECT_EXPLICIT(MSG_FLOW2_LANG_TABLE, 0)
 extern const char* const MSG_FOLLOW_CALIBRATION_FLOW_LANG_TABLE[LANG_NUM];
 #define MSG_FOLLOW_CALIBRATION_FLOW LANG_TABLE_SELECT(MSG_FOLLOW_CALIBRATION_FLOW_LANG_TABLE)
+extern const char* const MSG_FORCE_SELFTEST_LANG_TABLE[LANG_NUM];
+#define MSG_FORCE_SELFTEST LANG_TABLE_SELECT(MSG_FORCE_SELFTEST_LANG_TABLE)
 extern const char* const MSG_FREE_MEMORY_LANG_TABLE[1];
 #define MSG_FREE_MEMORY LANG_TABLE_SELECT_EXPLICIT(MSG_FREE_MEMORY_LANG_TABLE, 0)
 extern const char* const MSG_FSENSOR_NA_LANG_TABLE[1];
diff --git a/Firmware/language_en.h b/Firmware/language_en.h
index fd8c93104..a3ab5fb31 100644
--- a/Firmware/language_en.h
+++ b/Firmware/language_en.h
@@ -412,3 +412,4 @@
 #define(length=20, lines=8) MSG_FW_VERSION_ALPHA		 "You are using firmware alpha version. This is development version. Using this version is not recommended and may cause printer damage." 
 #define(length=20, lines=8) MSG_FW_VERSION_BETA			 "You are using firmware beta version. This is development version. Using this version is not recommended and may cause printer damage."
 #define(length=20, lines=8) MSG_FW_VERSION_RC			 "This firmware version is release candidate. Some of the features may not work properly."
+#define(length=20, lines=8) MSG_FORCE_SELFTEST			 "Selftest will be run to calibrate accurate sensorless rehoming."
diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp
index 291757d17..4a56868f0 100644
--- a/Firmware/temperature.cpp
+++ b/Firmware/temperature.cpp
@@ -468,14 +468,20 @@ void checkFanSpeed()
 	fans_check_enabled = (eeprom_read_byte((uint8_t*)EEPROM_FAN_CHECK_ENABLED) > 0);
 	static unsigned char fan_speed_errors[2] = { 0,0 };
 
-	if (fan_speed[0] == 0 && (current_temperature[0] > EXTRUDER_AUTO_FAN_TEMPERATURE)) fan_speed_errors[0]++;
+	if ((fan_speed[0] == 0) && (current_temperature[0] > EXTRUDER_AUTO_FAN_TEMPERATURE)) fan_speed_errors[0]++;
 	else fan_speed_errors[0] = 0;
 
-	if ((fan_speed[1] == 0)&& (fanSpeed > MIN_PRINT_FAN_SPEED)) fan_speed_errors[1]++;
+	if ((fan_speed[1] == 0) && ((blocks_queued()?block_buffer[block_buffer_tail].fan_speed:fanSpeed) > MIN_PRINT_FAN_SPEED)) fan_speed_errors[1]++;
 	else fan_speed_errors[1] = 0;
 
-	if ((fan_speed_errors[0] > 5) && fans_check_enabled) fanSpeedError(0); //extruder fan
-	if ((fan_speed_errors[1] > 15) && fans_check_enabled) fanSpeedError(1); //print fan
+	if ((fan_speed_errors[0] > 5) && fans_check_enabled) {
+		fan_speed_errors[0] = 0;
+		fanSpeedError(0); //extruder fan
+	}
+	if ((fan_speed_errors[1] > 15) && fans_check_enabled) {
+		fan_speed_errors[1] = 0;
+		fanSpeedError(1); //print fan
+	}
 }
 
 extern void stop_and_save_print_to_ram(float z_move, float e_move);
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index 91b21d84b..58f84cf43 100644
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -50,7 +50,7 @@ uint8_t tmc2130_sg_thr_home[4] = {3, 3, TMC2130_SG_THRS_Z, TMC2130_SG_THRS_E};
 uint8_t sg_homing_axes_mask = 0x00;
 
 uint8_t tmc2130_sg_meassure = 0xff;
-uint16_t tmc2130_sg_meassure_cnt = 0;
+uint32_t tmc2130_sg_meassure_cnt = 0;
 uint32_t tmc2130_sg_meassure_val = 0;
 
 uint8_t tmc2130_home_enabled = 0;
@@ -117,10 +117,11 @@ void tmc2130_wr_PWMCONF(uint8_t axis, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_
 void tmc2130_wr_TPWMTHRS(uint8_t axis, uint32_t val32);
 void tmc2130_wr_THIGH(uint8_t axis, uint32_t val32);
 
+#define tmc2130_rd(axis, addr, rval) tmc2130_rx(axis, addr, rval)
+#define tmc2130_wr(axis, addr, wval) tmc2130_tx(axis, addr | 0x80, wval)
 
-uint8_t tmc2130_wr(uint8_t axis, uint8_t addr, uint32_t wval);
-uint8_t tmc2130_rd(uint8_t axis, uint8_t addr, uint32_t* rval);
-uint8_t tmc2130_txrx(uint8_t axis, uint8_t addr, uint32_t wval, uint32_t* rval);
+uint8_t tmc2130_tx(uint8_t axis, uint8_t addr, uint32_t wval);
+uint8_t tmc2130_rx(uint8_t axis, uint8_t addr, uint32_t* rval);
 
 
 void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_t current_r);
@@ -130,10 +131,6 @@ void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_
 void tmc2130_init()
 {
 	DBG(_n("tmc2130_init(), mode=%S\n"), tmc2130_mode?_n("STEALTH"):_n("NORMAL"));
-/*	tmc2130_mres[X_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
-	tmc2130_mres[Y_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
-	tmc2130_mres[Z_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_Z);
-	tmc2130_mres[E_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_E);*/
 	WRITE(X_TMC2130_CS, HIGH);
 	WRITE(Y_TMC2130_CS, HIGH);
 	WRITE(Z_TMC2130_CS, HIGH);
@@ -188,10 +185,13 @@ void tmc2130_init()
 	tmc2130_sg_cnt[2] = 0;
 	tmc2130_sg_cnt[3] = 0;
 
+#ifdef TMC2130_LINEARITY_CORRECTION
 	tmc2130_set_wave(X_AXIS, 247, tmc2130_wave_fac[X_AXIS]);
 	tmc2130_set_wave(Y_AXIS, 247, tmc2130_wave_fac[Y_AXIS]);
 	tmc2130_set_wave(Z_AXIS, 247, tmc2130_wave_fac[Z_AXIS]);
 	tmc2130_set_wave(E_AXIS, 247, tmc2130_wave_fac[E_AXIS]);
+#endif //TMC2130_LINEARITY_CORRECTION
+
 }
 
 uint8_t tmc2130_sample_diag()
@@ -233,14 +233,7 @@ void tmc2130_st_isr(uint8_t last_step_mask)
 	}
 	if (sg_homing_axes_mask == 0)
 	{
-/*		if (crash)
-		{
-			if (diag_mask & 0x01) tmc2130_sg_cnt[0]++;
-			if (diag_mask & 0x02) tmc2130_sg_cnt[1]++;
-			if (diag_mask & 0x04) tmc2130_sg_cnt[2]++;
-			if (diag_mask & 0x08) tmc2130_sg_cnt[3]++;
-		}*/
-		if (/*!is_usb_printing && */tmc2130_sg_stop_on_crash && crash)
+		if (tmc2130_sg_stop_on_crash && crash)
 		{
 			tmc2130_sg_crash = crash;
 			tmc2130_sg_stop_on_crash = false;
@@ -499,7 +492,7 @@ void tmc2130_wr_MSLUTSTART(uint8_t axis, uint8_t start_sin, uint8_t start_sin90)
 	val |= (uint32_t)start_sin;
 	val |= ((uint32_t)start_sin90) << 16;
 	tmc2130_wr(axis, TMC2130_REG_MSLUTSTART, val);
-	//printf_P(PSTR("MSLUTSTART=%08lx (start_sin=%d start_sin90=%d)\n"), val, start_sin, start_sin90);

+	//printf_P(PSTR("MSLUTSTART=%08lx (start_sin=%d start_sin90=%d)\n"), val, start_sin, start_sin90);
 }
 
 void tmc2130_wr_MSLUTSEL(uint8_t axis, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t w0, uint8_t w1, uint8_t w2, uint8_t w3)
@@ -513,13 +506,13 @@ void tmc2130_wr_MSLUTSEL(uint8_t axis, uint8_t x1, uint8_t x2, uint8_t x3, uint8
 	val |= ((uint32_t)x2) << 16;
 	val |= ((uint32_t)x3) << 24;
 	tmc2130_wr(axis, TMC2130_REG_MSLUTSEL, val);
-	//printf_P(PSTR("MSLUTSEL=%08lx (x1=%d x2=%d x3=%d w0=%d w1=%d w2=%d w3=%d)\n"), val, x1, x2, x3, w0, w1, w2, w3);

+	//printf_P(PSTR("MSLUTSEL=%08lx (x1=%d x2=%d x3=%d w0=%d w1=%d w2=%d w3=%d)\n"), val, x1, x2, x3, w0, w1, w2, w3);
 }
 
 void tmc2130_wr_MSLUT(uint8_t axis, uint8_t i, uint32_t val)
 {
 	tmc2130_wr(axis, TMC2130_REG_MSLUT0 + (i & 7), val);
-	//printf_P(PSTR("MSLUT[%d]=%08lx\n"), i, val);

+	//printf_P(PSTR("MSLUT[%d]=%08lx\n"), i, val);
 }
 
 void tmc2130_wr_CHOPCONF(uint8_t axis, uint8_t toff, uint8_t hstrt, uint8_t hend, uint8_t fd3, uint8_t disfdcc, uint8_t rndtf, uint8_t chm, uint8_t tbl, uint8_t vsense, uint8_t vhighfs, uint8_t vhighchm, uint8_t sync, uint8_t mres, uint8_t intpol, uint8_t dedge, uint8_t diss2g)
@@ -574,43 +567,6 @@ uint8_t tmc2130_usteps2mres(uint16_t usteps)
 	return mres;
 }
 
-uint8_t tmc2130_wr(uint8_t axis, uint8_t addr, uint32_t wval)
-{
-	uint8_t stat = tmc2130_txrx(axis, addr | 0x80, wval, 0);
-#ifdef TMC2130_DEBUG_WR
-	MYSERIAL.print("tmc2130_wr(");
-	MYSERIAL.print((unsigned char)axis, DEC);
-	MYSERIAL.print(", 0x");
-	MYSERIAL.print((unsigned char)addr, HEX);
-	MYSERIAL.print(", 0x");
-	MYSERIAL.print((unsigned long)wval, HEX);
-	MYSERIAL.print(")=0x");
-	MYSERIAL.println((unsigned char)stat, HEX);
-#endif //TMC2130_DEBUG_WR
-	return stat;
-}
-
-uint8_t tmc2130_rd(uint8_t axis, uint8_t addr, uint32_t* rval)
-{
-	uint32_t val32 = 0;
-	uint8_t stat = tmc2130_txrx(axis, addr, 0x00000000, &val32);
-	if (rval != 0) *rval = val32;
-#ifdef TMC2130_DEBUG_RD
-	if (!skip_debug_msg)
-	{
-		MYSERIAL.print("tmc2130_rd(");
-		MYSERIAL.print((unsigned char)axis, DEC);
-		MYSERIAL.print(", 0x");
-		MYSERIAL.print((unsigned char)addr, HEX);
-		MYSERIAL.print(", 0x");
-		MYSERIAL.print((unsigned long)val32, HEX);
-		MYSERIAL.print(")=0x");
-		MYSERIAL.println((unsigned char)stat, HEX);
-	}
-	skip_debug_msg = false;
-#endif //TMC2130_DEBUG_RD
-	return stat;
-}
 
 inline void tmc2130_cs_low(uint8_t axis)
 {
@@ -634,7 +590,8 @@ inline void tmc2130_cs_high(uint8_t axis)
 	}
 }
 
-uint8_t tmc2130_txrx(uint8_t axis, uint8_t addr, uint32_t wval, uint32_t* rval)
+
+uint8_t tmc2130_tx(uint8_t axis, uint8_t addr, uint32_t wval)
 {
 	//datagram1 - request
 	SPI.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE3));
@@ -646,6 +603,20 @@ uint8_t tmc2130_txrx(uint8_t axis, uint8_t addr, uint32_t wval, uint32_t* rval)
 	SPI.transfer(wval & 0xff); // LSB
 	tmc2130_cs_high(axis);
 	SPI.endTransaction();
+}
+
+uint8_t tmc2130_rx(uint8_t axis, uint8_t addr, uint32_t* rval)
+{
+	//datagram1 - request
+	SPI.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE3));
+	tmc2130_cs_low(axis);
+	SPI.transfer(addr); // address
+	SPI.transfer(0); // MSB
+	SPI.transfer(0);
+	SPI.transfer(0);
+	SPI.transfer(0); // LSB
+	tmc2130_cs_high(axis);
+	SPI.endTransaction();
 	//datagram2 - response
 	SPI.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE3));
 	tmc2130_cs_low(axis);
@@ -852,8 +823,8 @@ void tmc2130_get_wave(uint8_t axis, uint8_t* data, FILE* stream)
 
 void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac200)
 {
-// TMC2130 wave compression algorithm

-// optimized for minimal memory requirements

+// TMC2130 wave compression algorithm
+// optimized for minimal memory requirements
 	printf_P(PSTR("tmc2130_set_wave %d %d\n"), axis, fac200);
 	if (fac200 < TMC2130_WAVE_FAC200_MIN) fac200 = 0;
 	if (fac200 > TMC2130_WAVE_FAC200_MAX) fac200 = TMC2130_WAVE_FAC200_MAX;
@@ -865,15 +836,15 @@ void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac200)
 	uint8_t w[4] = {1,1,1,1};      //W bits (MSLUTSEL)
 	uint8_t x[3] = {255,255,255};  //X segment bounds (MSLUTSEL)
 	uint8_t s = 0;                 //current segment
-	int8_t b;                      //encoded bit value

+	int8_t b;                      //encoded bit value
 	uint8_t dA;                    //delta value
-	int i;                         //microstep index

-	uint32_t reg;                  //tmc2130 register

-	tmc2130_wr_MSLUTSTART(axis, 0, amp);

+	int i;                         //microstep index
+	uint32_t reg;                  //tmc2130 register
+	tmc2130_wr_MSLUTSTART(axis, 0, amp);
 	for (i = 0; i < 256; i++)
-	{

-		if ((i & 31) == 0)

-			reg = 0;

+	{
+		if ((i & 31) == 0)
+			reg = 0;
 		// calculate value
 		if (fac == 0) // default TMC wave
 			vA = (uint8_t)((amp+1) * sin((2*PI*i + PI)/1024) + 0.5) - 1;
@@ -887,7 +858,7 @@ void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac200)
 		else
 		{
 			if (dA < d0) // delta < delta0 => switch wbit down
-			{

+			{
 				//printf("dn\n");
 				b = 0;
 				switch (dA)
@@ -897,11 +868,11 @@ void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac200)
 				case  1: d0 =  1; d1 = 2; w[s+1] = 2; break;
 				default: b = -1; break;
 				}
-				if (b >= 0) { x[s] = i; s++; }

+				if (b >= 0) { x[s] = i; s++; }
 			}
 			else if (dA > d1) // delta > delta0 => switch wbit up
 			{
-				//printf("up\n");

+				//printf("up\n");
 				b = 1;
 				switch (dA)
 				{
@@ -910,146 +881,20 @@ void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac200)
 				case  3: d0 =  2; d1 = 3; w[s+1] = 3; break;
 				default: b = -1; break;
 				}
-			    if (b >= 0) { x[s] = i; s++; }

+			    if (b >= 0) { x[s] = i; s++; }
 			}
 		}
-		if (b < 0) break; // delta out of range (<-1 or >3)

-		if (s > 3) break; // segment out of range (> 3)

-		//printf("%d\n", vA);

-		if (b == 1) reg |= 0x80000000;

-		if ((i & 31) == 31)

-			tmc2130_wr_MSLUT(axis, (uint8_t)(i >> 5), reg);

-		else

-			reg >>= 1;

-//		printf("%3d\t%3d\t%2d\t%2d\t%2d\t%2d    %08x\n", i, vA, dA, b, w[s], s, reg);

+		if (b < 0) break; // delta out of range (<-1 or >3)
+		if (s > 3) break; // segment out of range (> 3)
+		//printf("%d\n", vA);
+		if (b == 1) reg |= 0x80000000;
+		if ((i & 31) == 31)
+			tmc2130_wr_MSLUT(axis, (uint8_t)(i >> 5), reg);
+		else
+			reg >>= 1;
+//		printf("%3d\t%3d\t%2d\t%2d\t%2d\t%2d    %08x\n", i, vA, dA, b, w[s], s, reg);
 	}
-	tmc2130_wr_MSLUTSEL(axis, x[0], x[1], x[2], w[0], w[1], w[2], w[3]);

-
-/*
-//	printf_P(PSTR(" tmc2130_set_wave %d %d\n"), axis, fac200);
-	switch (fac200)
-	{
-	case 0: //default TMC wave 247/0
-		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0xaaaab556);
-		tmc2130_wr_MSLUT(axis, 1, 0x4a9554aa);
-		tmc2130_wr_MSLUT(axis, 2, 0x24492929);
-		tmc2130_wr_MSLUT(axis, 3, 0x10104222);
-		tmc2130_wr_MSLUT(axis, 4, 0xf8000000);
-		tmc2130_wr_MSLUT(axis, 5, 0xb5bb777d);
-		tmc2130_wr_MSLUT(axis, 6, 0x49295556);
-		tmc2130_wr_MSLUT(axis, 7, 0x00404222);
-		tmc2130_wr_MSLUTSEL(axis, 2, 154, 255, 1, 2, 1, 1);
-		break;
-	case 210: //calculated wave 247/1.050
-		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x55294a4e);
-		tmc2130_wr_MSLUT(axis, 1, 0xa52a552a);
-		tmc2130_wr_MSLUT(axis, 2, 0x48949294);
-		tmc2130_wr_MSLUT(axis, 3, 0x81042222);
-		tmc2130_wr_MSLUT(axis, 4, 0x00000000);
-		tmc2130_wr_MSLUT(axis, 5, 0xdb6eef7e);
-		tmc2130_wr_MSLUT(axis, 6, 0x9295555a);
-		tmc2130_wr_MSLUT(axis, 7, 0x00408444);
-		tmc2130_wr_MSLUTSEL(axis, 3, 160, 255, 1, 2, 1, 1);
-		break;
-	case 212: //calculated wave 247/1.060
-		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x4a94948e);
-		tmc2130_wr_MSLUT(axis, 1, 0x94a952a5);
-		tmc2130_wr_MSLUT(axis, 2, 0x24925252);
-		tmc2130_wr_MSLUT(axis, 3, 0x10421112);
-		tmc2130_wr_MSLUT(axis, 4, 0xc0000020);
-		tmc2130_wr_MSLUT(axis, 5, 0xdb7777df);
-		tmc2130_wr_MSLUT(axis, 6, 0x9295556a);
-		tmc2130_wr_MSLUT(axis, 7, 0x00408444);
-		tmc2130_wr_MSLUTSEL(axis, 3, 157, 255, 1, 2, 1, 1);
-		break;
-	case 214: //calculated wave 247/1.070
-		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0xa949489e);
-		tmc2130_wr_MSLUT(axis, 1, 0x52a54a54);
-		tmc2130_wr_MSLUT(axis, 2, 0x224a494a);
-		tmc2130_wr_MSLUT(axis, 3, 0x04108889);
-		tmc2130_wr_MSLUT(axis, 4, 0xffc08002);
-		tmc2130_wr_MSLUT(axis, 5, 0x6dbbbdfb);
-		tmc2130_wr_MSLUT(axis, 6, 0x94a555ab);
-		tmc2130_wr_MSLUT(axis, 7, 0x00408444);
-		tmc2130_wr_MSLUTSEL(axis, 4, 149, 255, 1, 2, 1, 1);
-		break;
-	case 215: //calculated wave 247/1.075
-		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x4a52491e);
-		tmc2130_wr_MSLUT(axis, 1, 0xa54a54a9);
-		tmc2130_wr_MSLUT(axis, 2, 0x49249494);
-		tmc2130_wr_MSLUT(axis, 3, 0x10421122);
-		tmc2130_wr_MSLUT(axis, 4, 0x00000008);
-		tmc2130_wr_MSLUT(axis, 5, 0x6ddbdefc);
-		tmc2130_wr_MSLUT(axis, 6, 0x94a555ad);
-		tmc2130_wr_MSLUT(axis, 7, 0x00408444);
-		tmc2130_wr_MSLUTSEL(axis, 4, 161, 255, 1, 2, 1, 1);
-		break;
-	case 216: //calculated wave 247/1.080
-		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x9494911e);
-		tmc2130_wr_MSLUT(axis, 1, 0x4a94a94a);
-		tmc2130_wr_MSLUT(axis, 2, 0x92492929);
-		tmc2130_wr_MSLUT(axis, 3, 0x41044444);
-		tmc2130_wr_MSLUT(axis, 4, 0x00000040);
-		tmc2130_wr_MSLUT(axis, 5, 0xaedddf7f);
-		tmc2130_wr_MSLUT(axis, 6, 0x94a956ad);
-		tmc2130_wr_MSLUT(axis, 7, 0x00808448);
-		tmc2130_wr_MSLUTSEL(axis, 4, 159, 255, 1, 2, 1, 1);
-		break;
-	case 218: //calculated wave 247/1.090
-		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x4a49223e);
-		tmc2130_wr_MSLUT(axis, 1, 0x4a52a529);
-		tmc2130_wr_MSLUT(axis, 2, 0x49252529);
-		tmc2130_wr_MSLUT(axis, 3, 0x08422224);
-		tmc2130_wr_MSLUT(axis, 4, 0xfc008004);
-		tmc2130_wr_MSLUT(axis, 5, 0xb6eef7df);
-		tmc2130_wr_MSLUT(axis, 6, 0xa4aaaab5);
-		tmc2130_wr_MSLUT(axis, 7, 0x00808448);
-		tmc2130_wr_MSLUTSEL(axis, 5, 153, 255, 1, 2, 1, 1);
-		break;
-	case 220: //calculated wave 247/1.100
-		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0xa492487e);
-		tmc2130_wr_MSLUT(axis, 1, 0x294a52a4);
-		tmc2130_wr_MSLUT(axis, 2, 0x492494a5);
-		tmc2130_wr_MSLUT(axis, 3, 0x82110912);
-		tmc2130_wr_MSLUT(axis, 4, 0x00000080);
-		tmc2130_wr_MSLUT(axis, 5, 0xdb777df8);
-		tmc2130_wr_MSLUT(axis, 6, 0x252aaad6);
-		tmc2130_wr_MSLUT(axis, 7, 0x00808449);
-		tmc2130_wr_MSLUTSEL(axis, 6, 162, 255, 1, 2, 1, 1);
-		break;
-	case 222: //calculated wave 247/1.110
-		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x524910fe);
-		tmc2130_wr_MSLUT(axis, 1, 0xa5294a52);
-		tmc2130_wr_MSLUT(axis, 2, 0x24929294);
-		tmc2130_wr_MSLUT(axis, 3, 0x20844489);
-		tmc2130_wr_MSLUT(axis, 4, 0xc0004008);
-		tmc2130_wr_MSLUT(axis, 5, 0xdbbbdf7f);
-		tmc2130_wr_MSLUT(axis, 6, 0x252aab5a);
-		tmc2130_wr_MSLUT(axis, 7, 0x00808449);
-		tmc2130_wr_MSLUTSEL(axis, 7, 157, 255, 1, 2, 1, 1);
-		break;
-	case 224: //calculated wave 247/1.120
-		tmc2130_wr_MSLUTSTART(axis, 0, 247);
-		tmc2130_wr_MSLUT(axis, 0, 0x292223fe);
-		tmc2130_wr_MSLUT(axis, 1, 0x94a52949);
-		tmc2130_wr_MSLUT(axis, 2, 0x92524a52);
-		tmc2130_wr_MSLUT(axis, 3, 0x04222244);
-		tmc2130_wr_MSLUT(axis, 4, 0x00000101);
-		tmc2130_wr_MSLUT(axis, 5, 0x6dddefe0);
-		tmc2130_wr_MSLUT(axis, 6, 0x254aad5b);
-		tmc2130_wr_MSLUT(axis, 7, 0x00810889);
-		tmc2130_wr_MSLUTSEL(axis, 9, 164, 255, 1, 2, 1, 1);
-		break;
-	}*/
+	tmc2130_wr_MSLUTSEL(axis, x[0], x[1], x[2], w[0], w[1], w[2], w[3]);
 }
 
 void bubblesort_uint8(uint8_t* data, uint8_t size, uint8_t* data2)
@@ -1106,7 +951,7 @@ uint8_t clusterize_uint8(uint8_t* data, uint8_t size, uint8_t* ccnt, uint8_t* cv
 	return ++cl;
 }
 
-void tmc2130_home_calibrate(uint8_t axis)
+bool tmc2130_home_calibrate(uint8_t axis)
 {
 	uint8_t step[16];
 	uint8_t cnt[16];
@@ -1125,6 +970,7 @@ void tmc2130_home_calibrate(uint8_t axis)
 	printf_P(PSTR("result value: %d\n"), tmc2130_home_origin[axis]);
 	if (axis == X_AXIS) eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_X_ORIGIN, tmc2130_home_origin[X_AXIS]);
 	else if (axis == Y_AXIS) eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_Y_ORIGIN, tmc2130_home_origin[Y_AXIS]);
+	return true;
 }
 
 #endif //TMC2130
diff --git a/Firmware/tmc2130.h b/Firmware/tmc2130.h
index ee0ce1baa..4f93a0c37 100644
--- a/Firmware/tmc2130.h
+++ b/Firmware/tmc2130.h
@@ -17,7 +17,7 @@ extern bool tmc2130_sg_stop_on_crash;
 extern uint8_t tmc2130_sg_crash; //crash mask
 
 extern uint8_t tmc2130_sg_meassure;
-extern uint16_t tmc2130_sg_meassure_cnt;
+extern uint32_t tmc2130_sg_meassure_cnt;
 extern uint32_t tmc2130_sg_meassure_val;
 
 #define TMC2130_MODE_NORMAL 0
@@ -119,6 +119,6 @@ extern void tmc2130_goto_step(uint8_t axis, uint8_t step, uint8_t dir, uint16_t
 extern void tmc2130_get_wave(uint8_t axis, uint8_t* data, FILE* stream);
 extern void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac200);
 
-extern void tmc2130_home_calibrate(uint8_t axis);
+extern bool tmc2130_home_calibrate(uint8_t axis);
 
 #endif //TMC2130_H
\ No newline at end of file
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 6ce2907e2..41c4b9f6d 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -1585,13 +1585,17 @@ static void lcd_menu_fails_stats()
 
 
 #ifdef DEBUG_BUILD
+#ifdef DEBUG_STACK_MONITOR
 extern uint16_t SP_min;
 extern char* __malloc_heap_start;
 extern char* __malloc_heap_end;
+#endif //DEBUG_STACK_MONITOR
 
 static void lcd_menu_debug()
 {
+#ifdef DEBUG_STACK_MONITOR
 	fprintf_P(lcdout, PSTR(ESC_H(1,1)"RAM statistics"ESC_H(5,1)"SP_min: 0x%04x"ESC_H(1,2)"heap_start: 0x%04x"ESC_H(3,3)"heap_end: 0x%04x"), SP_min, __malloc_heap_start, __malloc_heap_end);
+#endif //DEBUG_STACK_MONITOR
 
 	if (lcd_clicked())
     {
@@ -3949,6 +3953,7 @@ static void lcd_selftest_()
 	lcd_selftest();
 }
 
+#ifdef EXPERIMENTAL_FEATURES
 
 static void lcd_experimantal_menu();
 static void lcd_homing_accuracy_menu();
@@ -4160,6 +4165,7 @@ static void lcd_experimantal_menu()
 	MENU_ITEM(submenu, PSTR("uStep linearity"), lcd_ustep_linearity_menu);
 	END_MENU();
 }
+#endif //EXPERIMENTAL_FEATURES
 
 
 static void lcd_calibration_menu()
@@ -5371,7 +5377,10 @@ static void lcd_main_menu()
 	#endif
 	MENU_ITEM(submenu, MSG_SETTINGS, lcd_settings_menu);
     if(!isPrintPaused) MENU_ITEM(submenu, MSG_MENU_CALIBRATION, lcd_calibration_menu);
+
+#ifdef EXPERIMENTAL_FEATURES
 	MENU_ITEM(submenu, PSTR("Experimantal"), lcd_experimantal_menu);
+#endif //EXPERIMENTAL_FEATURES
   }
 
   if (!is_usb_printing && (lcd_commands_type != LCD_COMMAND_V2_CAL))
@@ -5840,10 +5849,10 @@ static void lcd_selftest_v()
 	(void)lcd_selftest();
 }
 
-static bool lcd_selftest()
+bool lcd_selftest()
 {
 	int _progress = 0;
-	bool _result = false;
+	bool _result = true;
 	lcd_wait_for_cool_down();
 	lcd_implementation_clear();
 	lcd.setCursor(0, 0); lcd_printPGM(MSG_SELFTEST_START);
@@ -5852,8 +5861,12 @@ static bool lcd_selftest()
 	#endif // TMC2130
 	delay(2000);
 	KEEPALIVE_STATE(IN_HANDLER);
-	_progress = lcd_selftest_screen(-1, _progress, 3, true, 2000);
-	_result = lcd_selftest_fan_dialog(0);
+
+	if (_result)
+	{
+		_progress = lcd_selftest_screen(-1, _progress, 3, true, 2000);
+		_result = lcd_selftest_fan_dialog(0);
+	}
 	
 	if (_result)
 	{
@@ -5932,12 +5945,24 @@ static bool lcd_selftest()
 		_result = lcd_selfcheck_axis(2, Z_MAX_POS);
 		if (eeprom_read_byte((uint8_t*)EEPROM_WIZARD_ACTIVE) != 1) {
 			enquecommand_P(PSTR("G28 W"));
-			enquecommand_P(PSTR("G1 Z15"));
+			enquecommand_P(PSTR("G1 Z15 F1000"));
 		}
 	}
 
 	if (_result)
 	{
+		_progress = lcd_selftest_screen(13, 0, 2, true, 0);
+		bool bres = tmc2130_home_calibrate(X_AXIS);
+		_progress = lcd_selftest_screen(13, 1, 2, true, 0);
+		bres &= tmc2130_home_calibrate(Y_AXIS);
+		_progress = lcd_selftest_screen(13, 2, 2, true, 0);
+		if (bres)
+			eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_ENABLED, 1);
+		_result = bres;
+	}
+
+	if (_result)
+	{
 		_progress = lcd_selftest_screen(7, _progress, 3, true, 2000); //check bed
 		_result = lcd_selfcheck_check_heater(true);
 	}
@@ -6719,6 +6744,7 @@ static int lcd_selftest_screen(int _step, int _progress, int _progress_scale, bo
 	if (_step == 10) lcd_printPGM(MSG_SELFTEST_CHECK_FSENSOR);
 	if (_step == 11) lcd_printPGM(MSG_SELFTEST_CHECK_ALLCORRECT);
 	if (_step == 12) lcd_printPGM(MSG_SELFTEST_FAILED);
+	if (_step == 13) lcd_printPGM(PSTR("Calibrating home"));
 
 	lcd.setCursor(0, 1);
 	lcd.print("--------------------");
diff --git a/Firmware/ultralcd.h b/Firmware/ultralcd.h
index 0940a9d8e..53ced8a1b 100644
--- a/Firmware/ultralcd.h
+++ b/Firmware/ultralcd.h
@@ -35,7 +35,7 @@ void lcd_mylang();
   bool lcd_detected(void);
 
   static void lcd_selftest_v();
-  static bool lcd_selftest();
+  extern bool lcd_selftest();
   static bool lcd_selfcheck_endstops();
 
 #ifdef TMC2130
diff --git a/Firmware/util.cpp b/Firmware/util.cpp
index 23acbee38..8aea54ab2 100644
--- a/Firmware/util.cpp
+++ b/Firmware/util.cpp
@@ -239,6 +239,30 @@ inline int8_t is_provided_version_newer(const char *version_string)
     return 0;
 }
 
+bool force_selftest_if_fw_version()
+{
+	//if fw version used before flashing new firmware (fw version currently stored in eeprom) is lower then 3.1.2-RC2, function returns true to force selftest
+
+	uint16_t ver_eeprom[4];
+	uint16_t ver_with_calibration[4] = {3, 1, 2, 4}; //hardcoded 3.1.2-RC2 version
+	bool force_selftest = false;
+
+	ver_eeprom[0] = eeprom_read_word((uint16_t*)EEPROM_FIRMWARE_VERSION_MAJOR);
+	ver_eeprom[1] = eeprom_read_word((uint16_t*)EEPROM_FIRMWARE_VERSION_MINOR);
+	ver_eeprom[2] = eeprom_read_word((uint16_t*)EEPROM_FIRMWARE_VERSION_REVISION);
+	ver_eeprom[3] = eeprom_read_word((uint16_t*)EEPROM_FIRMWARE_VERSION_FLAVOR);
+
+	for (uint8_t i = 0; i < 4; ++i) {
+		if (ver_with_calibration[i] > ver_eeprom[i]) {
+			force_selftest = true;
+			break;
+		}
+		else if (ver_with_calibration[i] < ver_eeprom[i])
+			break;
+	}
+	return force_selftest;
+}
+
 bool show_upgrade_dialog_if_version_newer(const char *version_string)
 {
     uint16_t ver_gcode[4], ver_current[4];
diff --git a/Firmware/util.h b/Firmware/util.h
index 52e4b6bce..7cbcb027b 100644
--- a/Firmware/util.h
+++ b/Firmware/util.h
@@ -18,6 +18,7 @@ enum FirmwareRevisionFlavorType
 };
 
 extern bool show_upgrade_dialog_if_version_newer(const char *version_string);
+extern bool force_selftest_if_fw_version();
 
 extern void update_current_firmware_version_to_eeprom();