diff --git a/Firmware/Configuration.h b/Firmware/Configuration.h
index eba8eb67f..5d7802bdb 100644
--- a/Firmware/Configuration.h
+++ b/Firmware/Configuration.h
@@ -5,7 +5,7 @@
 #include "Configuration_prusa.h"
 
 // Firmware version
-#define FW_version "3.0.10-8"
+#define FW_version "3.0.10-9"
 
 #define FW_PRUSA3D_MAGIC "PRUSA3DFW"
 #define FW_PRUSA3D_MAGIC_LEN 10
@@ -44,6 +44,8 @@
 #define EEPROM_BED_CORRECTION_REAR  (EEPROM_BED_CORRECTION_FRONT-1)
 #define EEPROM_TOSHIBA_FLASH_AIR_COMPATIBLITY (EEPROM_BED_CORRECTION_REAR-1)
 #define EEPROM_PRINT_FLAG (EEPROM_TOSHIBA_FLASH_AIR_COMPATIBLITY-1)
+#define EEPROM_PROBE_TEMP_SHIFT (EEPROM_PRINT_FLAG - 2*5) //5 x int for storing pinda probe temp shift relative to 50 C; unit: motor steps 
+#define EEPROM_TEMP_CAL_ACTIVE (EEPROM_PROBE_TEMP_SHIFT - 1)
 
 // Currently running firmware, each digit stored as uint16_t.
 // The flavor differentiates a dev, alpha, beta, release candidate or a release version.
@@ -458,8 +460,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define SDSUPPORT // Enable SD Card Support in Hardware Console
 //#define SDSLOW // Use slower SD transfer mode (not normally needed - uncomment if you're getting volume init error)
 #define SD_CHECK_AND_RETRY // Use CRC checks and retries on the SD communication
-#define ENCODER_PULSES_PER_STEP 2 // Increase if you have a high resolution encoder
-#define ENCODER_STEPS_PER_MENU_ITEM 2 // Set according to ENCODER_PULSES_PER_STEP or your liking
+#define ENCODER_PULSES_PER_STEP 4 // Increase if you have a high resolution encoder
+#define ENCODER_STEPS_PER_MENU_ITEM 1 // Set according to ENCODER_PULSES_PER_STEP or your liking
 //#define ULTIMAKERCONTROLLER //as available from the Ultimaker online store.
 //#define ULTIPANEL  //the UltiPanel as on Thingiverse
 //#define LCD_FEEDBACK_FREQUENCY_HZ 1000	// this is the tone frequency the buzzer plays when on UI feedback. ie Screen Click
@@ -699,17 +701,20 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // (unsigned char*)EEPROM_CALIBRATION_STATUS
 enum CalibrationStatus
 {
-    // Freshly assembled, needs to peform a self-test and the XYZ calibration.
-    CALIBRATION_STATUS_ASSEMBLED = 255,
+	// Freshly assembled, needs to peform a self-test and the XYZ calibration.
+	CALIBRATION_STATUS_ASSEMBLED = 255,
 
-    // For the wizard: self test has been performed, now the XYZ calibration is needed.
-    // CALIBRATION_STATUS_XYZ_CALIBRATION = 250,
+	// For the wizard: self test has been performed, now the XYZ calibration is needed.
+	// CALIBRATION_STATUS_XYZ_CALIBRATION = 250,
 
-    // For the wizard: factory assembled, needs to run Z calibration.
-    CALIBRATION_STATUS_Z_CALIBRATION = 240,
+	// For the wizard: factory assembled, needs to run Z calibration.
+	CALIBRATION_STATUS_Z_CALIBRATION = 240,
 
-    // The XYZ calibration has been performed, now it remains to run the V2Calibration.gcode.
-    CALIBRATION_STATUS_LIVE_ADJUST = 230,
+	// The XYZ calibration has been performed, now it remains to run the V2Calibration.gcode.
+	CALIBRATION_STATUS_LIVE_ADJUST = 230,
+
+	//V2 calibration has been run, now run PINDA probe temperature calibration
+	CALIBRATION_STATUS_PINDA = 220,
 
     // Calibrated, ready to print.
     CALIBRATION_STATUS_CALIBRATED = 1,
diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index ed981cad6..c0cc98fb6 100644
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -284,6 +284,7 @@ extern unsigned long starttime;
 extern unsigned long stoptime;
 extern bool is_usb_printing;
 extern bool homing_flag;
+extern bool temp_cal_active;
 extern bool loading_flag;
 extern unsigned int usb_printing_counter;
 
@@ -310,22 +311,33 @@ extern void digipot_i2c_init();
 
 #endif
 
+//Long pause
+extern int saved_feedmultiply;
+extern float HotendTempBckp;
+extern int fanSpeedBckp;
+extern float pause_lastpos[4];
+extern unsigned long pause_time;
 
-
-
+extern bool mesh_bed_leveling_flag;
 
 extern void calculate_volumetric_multipliers();
 
 // Similar to the default Arduino delay function, 
 // but it keeps the background tasks running.
-extern void delay_keep_alive(int ms);
+extern void delay_keep_alive(unsigned int ms);
 
 extern void check_babystep();
 
+extern void long_pause();
+
 #ifdef DIS
 
 void d_setup();
 float d_ReadData();
 void bed_analysis(float x_dimension, float y_dimension, int x_points_num, int y_points_num, float shift_x, float shift_y);
 
-#endif
\ No newline at end of file
+#endif
+float temp_comp_interpolation(float temperature);
+void temp_compensation_apply();
+void temp_compensation_start();
+void wait_for_heater(long codenum);
\ No newline at end of file
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index dc00e4b72..7793431b0 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -27,14 +27,6 @@
     http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
  */
 
-
-
-
-
-
-
-
-
 #include "Marlin.h"
 
 #ifdef ENABLE_AUTO_BED_LEVELING
@@ -63,6 +55,7 @@
 #include "math.h"
 #include "util.h"
 
+
 #ifdef BLINKM
 #include "BlinkM.h"
 #include "Wire.h"
@@ -257,12 +250,22 @@ int extruder_multiply[EXTRUDERS] = {100
 bool is_usb_printing = false;
 bool homing_flag = false;
 
+bool temp_cal_active = false;
+
 unsigned long kicktime = millis()+100000;
 
 unsigned int  usb_printing_counter;
 
 int lcd_change_fil_state = 0;
+
 int feedmultiplyBckp = 100;
+float HotendTempBckp = 0;
+int fanSpeedBckp = 0;
+float pause_lastpos[4];
+unsigned long pause_time = 0;
+
+bool mesh_bed_leveling_flag = false;
+
 unsigned char lang_selected = 0;
 int8_t FarmMode = 0;
 
@@ -1127,7 +1130,8 @@ void setup()
 #endif
   farm_mode = eeprom_read_byte((uint8_t*)EEPROM_FARM_MODE);
   EEPROM_read_B(EEPROM_FARM_NUMBER, &farm_no);
-  if (farm_mode == 0xFF && farm_no == 0) farm_mode = false; //if farm_mode has not been stored to eeprom yet and farm number is set to zero, deactivate farm mode
+  if ((farm_mode == 0xFF && farm_no == 0) || (farm_no == 0xFFFF)) farm_mode = false; //if farm_mode has not been stored to eeprom yet and farm number is set to zero or EEPROM is fresh, deactivate farm mode 
+  if (farm_no == 0xFFFF) farm_no = 0;
   if (farm_mode)
   {
 	  prusa_statistics(8);
@@ -1147,9 +1151,10 @@ void setup()
       // Once a firmware boots up, it forces at least a language selection, which changes
       // EEPROM_LANG to number lower than 0x0ff.
       // 1) Set a high power mode.
-      eeprom_write_byte((uint8_t*)EEPROM_SILENT, 0);
+      eeprom_write_byte((uint8_t*)EEPROM_SILENT, 0);	  
   }
 
+
   // In the future, somewhere here would one compare the current firmware version against the firmware version stored in the EEPROM.
   // If they differ, an update procedure may need to be performed. At the end of this block, the current firmware version
   // is being written into the EEPROM, so the update procedure will be triggered only once.
@@ -1157,6 +1162,11 @@ void setup()
     if (lang_selected >= LANG_NUM){
       lcd_mylang();
     }
+	
+	if (eeprom_read_byte((uint8_t*)EEPROM_TEMP_CAL_ACTIVE) == 255) {
+		eeprom_write_byte((uint8_t*)EEPROM_TEMP_CAL_ACTIVE, 0);
+		temp_cal_active = false;
+	} else temp_cal_active = eeprom_read_byte((uint8_t*)EEPROM_TEMP_CAL_ACTIVE);
 
 	check_babystep(); //checking if Z babystep is in allowed range
 	
@@ -1169,6 +1179,10 @@ void setup()
   } else if (calibration_status() == CALIBRATION_STATUS_LIVE_ADJUST) {
       // Show the message.
       lcd_show_fullscreen_message_and_wait_P(MSG_BABYSTEP_Z_NOT_SET);
+      lcd_update_enable(true);
+  } else if (calibration_status() == CALIBRATION_STATUS_PINDA && temp_cal_active == true) {
+	  lcd_show_fullscreen_message_and_wait_P(MSG_PINDA_NOT_CALIBRATED);
+	  lcd_update_enable(true);
   } else if (calibration_status() == CALIBRATION_STATUS_Z_CALIBRATION) {
       // Show the message.
       lcd_show_fullscreen_message_and_wait_P(MSG_FOLLOW_CALIBRATION_FLOW);
@@ -1313,7 +1327,7 @@ void loop()
 }
   //check heater every n milliseconds
   manage_heater();
-  manage_inactivity();
+  isPrintPaused ? manage_inactivity(true) : manage_inactivity(false);
   checkHitEndstops();
   lcd_update();
 }
@@ -1515,7 +1529,8 @@ void get_command()
         SERIAL_PROTOCOLLNRPGM(MSG_FILE_PRINTED);
         stoptime=millis();
         char time[30];
-        unsigned long t=(stoptime-starttime)/1000;
+        unsigned long t=(stoptime-starttime-pause_time)/1000;
+		pause_time = 0;
         int hours, minutes;
         minutes=(t/60)%60;
         hours=t/60/60;
@@ -2776,6 +2791,98 @@ void process_commands()
         }
         break;
 
+	case 76: //PINDA probe temperature calibration
+	{
+		setTargetBed(PINDA_MIN_T);
+		float zero_z;
+		int z_shift = 0; //unit: steps
+		int t_c; // temperature
+
+		if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS])) {
+			// We don't know where we are! HOME!
+			// Push the commands to the front of the message queue in the reverse order!
+			// There shall be always enough space reserved for these commands.
+			repeatcommand_front(); // repeat G76 with all its parameters
+			enquecommand_front_P((PSTR("G28 W0")));
+			break;
+		}
+		custom_message = true;
+		custom_message_type = 4;
+		custom_message_state = 1;
+		custom_message = MSG_TEMP_CALIBRATION;
+		current_position[X_AXIS] = PINDA_PREHEAT_X;
+		current_position[Y_AXIS] = PINDA_PREHEAT_Y;
+		current_position[Z_AXIS] = 0;
+		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
+		st_synchronize();
+
+		while (abs(degBed() - PINDA_MIN_T) > 1 ) delay_keep_alive(1000);
+		
+		//enquecommand_P(PSTR("M190 S50"));
+		for (int i = 0; i < PINDA_HEAT_T; i++)	delay_keep_alive(1000);
+
+		current_position[Z_AXIS] = 5;
+		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
+
+		current_position[X_AXIS] = pgm_read_float(bed_ref_points);
+		current_position[Y_AXIS] = pgm_read_float(bed_ref_points + 1);
+		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
+		st_synchronize();
+		
+		find_bed_induction_sensor_point_z(-1.f);
+		zero_z = current_position[Z_AXIS];
+
+		//current_position[Z_AXIS]
+		SERIAL_ECHOLNPGM("");
+		SERIAL_ECHOPGM("ZERO: ");
+		MYSERIAL.print(current_position[Z_AXIS]);
+		SERIAL_ECHOLNPGM("");
+
+		for (int i = 0; i<5; i++) {
+			custom_message_state = i + 2;
+			t_c = 60 + i * 10;
+
+			setTargetBed(t_c);
+			current_position[X_AXIS] = PINDA_PREHEAT_X;
+			current_position[Y_AXIS] = PINDA_PREHEAT_Y;
+			current_position[Z_AXIS] = 0;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
+			st_synchronize();
+			while (degBed() < t_c) delay_keep_alive(1000);
+			for (int i = 0; i < PINDA_HEAT_T; i++)	delay_keep_alive(1000);
+			current_position[Z_AXIS] = 5;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
+			current_position[X_AXIS] = pgm_read_float(bed_ref_points);
+			current_position[Y_AXIS] = pgm_read_float(bed_ref_points + 1);
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
+			st_synchronize();
+			find_bed_induction_sensor_point_z(-1.f);
+			z_shift = (int)((current_position[Z_AXIS] - zero_z)*axis_steps_per_unit[Z_AXIS]);
+
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOPGM("Temperature: ");
+			MYSERIAL.print(t_c);
+			SERIAL_ECHOPGM(" Z shift (mm):");
+			MYSERIAL.print(current_position[Z_AXIS] - zero_z);
+			SERIAL_ECHOLNPGM("");
+
+			EEPROM_save_B(EEPROM_PROBE_TEMP_SHIFT + i*2, &z_shift);
+			
+		
+		}
+		custom_message_type = 0;
+		custom_message = false;
+
+		calibration_status_store(CALIBRATION_STATUS_CALIBRATED);
+		lcd_show_fullscreen_message_and_wait_P(MSG_TEMP_CALIBRATION_DONE);
+		lcd_update_enable(true);
+		lcd_update(2);		
+
+		setTargetBed(0); //set bed target temperature back to 0
+
+	}
+	break;
+
 #ifdef DIS
 	case 77:
 	{
@@ -2818,181 +2925,255 @@ void process_commands()
 	*  v Y-axis
 	*
 	*/
-    case 80:
-    case_G80:
-        {
-            // Firstly check if we know where we are
-            if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS] ) ){
-                // We don't know where we are! HOME!
-                // Push the commands to the front of the message queue in the reverse order!
-                // There shall be always enough space reserved for these commands.
-                repeatcommand_front(); // repeat G80 with all its parameters
-                enquecommand_front_P((PSTR("G28 W0")));
-                break;
-            }
 
-            // Save custom message state, set a new custom message state to display: Calibrating point 9.
-            bool custom_message_old = custom_message;
-            unsigned int custom_message_type_old = custom_message_type;
-            unsigned int custom_message_state_old = custom_message_state;
-            custom_message = true;
-            custom_message_type = 1;
-            custom_message_state = (MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) + 10;
-            lcd_update(1);
-            
-            mbl.reset();
+  case 80:
+	case_G80:
+	{
+		mesh_bed_leveling_flag = true;
+		int8_t verbosity_level = 0;
+		static bool run = false;
 
-            // Reset baby stepping to zero, if the babystepping has already been loaded before. The babystepsTodo value will be
-            // consumed during the first movements following this statement.
-            babystep_undo();
+		if (code_seen('V')) {
+			// Just 'V' without a number counts as V1.
+			char c = strchr_pointer[1];
+			verbosity_level = (c == ' ' || c == '\t' || c == 0) ? 1 : code_value_short();
+		}
+		// Firstly check if we know where we are
+		if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS])) {
+			// We don't know where we are! HOME!
+			// Push the commands to the front of the message queue in the reverse order!
+			// There shall be always enough space reserved for these commands.
+			repeatcommand_front(); // repeat G80 with all its parameters
+			enquecommand_front_P((PSTR("G28 W0")));
+			break;
+		} 
+		
+		if (run == false && card.sdprinting == true && temp_cal_active == true && calibration_status() < CALIBRATION_STATUS_PINDA) {
+			temp_compensation_start();
+			run = true;
+			repeatcommand_front(); // repeat G80 with all its parameters
+			enquecommand_front_P((PSTR("G28 W0")));
+			break;
+		}
+		run = false;
 
-            // Cycle through all points and probe them
-            // First move up. During this first movement, the babystepping will be reverted.
-            current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
-            // The move to the first calibration point.
-            current_position[X_AXIS] = pgm_read_float(bed_ref_points);
-            current_position[Y_AXIS] = pgm_read_float(bed_ref_points+1);
-            world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-//            mbl.get_meas_xy(0, 0, current_position[X_AXIS], current_position[Y_AXIS], false);            
-            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/30, active_extruder);
-            // Wait until the move is finished.
-            st_synchronize();
-            
-            int mesh_point = 0;
-            
-            int ix = 0;
-            int iy = 0;
-            
-            int XY_AXIS_FEEDRATE = homing_feedrate[X_AXIS]/20;
-            int Z_PROBE_FEEDRATE = homing_feedrate[Z_AXIS]/60;
-            int Z_LIFT_FEEDRATE = homing_feedrate[Z_AXIS]/40;
-            bool has_z = is_bed_z_jitter_data_valid();
-            setup_for_endstop_move(false);
-            const char *kill_message = NULL;
-            while (mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) {
-                // Get coords of a measuring point.
-                ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
-                iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
-                if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
-                float z0 = 0.f;
-                if (has_z && mesh_point > 0) {
-                    uint16_t z_offset_u = eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER + 2 * (ix + iy * 3 - 1)));
-                    z0 = mbl.z_values[0][0] + *reinterpret_cast<int16_t*>(&z_offset_u) * 0.01;
-                    #if 0
-                    SERIAL_ECHOPGM("Bed leveling, point: ");
-                    MYSERIAL.print(mesh_point);
-                    SERIAL_ECHOPGM(", calibration z: ");
-                    MYSERIAL.print(z0, 5);
-                    SERIAL_ECHOLNPGM("");
-                    #endif
-                }
-            
-                // Move Z up to MESH_HOME_Z_SEARCH.
-                current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-                plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], Z_LIFT_FEEDRATE, active_extruder);
-                st_synchronize();
+		// Save custom message state, set a new custom message state to display: Calibrating point 9.
+		bool custom_message_old = custom_message;
+		unsigned int custom_message_type_old = custom_message_type;
+		unsigned int custom_message_state_old = custom_message_state;
+		custom_message = true;
+		custom_message_type = 1;
+		custom_message_state = (MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) + 10;
+		lcd_update(1);
 
-                // Move to XY position of the sensor point.
-                current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
-                current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
-                world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-                plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder);
-                st_synchronize();
-                
-                // Go down until endstop is hit
-                const float Z_CALIBRATION_THRESHOLD = 1.f;
-                if (! find_bed_induction_sensor_point_z((has_z && mesh_point > 0) ? z0 - Z_CALIBRATION_THRESHOLD : -10.f)) {
-                    kill_message = MSG_BED_LEVELING_FAILED_POINT_LOW;
-                    break;
-                }
-                if (MESH_HOME_Z_SEARCH - current_position[Z_AXIS] < 0.1f) {
-                    kill_message = MSG_BED_LEVELING_FAILED_PROBE_DISCONNECTED;
-                    break;
-                }
-                if (has_z && fabs(z0 - current_position[Z_AXIS]) > Z_CALIBRATION_THRESHOLD) {
-                    kill_message = MSG_BED_LEVELING_FAILED_POINT_HIGH;
-                    break;
-                }
+		mbl.reset(); //reset mesh bed leveling
 
-                mbl.set_z(ix, iy, current_position[Z_AXIS]);
+					 // Reset baby stepping to zero, if the babystepping has already been loaded before. The babystepsTodo value will be
+					 // consumed during the first movements following this statement.
+		babystep_undo();
 
-        				custom_message_state--;
-                mesh_point++;
-                lcd_update(1);
-            }
-            current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], Z_LIFT_FEEDRATE, active_extruder);
-            st_synchronize();
-            if (mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) {
-                kill(kill_message);
-            }
-            clean_up_after_endstop_move();
+		// Cycle through all points and probe them
+		// First move up. During this first movement, the babystepping will be reverted.
+		current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 60, active_extruder);
+		// The move to the first calibration point.
+		current_position[X_AXIS] = pgm_read_float(bed_ref_points);
+		current_position[Y_AXIS] = pgm_read_float(bed_ref_points + 1);
+		bool clamped = world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
 
-            // Apply Z height correction aka baby stepping before mesh bed leveing gets activated.
-            babystep_apply();
+		if (verbosity_level >= 1) {
+			clamped ? SERIAL_PROTOCOLPGM("First calibration point clamped.\n") : SERIAL_PROTOCOLPGM("No clamping for first calibration point.\n");
+		}
+		//            mbl.get_meas_xy(0, 0, current_position[X_AXIS], current_position[Y_AXIS], false);            
+		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS] / 30, active_extruder);
+		// Wait until the move is finished.
+		st_synchronize();
 
-            bool eeprom_bed_correction_valid = eeprom_read_byte((unsigned char*)EEPROM_BED_CORRECTION_VALID) == 1;
-            for (uint8_t i = 0; i < 4; ++ i) {
-                unsigned char codes[4] = { 'L', 'R', 'F', 'B' };
-                long correction = 0;
-                if (code_seen(codes[i]))
-                    correction = code_value_long();
-                else if (eeprom_bed_correction_valid) {
-                    unsigned char *addr = (i < 2) ? 
-                        ((i == 0) ? (unsigned char*)EEPROM_BED_CORRECTION_LEFT  : (unsigned char*)EEPROM_BED_CORRECTION_RIGHT) :
-                        ((i == 2) ? (unsigned char*)EEPROM_BED_CORRECTION_FRONT : (unsigned char*)EEPROM_BED_CORRECTION_REAR);
-                    correction = eeprom_read_int8(addr);
-                }
-                if (correction == 0)
-                    continue;
-                float offset = float(correction) * 0.001f;
-                if (fabs(offset) > 0.101f) {				
-                    SERIAL_ERROR_START;
-                    SERIAL_ECHOPGM("Excessive bed leveling correction: ");
-                    SERIAL_ECHO(offset);
-                    SERIAL_ECHOLNPGM(" microns");
-                } else {
-                    switch (i) {
-                    case 0:
-                        for (uint8_t row = 0; row < 3; ++ row) {
-                            mbl.z_values[row][1] += 0.5f * offset;
-                            mbl.z_values[row][0] += offset;
-                        }
-                        break;
-                    case 1:
-                        for (uint8_t row = 0; row < 3; ++ row) {
-                            mbl.z_values[row][1] += 0.5f * offset;
-                            mbl.z_values[row][2] += offset;
-                        }
-                        break;
-                    case 2:
-                        for (uint8_t col = 0; col < 3; ++ col) {
-                            mbl.z_values[1][col] += 0.5f * offset;
-                            mbl.z_values[0][col] += offset;
-                        }
-                        break;
-                    case 3:
-                        for (uint8_t col = 0; col < 3; ++ col) {
-                            mbl.z_values[1][col] += 0.5f * offset;
-                            mbl.z_values[2][col] += offset;
-                        }
-                        break;
-                    }
-                }
-            }
+		int mesh_point = 0; //index number of calibration point
 
-            mbl.upsample_3x3();
-            mbl.active = 1;
-            go_home_with_z_lift();
+		int ix = 0;
+		int iy = 0;
 
-            // Restore custom message state
-            custom_message       = custom_message_old;
-            custom_message_type  = custom_message_type_old;
-            custom_message_state = custom_message_state_old;
-            lcd_update(1);
-        }
-        break;
+		int XY_AXIS_FEEDRATE = homing_feedrate[X_AXIS] / 20;
+		int Z_PROBE_FEEDRATE = homing_feedrate[Z_AXIS] / 60;
+		int Z_LIFT_FEEDRATE = homing_feedrate[Z_AXIS] / 40;
+		bool has_z = is_bed_z_jitter_data_valid(); //checks if we have data from Z calibration (offsets of the Z heiths of the 8 calibration points from the first point)
+		if (verbosity_level >= 1) {
+			has_z ? SERIAL_PROTOCOLPGM("Z jitter data from Z cal. valid.\n") : SERIAL_PROTOCOLPGM("Z jitter data from Z cal. not valid.\n");
+		}
+		setup_for_endstop_move(false); //save feedrate and feedmultiply, sets feedmultiply to 100
+		const char *kill_message = NULL;
+		while (mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) {
+			if (verbosity_level >= 1) SERIAL_ECHOLNPGM("");
+			// Get coords of a measuring point.
+			ix = mesh_point % MESH_MEAS_NUM_X_POINTS; // from 0 to MESH_NUM_X_POINTS - 1
+			iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
+			if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
+			float z0 = 0.f;
+			if (has_z && mesh_point > 0) {
+				uint16_t z_offset_u = eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER + 2 * (ix + iy * 3 - 1)));
+				z0 = mbl.z_values[0][0] + *reinterpret_cast<int16_t*>(&z_offset_u) * 0.01;
+				//#if 0
+				if (verbosity_level >= 1) {
+					SERIAL_ECHOPGM("Bed leveling, point: ");
+					MYSERIAL.print(mesh_point);
+					SERIAL_ECHOPGM(", calibration z: ");
+					MYSERIAL.print(z0, 5);
+					SERIAL_ECHOLNPGM("");
+				}
+				//#endif
+			}
+
+			// Move Z up to MESH_HOME_Z_SEARCH.
+			current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], Z_LIFT_FEEDRATE, active_extruder);
+			st_synchronize();
+
+			// Move to XY position of the sensor point.
+			current_position[X_AXIS] = pgm_read_float(bed_ref_points + 2 * mesh_point);
+			current_position[Y_AXIS] = pgm_read_float(bed_ref_points + 2 * mesh_point + 1);
+
+
+
+			world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
+			if (verbosity_level >= 1) {
+
+				SERIAL_PROTOCOL(mesh_point);
+				clamped ? SERIAL_PROTOCOLPGM(": xy clamped.\n") : SERIAL_PROTOCOLPGM(": no xy clamping\n");
+			}
+
+
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder);
+			st_synchronize();
+
+			// Go down until endstop is hit
+			const float Z_CALIBRATION_THRESHOLD = 1.f;
+			if (!find_bed_induction_sensor_point_z((has_z && mesh_point > 0) ? z0 - Z_CALIBRATION_THRESHOLD : -10.f)) { //if we have data from z calibration max allowed difference is 1mm for each point, if we dont have data max difference is 10mm from initial point  
+				kill_message = MSG_BED_LEVELING_FAILED_POINT_LOW;
+				break;
+			}
+			if (MESH_HOME_Z_SEARCH - current_position[Z_AXIS] < 0.1f) {
+				kill_message = MSG_BED_LEVELING_FAILED_PROBE_DISCONNECTED;
+				break;
+			}
+			if (has_z && fabs(z0 - current_position[Z_AXIS]) > Z_CALIBRATION_THRESHOLD) { //if we have data from z calibration, max. allowed difference is 1mm for each point
+				kill_message = MSG_BED_LEVELING_FAILED_POINT_HIGH;
+				break;
+			}
+
+			if (verbosity_level >= 10) {
+				SERIAL_ECHOPGM("X: ");
+				MYSERIAL.print(current_position[X_AXIS], 5);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOPGM("Y: ");
+				MYSERIAL.print(current_position[Y_AXIS], 5);
+				SERIAL_PROTOCOLPGM("\n");
+			}
+
+			if (verbosity_level >= 1) {
+				SERIAL_ECHOPGM("mesh bed leveling: ");
+				MYSERIAL.print(current_position[Z_AXIS], 5);
+				SERIAL_ECHOLNPGM("");
+			}
+			mbl.set_z(ix, iy, current_position[Z_AXIS]); //store measured z values z_values[iy][ix] = z;
+
+			custom_message_state--;
+			mesh_point++;
+			lcd_update(1);
+		}
+		if (verbosity_level >= 20) SERIAL_ECHOLNPGM("Mesh bed leveling while loop finished.");
+		current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+		if (verbosity_level >= 20) {
+			SERIAL_ECHOLNPGM("MESH_HOME_Z_SEARCH: ");
+			MYSERIAL.print(current_position[Z_AXIS], 5);
+		}
+		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], Z_LIFT_FEEDRATE, active_extruder);
+		st_synchronize();
+		if (mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) {
+			kill(kill_message);
+			SERIAL_ECHOLNPGM("killed");
+		}
+		clean_up_after_endstop_move();
+		SERIAL_ECHOLNPGM("clean up finished ");
+		if(temp_cal_active == true && calibration_status() < CALIBRATION_STATUS_PINDA) temp_compensation_apply(); //apply PINDA temperature compensation
+		babystep_apply(); // Apply Z height correction aka baby stepping before mesh bed leveing gets activated.
+		SERIAL_ECHOLNPGM("babystep applied");
+		bool eeprom_bed_correction_valid = eeprom_read_byte((unsigned char*)EEPROM_BED_CORRECTION_VALID) == 1;
+
+		if (verbosity_level >= 1) {
+			eeprom_bed_correction_valid ? SERIAL_PROTOCOLPGM("Bed correction data valid\n") : SERIAL_PROTOCOLPGM("Bed correction data not valid\n");
+		}
+
+		for (uint8_t i = 0; i < 4; ++i) {
+			unsigned char codes[4] = { 'L', 'R', 'F', 'B' };
+			long correction = 0;
+			if (code_seen(codes[i]))
+				correction = code_value_long();
+			else if (eeprom_bed_correction_valid) {
+				unsigned char *addr = (i < 2) ?
+					((i == 0) ? (unsigned char*)EEPROM_BED_CORRECTION_LEFT : (unsigned char*)EEPROM_BED_CORRECTION_RIGHT) :
+					((i == 2) ? (unsigned char*)EEPROM_BED_CORRECTION_FRONT : (unsigned char*)EEPROM_BED_CORRECTION_REAR);
+				correction = eeprom_read_int8(addr);
+			}
+			if (correction == 0)
+				continue;
+			float offset = float(correction) * 0.001f;
+			if (fabs(offset) > 0.101f) {
+				SERIAL_ERROR_START;
+				SERIAL_ECHOPGM("Excessive bed leveling correction: ");
+				SERIAL_ECHO(offset);
+				SERIAL_ECHOLNPGM(" microns");
+			}
+			else {
+				switch (i) {
+				case 0:
+					for (uint8_t row = 0; row < 3; ++row) {
+						mbl.z_values[row][1] += 0.5f * offset;
+						mbl.z_values[row][0] += offset;
+					}
+					break;
+				case 1:
+					for (uint8_t row = 0; row < 3; ++row) {
+						mbl.z_values[row][1] += 0.5f * offset;
+						mbl.z_values[row][2] += offset;
+					}
+					break;
+				case 2:
+					for (uint8_t col = 0; col < 3; ++col) {
+						mbl.z_values[1][col] += 0.5f * offset;
+						mbl.z_values[0][col] += offset;
+					}
+					break;
+				case 3:
+					for (uint8_t col = 0; col < 3; ++col) {
+						mbl.z_values[1][col] += 0.5f * offset;
+						mbl.z_values[2][col] += offset;
+					}
+					break;
+				}
+			}
+		}
+		SERIAL_ECHOLNPGM("Bed leveling correction finished");
+		mbl.upsample_3x3(); //bilinear interpolation from 3x3 to 7x7 points while using the same array z_values[iy][ix] for storing (just coppying measured data to new destination and interpolating between them)
+		SERIAL_ECHOLNPGM("Upsample finished");
+		mbl.active = 1; //activate mesh bed leveling
+		SERIAL_ECHOLNPGM("Mesh bed leveling activated");
+		go_home_with_z_lift();
+		SERIAL_ECHOLNPGM("Go home finished");
+		//unretract (after PINDA preheat retraction)
+		if (card.sdprinting == true && degHotend(active_extruder) > EXTRUDE_MINTEMP && temp_cal_active == true) {
+			current_position[E_AXIS] += DEFAULT_RETRACTION;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 400, active_extruder);
+		}
+		// Restore custom message state
+		custom_message = custom_message_old;
+		custom_message_type = custom_message_type_old;
+		custom_message_state = custom_message_state_old;
+		mesh_bed_leveling_flag = false;
+		lcd_update(2);
+		
+	}
+	break;
 
         /**
          * G81: Print mesh bed leveling status and bed profile if activated
@@ -3088,7 +3269,7 @@ void process_commands()
              * This G-code will be performed at the end of a calibration script.
              */
         case 87:
-            calibration_status_store(CALIBRATION_STATUS_CALIBRATED);
+			calibration_status_store(CALIBRATION_STATUS_PINDA);
             break;
 
             /**
@@ -3874,11 +4055,18 @@ Sigma_Exit:
       #endif
 
         #ifdef SHOW_TEMP_ADC_VALUES
+          {float raw = 0.0;
+
           #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
             SERIAL_PROTOCOLPGM("    ADC B:");
             SERIAL_PROTOCOL_F(degBed(),1);
             SERIAL_PROTOCOLPGM("C->");
-            SERIAL_PROTOCOL_F(rawBedTemp()/OVERSAMPLENR,0);
+            raw = rawBedTemp();
+            SERIAL_PROTOCOL_F(raw/OVERSAMPLENR,5);
+            SERIAL_PROTOCOLPGM(" Rb->");
+            SERIAL_PROTOCOL_F(100 * (1 + (PtA * (raw/OVERSAMPLENR)) + (PtB * sq((raw/OVERSAMPLENR)))), 5);
+            SERIAL_PROTOCOLPGM(" Rxb->");
+            SERIAL_PROTOCOL_F(raw, 5);
           #endif
           for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
             SERIAL_PROTOCOLPGM("  T");
@@ -3886,8 +4074,17 @@ Sigma_Exit:
             SERIAL_PROTOCOLPGM(":");
             SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
             SERIAL_PROTOCOLPGM("C->");
-            SERIAL_PROTOCOL_F(rawHotendTemp(cur_extruder)/OVERSAMPLENR,0);
-          }
+            raw = rawHotendTemp(cur_extruder);
+            SERIAL_PROTOCOL_F(raw/OVERSAMPLENR,5);
+            SERIAL_PROTOCOLPGM(" Rt");
+            SERIAL_PROTOCOL(cur_extruder);
+            SERIAL_PROTOCOLPGM("->");
+            SERIAL_PROTOCOL_F(100 * (1 + (PtA * (raw/OVERSAMPLENR)) + (PtB * sq((raw/OVERSAMPLENR)))), 5);
+            SERIAL_PROTOCOLPGM(" Rx");
+            SERIAL_PROTOCOL(cur_extruder);
+            SERIAL_PROTOCOLPGM("->");
+            SERIAL_PROTOCOL_F(raw, 5);
+          }}
         #endif
 		SERIAL_PROTOCOLLN("");
       return;
@@ -3929,60 +4126,13 @@ Sigma_Exit:
 
       cancel_heatup = false;
 
-      #ifdef TEMP_RESIDENCY_TIME
-        long residencyStart;
-        residencyStart = -1;
-        /* continue to loop until we have reached the target temp
-          _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
-        while((!cancel_heatup)&&((residencyStart == -1) ||
-              (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL)))) ) {
-      #else
-        while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) ) {
-      #endif //TEMP_RESIDENCY_TIME
-          if( (millis() - codenum) > 1000UL )
-          { //Print Temp Reading and remaining time every 1 second while heating up/cooling down
-			  if (!farm_mode) {
-				  SERIAL_PROTOCOLPGM("T:");
-				  SERIAL_PROTOCOL_F(degHotend(tmp_extruder), 1);
-				  SERIAL_PROTOCOLPGM(" E:");
-				  SERIAL_PROTOCOL((int)tmp_extruder);
+	  wait_for_heater(codenum); //loops until target temperature is reached
 
-			#ifdef TEMP_RESIDENCY_TIME
-				  SERIAL_PROTOCOLPGM(" W:");
-				  if (residencyStart > -1)
-				  {
-					  codenum = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
-					  SERIAL_PROTOCOLLN(codenum);
-				  }
-				  else
-				  {
-					  SERIAL_PROTOCOLLN("?");
-				  }
-			  }
-            #else
-              SERIAL_PROTOCOLLN("");
-            #endif
-            codenum = millis();
-          }
-          manage_heater();
-          manage_inactivity();
-          lcd_update();
-        #ifdef TEMP_RESIDENCY_TIME
-            /* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
-              or when current temp falls outside the hysteresis after target temp was reached */
-          if ((residencyStart == -1 &&  target_direction && (degHotend(tmp_extruder) >= (degTargetHotend(tmp_extruder)-TEMP_WINDOW))) ||
-              (residencyStart == -1 && !target_direction && (degHotend(tmp_extruder) <= (degTargetHotend(tmp_extruder)+TEMP_WINDOW))) ||
-              (residencyStart > -1 && labs(degHotend(tmp_extruder) - degTargetHotend(tmp_extruder)) > TEMP_HYSTERESIS) )
-			  {
-				residencyStart = millis();
-			  }
-        #endif //TEMP_RESIDENCY_TIME
-        }
         LCD_MESSAGERPGM(MSG_HEATING_COMPLETE);
 		heating_status = 2;
 		if (farm_mode) { prusa_statistics(2); };
         
-        starttime=millis();
+        //starttime=millis();
         previous_millis_cmd = millis();
       }
       break;
@@ -4858,6 +5008,8 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
     case 600: //Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
     {
 		st_synchronize();
+		float target[4];
+		float lastpos[4];
 
         if (farm_mode)
             
@@ -4869,8 +5021,7 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
         
         feedmultiplyBckp=feedmultiply;
         int8_t TooLowZ = 0;
-        float target[4];
-        float lastpos[4];
+
         target[X_AXIS]=current_position[X_AXIS];
         target[Y_AXIS]=current_position[Y_AXIS];
         target[Z_AXIS]=current_position[Z_AXIS];
@@ -5078,6 +5229,15 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
     }
     break;
     #endif //FILAMENTCHANGEENABLE
+	case 601: {
+		if(lcd_commands_type == 0)  lcd_commands_type = LCD_COMMAND_LONG_PAUSE;
+	}
+	break;
+
+	case 602: {
+		if(lcd_commands_type == 0)	lcd_commands_type = LCD_COMMAND_LONG_PAUSE_RESUME;
+	}
+	break;
 
     case 907: // M907 Set digital trimpot motor current using axis codes.
     {
@@ -5148,10 +5308,10 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 		custom_message_type = 2;
 		
 		lcd_setstatuspgm(MSG_LOADING_FILAMENT);
-		current_position[E_AXIS] += 65;
+		current_position[E_AXIS] += 70;
 		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 400 / 60, active_extruder); //fast sequence
 
-		current_position[E_AXIS] += 40;
+		current_position[E_AXIS] += 25;
 		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 100 / 60, active_extruder); //slow sequence
 		st_synchronize();
 
@@ -5161,7 +5321,7 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 			while (!clean) {
 				lcd_update_enable(true);
 				lcd_update(2);
-				current_position[E_AXIS] += 40;
+				current_position[E_AXIS] += 25;
 				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 100 / 60, active_extruder); //slow sequence
 				st_synchronize();
 				clean = lcd_show_fullscreen_message_yes_no_and_wait_P(MSG_FILAMENT_CLEAN, false, true);
@@ -5181,7 +5341,8 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 		custom_message = true;
 		custom_message_type = 2;
 		lcd_setstatuspgm(MSG_UNLOADING_FILAMENT); 
-		
+		current_position[E_AXIS] += 3;
+		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 400 / 60, active_extruder);
 		current_position[E_AXIS] -= 80;
 		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 7000 / 60, active_extruder);
 		st_synchronize();
@@ -5825,7 +5986,7 @@ void calculate_volumetric_multipliers() {
 #endif
 }
 
-void delay_keep_alive(int ms)
+void delay_keep_alive(unsigned int ms)
 {
     for (;;) {
         manage_heater();
@@ -5844,6 +6005,59 @@ void delay_keep_alive(int ms)
     }
 }
 
+void wait_for_heater(long codenum) {
+
+#ifdef TEMP_RESIDENCY_TIME
+	long residencyStart;
+	residencyStart = -1;
+	/* continue to loop until we have reached the target temp
+	_and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
+	while ((!cancel_heatup) && ((residencyStart == -1) ||
+		(residencyStart >= 0 && (((unsigned int)(millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL))))) {
+#else
+	while (target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder) && (CooldownNoWait == false))) {
+#endif //TEMP_RESIDENCY_TIME
+		if ((millis() - codenum) > 1000UL)
+		{ //Print Temp Reading and remaining time every 1 second while heating up/cooling down
+			if (!farm_mode) {
+				SERIAL_PROTOCOLPGM("T:");
+				SERIAL_PROTOCOL_F(degHotend(tmp_extruder), 1);
+				SERIAL_PROTOCOLPGM(" E:");
+				SERIAL_PROTOCOL((int)tmp_extruder);
+
+#ifdef TEMP_RESIDENCY_TIME
+				SERIAL_PROTOCOLPGM(" W:");
+				if (residencyStart > -1)
+				{
+					codenum = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
+					SERIAL_PROTOCOLLN(codenum);
+				}
+				else
+				{
+					SERIAL_PROTOCOLLN("?");
+				}
+			}
+#else
+				SERIAL_PROTOCOLLN("");
+#endif
+				codenum = millis();
+		}
+			manage_heater();
+			manage_inactivity();
+			lcd_update();
+#ifdef TEMP_RESIDENCY_TIME
+			/* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
+			or when current temp falls outside the hysteresis after target temp was reached */
+			if ((residencyStart == -1 && target_direction && (degHotend(tmp_extruder) >= (degTargetHotend(tmp_extruder) - TEMP_WINDOW))) ||
+				(residencyStart == -1 && !target_direction && (degHotend(tmp_extruder) <= (degTargetHotend(tmp_extruder) + TEMP_WINDOW))) ||
+				(residencyStart > -1 && labs(degHotend(tmp_extruder) - degTargetHotend(tmp_extruder)) > TEMP_HYSTERESIS))
+			{
+				residencyStart = millis();
+			}
+#endif //TEMP_RESIDENCY_TIME
+	}
+}
+
 void check_babystep() {
 	int babystep_z;
 	EEPROM_read_B(EEPROM_BABYSTEP_Z, &babystep_z);
@@ -6091,5 +6305,158 @@ void bed_analysis(float x_dimension, float y_dimension, int x_points_num, int y_
 	card.closefile();
 
 }
+#endif
 
-#endif
\ No newline at end of file
+void temp_compensation_start() {
+	custom_message = true;
+	custom_message_type = 5;
+	if (degHotend(active_extruder)>EXTRUDE_MINTEMP) current_position[E_AXIS] -= DEFAULT_RETRACTION;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 400, active_extruder);
+	
+	current_position[X_AXIS] = PINDA_PREHEAT_X;
+	current_position[Y_AXIS] = PINDA_PREHEAT_Y;
+	current_position[Z_AXIS] = 0;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
+	st_synchronize();
+
+	while (fabs(degBed() - target_temperature_bed) > 3) delay_keep_alive(1000);
+
+	for(int i = 0; i < PINDA_HEAT_T; i++) delay_keep_alive(1000);
+
+	custom_message_type = 0;
+	custom_message = false;
+}
+
+void temp_compensation_apply() {
+	int i_add;
+	int compensation_value;
+	int z_shift = 0;
+	float z_shift_mm;
+
+	if (calibration_status() == CALIBRATION_STATUS_CALIBRATED) {
+		if (target_temperature_bed % 10 == 0 && target_temperature_bed >= 50 && target_temperature_bed <= 100) {
+			i_add = (target_temperature_bed - 60) / 10;
+			EEPROM_read_B(EEPROM_PROBE_TEMP_SHIFT + i_add * 2, &z_shift);
+			z_shift_mm = z_shift / axis_steps_per_unit[Z_AXIS];
+		}
+		else {
+			//interpolation
+			z_shift_mm = temp_comp_interpolation(target_temperature_bed) / axis_steps_per_unit[Z_AXIS];
+		}
+		SERIAL_PROTOCOLPGM("\n");
+		SERIAL_PROTOCOLPGM("Z shift applied:");
+		MYSERIAL.print(z_shift_mm);
+		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] - z_shift_mm, current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
+		st_synchronize();
+		plan_set_z_position(current_position[Z_AXIS]);
+	}
+	else {		
+		//message that we have no temp compensation data ?
+	}
+}
+
+float temp_comp_interpolation(float inp_temperature) {
+
+	//cubic spline interpolation
+
+	int n, i, j, k;
+	float h[10], a, b, c, d, sum, s[10] = { 0 }, x[10], F[10], f[10], p, m[10][10] = { 0 }, temp;
+	int shift[10];
+	int temp_C[10];
+
+	p = inp_temperature;
+	n = 6; //number of measured points
+
+	shift[0] = 0;
+	for (i = 0; i < n; i++) {
+		//scanf_s("%f%f", &x[i], &f[i]);
+		if (i>0) EEPROM_read_B(EEPROM_PROBE_TEMP_SHIFT + (i-1) * 2, &shift[i]); //read shift in steps from EEPROM
+		temp_C[i] = 50 + i * 10; //temperature in C
+		
+		x[i] = (float)temp_C[i];
+		f[i] = (float)shift[i];
+	}
+
+
+
+	for (i = n - 1; i>0; i--) {
+		F[i] = (f[i] - f[i - 1]) / (x[i] - x[i - 1]);
+		h[i - 1] = x[i] - x[i - 1];
+	}
+	//*********** formation of h, s , f matrix **************
+	for (i = 1; i<n - 1; i++) {
+		m[i][i] = 2 * (h[i - 1] + h[i]);
+		if (i != 1) {
+			m[i][i - 1] = h[i - 1];
+			m[i - 1][i] = h[i - 1];
+		}
+		m[i][n - 1] = 6 * (F[i + 1] - F[i]);
+	}
+	//*********** forward elimination **************
+	for (i = 1; i<n - 2; i++) {
+		temp = (m[i + 1][i] / m[i][i]);
+		for (j = 1; j <= n - 1; j++)
+			m[i + 1][j] -= temp*m[i][j];
+	}
+	//*********** backward substitution *********
+	for (i = n - 2; i>0; i--) {
+		sum = 0;
+		for (j = i; j <= n - 2; j++)
+			sum += m[i][j] * s[j];
+		s[i] = (m[i][n - 1] - sum) / m[i][i];
+	}
+
+		for (i = 0; i<n - 1; i++)
+			if (x[i] <= p&&p <= x[i + 1]) {
+				a = (s[i + 1] - s[i]) / (6 * h[i]);
+				b = s[i] / 2;
+				c = (f[i + 1] - f[i]) / h[i] - (2 * h[i] * s[i] + s[i + 1] * h[i]) / 6;
+				d = f[i];
+				sum = a*pow((p - x[i]), 3) + b*pow((p - x[i]), 2) + c*(p - x[i]) + d;
+			}
+
+		return sum;
+
+}
+
+void long_pause() //long pause print
+{
+	st_synchronize();
+	
+	//save currently set parameters to global variables
+	saved_feedmultiply = feedmultiply; 
+	HotendTempBckp = degTargetHotend(active_extruder);
+	fanSpeedBckp = fanSpeed;
+	pause_time += (millis() - starttime);
+	
+
+	//save position
+	pause_lastpos[X_AXIS] = current_position[X_AXIS];
+	pause_lastpos[Y_AXIS] = current_position[Y_AXIS];
+	pause_lastpos[Z_AXIS] = current_position[Z_AXIS];
+	pause_lastpos[E_AXIS] = current_position[E_AXIS];
+
+	//retract
+	current_position[E_AXIS] -= PAUSE_RETRACT;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 400, active_extruder);
+
+	//lift z
+	current_position[Z_AXIS] += Z_PAUSE_LIFT;
+	if (current_position[Z_AXIS] > Z_MAX_POS) current_position[Z_AXIS] = Z_MAX_POS;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 15, active_extruder);
+
+	//set nozzle target temperature to 0
+	setTargetHotend(0, 0);
+	setTargetHotend(0, 1);
+	setTargetHotend(0, 2);
+
+	//Move XY to side
+	current_position[X_AXIS] = X_PAUSE_POS;
+	current_position[Y_AXIS] = Y_PAUSE_POS;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 50, active_extruder);
+
+	// Turn off the print fan
+	fanSpeed = 0;
+
+	st_synchronize();
+}
\ No newline at end of file
diff --git a/Firmware/SdBaseFile.cpp b/Firmware/SdBaseFile.cpp
index dbcf77fbd..be04ab1a2 100644
--- a/Firmware/SdBaseFile.cpp
+++ b/Firmware/SdBaseFile.cpp
@@ -294,7 +294,7 @@ bool SdBaseFile::getFilename(char* name) {
   return true;
 }
 //------------------------------------------------------------------------------
-void SdBaseFile::getpos(fpos_t* pos) {
+void SdBaseFile::getpos(filepos_t* pos) {
   pos->position = curPosition_;
   pos->cluster = curCluster_;
 }
@@ -925,7 +925,7 @@ bool SdBaseFile::openRoot(SdVolume* vol) {
  * \return The byte if no error and not at eof else -1;
  */
 int SdBaseFile::peek() {
-  fpos_t pos;
+  filepos_t pos;
   getpos(&pos);
   int c = read();
   if (c >= 0) setpos(&pos);
@@ -1492,7 +1492,7 @@ bool SdBaseFile::seekSet(uint32_t pos) {
   return false;
 }
 //------------------------------------------------------------------------------
-void SdBaseFile::setpos(fpos_t* pos) {
+void SdBaseFile::setpos(filepos_t* pos) {
   curPosition_ = pos->position;
   curCluster_ = pos->cluster;
 }
diff --git a/Firmware/SdBaseFile.h b/Firmware/SdBaseFile.h
index dea299a64..923a391dd 100644
--- a/Firmware/SdBaseFile.h
+++ b/Firmware/SdBaseFile.h
@@ -31,16 +31,16 @@
 #include "SdVolume.h"
 //------------------------------------------------------------------------------
 /**
- * \struct fpos_t
+ * \struct filepos_t
  * \brief internal type for istream
  * do not use in user apps
  */
-struct fpos_t {
+struct filepos_t {
   /** stream position */
   uint32_t position;
   /** cluster for position */
   uint32_t cluster;
-  fpos_t() : position(0), cluster(0) {}
+  filepos_t() : position(0), cluster(0) {}
 };
 
 // use the gnu style oflag in open()
@@ -196,11 +196,11 @@ class SdBaseFile {
   /** get position for streams
    * \param[out] pos struct to receive position
    */
-  void getpos(fpos_t* pos);
+  void getpos(filepos_t* pos);
   /** set position for streams
    * \param[out] pos struct with value for new position
    */
-  void setpos(fpos_t* pos);
+  void setpos(filepos_t* pos);
   //----------------------------------------------------------------------------
   bool close();
   bool contiguousRange(uint32_t* bgnBlock, uint32_t* endBlock);
diff --git a/Firmware/language_all.cpp b/Firmware/language_all.cpp
index 3cdad48f0..a55b30e85 100644
--- a/Firmware/language_all.cpp
+++ b/Firmware/language_all.cpp
@@ -494,6 +494,28 @@ const char * const MSG_CALIBRATE_E_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_CALIBRATE_E_DE
 };
 
+const char MSG_CALIBRATE_PINDA_EN[] PROGMEM = "Calibrate";
+const char MSG_CALIBRATE_PINDA_CZ[] PROGMEM = "Zkalibrovat";
+const char * const MSG_CALIBRATE_PINDA_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_CALIBRATE_PINDA_EN,
+	MSG_CALIBRATE_PINDA_CZ,
+	MSG_CALIBRATE_PINDA_EN,
+	MSG_CALIBRATE_PINDA_EN,
+	MSG_CALIBRATE_PINDA_EN,
+	MSG_CALIBRATE_PINDA_EN
+};
+
+const char MSG_CALIBRATION_PINDA_MENU_EN[] PROGMEM = "Temp. calibration";
+const char MSG_CALIBRATION_PINDA_MENU_CZ[] PROGMEM = "Teplotni kalibrace";
+const char * const MSG_CALIBRATION_PINDA_MENU_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_CALIBRATION_PINDA_MENU_EN,
+	MSG_CALIBRATION_PINDA_MENU_CZ,
+	MSG_CALIBRATION_PINDA_MENU_EN,
+	MSG_CALIBRATION_PINDA_MENU_EN,
+	MSG_CALIBRATION_PINDA_MENU_EN,
+	MSG_CALIBRATION_PINDA_MENU_EN
+};
+
 const char MSG_CARD_MENU_EN[] PROGMEM = "Print from SD";
 const char MSG_CARD_MENU_CZ[] PROGMEM = "Tisk z SD";
 const char MSG_CARD_MENU_IT[] PROGMEM = "Stampa da SD";
@@ -966,6 +988,11 @@ const char * const MSG_FIL_TUNING_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_FIL_TUNING_DE
 };
 
+const char MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION_EN[] PROGMEM = "Iteration ";
+const char * const MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION_LANG_TABLE[1] PROGMEM = {
+	MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION_EN
+};
+
 const char MSG_FIND_BED_OFFSET_AND_SKEW_LINE1_EN[] PROGMEM = "Searching bed calibration point";
 const char MSG_FIND_BED_OFFSET_AND_SKEW_LINE1_CZ[] PROGMEM = "Hledam kalibracni bod podlozky";
 const char MSG_FIND_BED_OFFSET_AND_SKEW_LINE1_IT[] PROGMEM = "Ricerca del letto punto di calibraz.";
@@ -996,6 +1023,17 @@ const char * const MSG_FIND_BED_OFFSET_AND_SKEW_LINE2_LANG_TABLE[LANG_NUM] PROGM
 	MSG_FIND_BED_OFFSET_AND_SKEW_LINE2_DE
 };
 
+const char MSG_FINISHING_MOVEMENTS_EN[] PROGMEM = "Finishing movements";
+const char MSG_FINISHING_MOVEMENTS_CZ[] PROGMEM = "Dokoncovani pohybu";
+const char * const MSG_FINISHING_MOVEMENTS_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_FINISHING_MOVEMENTS_EN,
+	MSG_FINISHING_MOVEMENTS_CZ,
+	MSG_FINISHING_MOVEMENTS_EN,
+	MSG_FINISHING_MOVEMENTS_EN,
+	MSG_FINISHING_MOVEMENTS_EN,
+	MSG_FINISHING_MOVEMENTS_EN
+};
+
 const char MSG_FLOW_EN[] PROGMEM = "Flow";
 const char MSG_FLOW_CZ[] PROGMEM = "Prutok";
 const char MSG_FLOW_IT[] PROGMEM = "Flusso";
@@ -1731,6 +1769,61 @@ const char * const MSG_PICK_Z_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_PICK_Z_DE
 };
 
+const char MSG_PID_EXTRUDER_EN[] PROGMEM = "PID calibration";
+const char MSG_PID_EXTRUDER_CZ[] PROGMEM = "PID kalibrace";
+const char * const MSG_PID_EXTRUDER_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_PID_EXTRUDER_EN,
+	MSG_PID_EXTRUDER_CZ,
+	MSG_PID_EXTRUDER_EN,
+	MSG_PID_EXTRUDER_EN,
+	MSG_PID_EXTRUDER_EN,
+	MSG_PID_EXTRUDER_EN
+};
+
+const char MSG_PID_FINISHED_EN[] PROGMEM = "PID cal. finished";
+const char MSG_PID_FINISHED_CZ[] PROGMEM = "PID kal. ukoncena";
+const char * const MSG_PID_FINISHED_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_PID_FINISHED_EN,
+	MSG_PID_FINISHED_CZ,
+	MSG_PID_FINISHED_EN,
+	MSG_PID_FINISHED_EN,
+	MSG_PID_FINISHED_EN,
+	MSG_PID_FINISHED_EN
+};
+
+const char MSG_PID_RUNNING_EN[] PROGMEM = "PID cal.           ";
+const char MSG_PID_RUNNING_CZ[] PROGMEM = "PID kal.           ";
+const char * const MSG_PID_RUNNING_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_PID_RUNNING_EN,
+	MSG_PID_RUNNING_CZ,
+	MSG_PID_RUNNING_EN,
+	MSG_PID_RUNNING_EN,
+	MSG_PID_RUNNING_EN,
+	MSG_PID_RUNNING_EN
+};
+
+const char MSG_PINDA_NOT_CALIBRATED_EN[] PROGMEM = "Temperature calibration has not been run yet";
+const char MSG_PINDA_NOT_CALIBRATED_CZ[] PROGMEM = "Tiskarna nebyla teplotne zkalibrovana";
+const char * const MSG_PINDA_NOT_CALIBRATED_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_PINDA_NOT_CALIBRATED_EN,
+	MSG_PINDA_NOT_CALIBRATED_CZ,
+	MSG_PINDA_NOT_CALIBRATED_EN,
+	MSG_PINDA_NOT_CALIBRATED_EN,
+	MSG_PINDA_NOT_CALIBRATED_EN,
+	MSG_PINDA_NOT_CALIBRATED_EN
+};
+
+const char MSG_PINDA_PREHEAT_EN[] PROGMEM = "Preheating";
+const char MSG_PINDA_PREHEAT_CZ[] PROGMEM = "Predehrivani";
+const char * const MSG_PINDA_PREHEAT_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_PINDA_PREHEAT_EN,
+	MSG_PINDA_PREHEAT_CZ,
+	MSG_PINDA_PREHEAT_EN,
+	MSG_PINDA_PREHEAT_EN,
+	MSG_PINDA_PREHEAT_EN,
+	MSG_PINDA_PREHEAT_EN
+};
+
 const char MSG_PLANNER_BUFFER_BYTES_EN[] PROGMEM = "  PlannerBufferBytes: ";
 const char * const MSG_PLANNER_BUFFER_BYTES_LANG_TABLE[1] PROGMEM = {
 	MSG_PLANNER_BUFFER_BYTES_EN
@@ -1826,6 +1919,17 @@ const char * const MSG_PRINT_ABORTED_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_PRINT_ABORTED_DE
 };
 
+const char MSG_PRINT_PAUSED_EN[] PROGMEM = "Print paused";
+const char MSG_PRINT_PAUSED_CZ[] PROGMEM = "Tisk pozastaven";
+const char * const MSG_PRINT_PAUSED_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_PRINT_PAUSED_EN,
+	MSG_PRINT_PAUSED_CZ,
+	MSG_PRINT_PAUSED_EN,
+	MSG_PRINT_PAUSED_EN,
+	MSG_PRINT_PAUSED_EN,
+	MSG_PRINT_PAUSED_EN
+};
+
 const char MSG_PRUSA3D_EN[] PROGMEM = "prusa3d.com";
 const char MSG_PRUSA3D_CZ[] PROGMEM = "prusa3d.cz";
 const char MSG_PRUSA3D_PL[] PROGMEM = "prusa3d.cz";
@@ -1932,6 +2036,17 @@ const char * const MSG_RESUMING_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_RESUMING_DE
 };
 
+const char MSG_RESUMING_PRINT_EN[] PROGMEM = "Resuming print";
+const char MSG_RESUMING_PRINT_CZ[] PROGMEM = "Obnovovani tisku";
+const char * const MSG_RESUMING_PRINT_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_RESUMING_PRINT_EN,
+	MSG_RESUMING_PRINT_CZ,
+	MSG_RESUMING_PRINT_EN,
+	MSG_RESUMING_PRINT_EN,
+	MSG_RESUMING_PRINT_EN,
+	MSG_RESUMING_PRINT_EN
+};
+
 const char MSG_SD_CANT_ENTER_SUBDIR_EN[] PROGMEM = "Cannot enter subdir: ";
 const char * const MSG_SD_CANT_ENTER_SUBDIR_LANG_TABLE[1] PROGMEM = {
 	MSG_SD_CANT_ENTER_SUBDIR_EN
@@ -2431,6 +2546,17 @@ const char * const MSG_SET_ORIGIN_LANG_TABLE[1] PROGMEM = {
 	MSG_SET_ORIGIN_EN
 };
 
+const char MSG_SET_TEMPERATURE_EN[] PROGMEM = "Set temperature:";
+const char MSG_SET_TEMPERATURE_CZ[] PROGMEM = "Nastavte teplotu:";
+const char * const MSG_SET_TEMPERATURE_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_SET_TEMPERATURE_EN,
+	MSG_SET_TEMPERATURE_CZ,
+	MSG_SET_TEMPERATURE_EN,
+	MSG_SET_TEMPERATURE_EN,
+	MSG_SET_TEMPERATURE_EN,
+	MSG_SET_TEMPERATURE_EN
+};
+
 const char MSG_SHOW_END_STOPS_EN[] PROGMEM = "Show end stops";
 const char MSG_SHOW_END_STOPS_CZ[] PROGMEM = "Stav konc. spin.";
 const char MSG_SHOW_END_STOPS_IT[] PROGMEM = "Stato finecorsa";
@@ -2683,6 +2809,50 @@ const char * const MSG_TEMPERATURE_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_TEMPERATURE_DE
 };
 
+const char MSG_TEMP_CALIBRATION_EN[] PROGMEM = "Temp. cal.          ";
+const char MSG_TEMP_CALIBRATION_CZ[] PROGMEM = "Tepl. kal.          ";
+const char * const MSG_TEMP_CALIBRATION_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_TEMP_CALIBRATION_EN,
+	MSG_TEMP_CALIBRATION_CZ,
+	MSG_TEMP_CALIBRATION_EN,
+	MSG_TEMP_CALIBRATION_EN,
+	MSG_TEMP_CALIBRATION_EN,
+	MSG_TEMP_CALIBRATION_EN
+};
+
+const char MSG_TEMP_CALIBRATION_DONE_EN[] PROGMEM = "Temperature calibration is finished. Click to continue.";
+const char MSG_TEMP_CALIBRATION_DONE_CZ[] PROGMEM = "Teplotni kalibrace dokoncena. Pokracujte stiskem tlacitka.";
+const char * const MSG_TEMP_CALIBRATION_DONE_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_TEMP_CALIBRATION_DONE_EN,
+	MSG_TEMP_CALIBRATION_DONE_CZ,
+	MSG_TEMP_CALIBRATION_DONE_EN,
+	MSG_TEMP_CALIBRATION_DONE_EN,
+	MSG_TEMP_CALIBRATION_DONE_EN,
+	MSG_TEMP_CALIBRATION_DONE_EN
+};
+
+const char MSG_TEMP_CALIBRATION_OFF_EN[] PROGMEM = "Temp. cal. [OFF]";
+const char MSG_TEMP_CALIBRATION_OFF_CZ[] PROGMEM = "Tepl. kal. [OFF]";
+const char * const MSG_TEMP_CALIBRATION_OFF_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_TEMP_CALIBRATION_OFF_EN,
+	MSG_TEMP_CALIBRATION_OFF_CZ,
+	MSG_TEMP_CALIBRATION_OFF_EN,
+	MSG_TEMP_CALIBRATION_OFF_EN,
+	MSG_TEMP_CALIBRATION_OFF_EN,
+	MSG_TEMP_CALIBRATION_OFF_EN
+};
+
+const char MSG_TEMP_CALIBRATION_ON_EN[] PROGMEM = "Temp. cal. [ON]";
+const char MSG_TEMP_CALIBRATION_ON_CZ[] PROGMEM = "Tepl. kal. [ON]";
+const char * const MSG_TEMP_CALIBRATION_ON_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_TEMP_CALIBRATION_ON_EN,
+	MSG_TEMP_CALIBRATION_ON_CZ,
+	MSG_TEMP_CALIBRATION_ON_EN,
+	MSG_TEMP_CALIBRATION_ON_EN,
+	MSG_TEMP_CALIBRATION_ON_EN,
+	MSG_TEMP_CALIBRATION_ON_EN
+};
+
 const char MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF_EN[] PROGMEM = "SD card [normal]";
 const char MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF_PL[] PROGMEM = "karta SD [normal]";
 const char * const MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF_LANG_TABLE[LANG_NUM] PROGMEM = {
diff --git a/Firmware/language_all.h b/Firmware/language_all.h
index 47dde007e..1844f09c1 100644
--- a/Firmware/language_all.h
+++ b/Firmware/language_all.h
@@ -107,6 +107,10 @@ extern const char* const MSG_CALIBRATE_BED_RESET_LANG_TABLE[LANG_NUM];
 #define MSG_CALIBRATE_BED_RESET LANG_TABLE_SELECT(MSG_CALIBRATE_BED_RESET_LANG_TABLE)
 extern const char* const MSG_CALIBRATE_E_LANG_TABLE[LANG_NUM];
 #define MSG_CALIBRATE_E LANG_TABLE_SELECT(MSG_CALIBRATE_E_LANG_TABLE)
+extern const char* const MSG_CALIBRATE_PINDA_LANG_TABLE[LANG_NUM];
+#define MSG_CALIBRATE_PINDA LANG_TABLE_SELECT(MSG_CALIBRATE_PINDA_LANG_TABLE)
+extern const char* const MSG_CALIBRATION_PINDA_MENU_LANG_TABLE[LANG_NUM];
+#define MSG_CALIBRATION_PINDA_MENU LANG_TABLE_SELECT(MSG_CALIBRATION_PINDA_MENU_LANG_TABLE)
 extern const char* const MSG_CARD_MENU_LANG_TABLE[LANG_NUM];
 #define MSG_CARD_MENU LANG_TABLE_SELECT(MSG_CARD_MENU_LANG_TABLE)
 extern const char* const MSG_CHANGE_EXTR_LANG_TABLE[LANG_NUM];
@@ -201,10 +205,14 @@ extern const char* const MSG_FIL_LOADED_CHECK_LANG_TABLE[LANG_NUM];
 #define MSG_FIL_LOADED_CHECK LANG_TABLE_SELECT(MSG_FIL_LOADED_CHECK_LANG_TABLE)
 extern const char* const MSG_FIL_TUNING_LANG_TABLE[LANG_NUM];
 #define MSG_FIL_TUNING LANG_TABLE_SELECT(MSG_FIL_TUNING_LANG_TABLE)
+extern const char* const MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION_LANG_TABLE[1];
+#define MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION LANG_TABLE_SELECT_EXPLICIT(MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION_LANG_TABLE, 0)
 extern const char* const MSG_FIND_BED_OFFSET_AND_SKEW_LINE1_LANG_TABLE[LANG_NUM];
 #define MSG_FIND_BED_OFFSET_AND_SKEW_LINE1 LANG_TABLE_SELECT(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1_LANG_TABLE)
 extern const char* const MSG_FIND_BED_OFFSET_AND_SKEW_LINE2_LANG_TABLE[LANG_NUM];
 #define MSG_FIND_BED_OFFSET_AND_SKEW_LINE2 LANG_TABLE_SELECT(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2_LANG_TABLE)
+extern const char* const MSG_FINISHING_MOVEMENTS_LANG_TABLE[LANG_NUM];
+#define MSG_FINISHING_MOVEMENTS LANG_TABLE_SELECT(MSG_FINISHING_MOVEMENTS_LANG_TABLE)
 extern const char* const MSG_FLOW_LANG_TABLE[LANG_NUM];
 #define MSG_FLOW LANG_TABLE_SELECT(MSG_FLOW_LANG_TABLE)
 extern const char* const MSG_FLOW0_LANG_TABLE[1];
@@ -343,6 +351,16 @@ extern const char* const MSG_PAUSE_PRINT_LANG_TABLE[LANG_NUM];
 #define MSG_PAUSE_PRINT LANG_TABLE_SELECT(MSG_PAUSE_PRINT_LANG_TABLE)
 extern const char* const MSG_PICK_Z_LANG_TABLE[LANG_NUM];
 #define MSG_PICK_Z LANG_TABLE_SELECT(MSG_PICK_Z_LANG_TABLE)
+extern const char* const MSG_PID_EXTRUDER_LANG_TABLE[LANG_NUM];
+#define MSG_PID_EXTRUDER LANG_TABLE_SELECT(MSG_PID_EXTRUDER_LANG_TABLE)
+extern const char* const MSG_PID_FINISHED_LANG_TABLE[LANG_NUM];
+#define MSG_PID_FINISHED LANG_TABLE_SELECT(MSG_PID_FINISHED_LANG_TABLE)
+extern const char* const MSG_PID_RUNNING_LANG_TABLE[LANG_NUM];
+#define MSG_PID_RUNNING LANG_TABLE_SELECT(MSG_PID_RUNNING_LANG_TABLE)
+extern const char* const MSG_PINDA_NOT_CALIBRATED_LANG_TABLE[LANG_NUM];
+#define MSG_PINDA_NOT_CALIBRATED LANG_TABLE_SELECT(MSG_PINDA_NOT_CALIBRATED_LANG_TABLE)
+extern const char* const MSG_PINDA_PREHEAT_LANG_TABLE[LANG_NUM];
+#define MSG_PINDA_PREHEAT LANG_TABLE_SELECT(MSG_PINDA_PREHEAT_LANG_TABLE)
 extern const char* const MSG_PLANNER_BUFFER_BYTES_LANG_TABLE[1];
 #define MSG_PLANNER_BUFFER_BYTES LANG_TABLE_SELECT_EXPLICIT(MSG_PLANNER_BUFFER_BYTES_LANG_TABLE, 0)
 extern const char* const MSG_PLEASE_WAIT_LANG_TABLE[LANG_NUM];
@@ -361,6 +379,8 @@ extern const char* const MSG_PRINTER_DISCONNECTED_LANG_TABLE[1];
 #define MSG_PRINTER_DISCONNECTED LANG_TABLE_SELECT_EXPLICIT(MSG_PRINTER_DISCONNECTED_LANG_TABLE, 0)
 extern const char* const MSG_PRINT_ABORTED_LANG_TABLE[LANG_NUM];
 #define MSG_PRINT_ABORTED LANG_TABLE_SELECT(MSG_PRINT_ABORTED_LANG_TABLE)
+extern const char* const MSG_PRINT_PAUSED_LANG_TABLE[LANG_NUM];
+#define MSG_PRINT_PAUSED LANG_TABLE_SELECT(MSG_PRINT_PAUSED_LANG_TABLE)
 extern const char* const MSG_PRUSA3D_LANG_TABLE[LANG_NUM];
 #define MSG_PRUSA3D LANG_TABLE_SELECT(MSG_PRUSA3D_LANG_TABLE)
 extern const char* const MSG_PRUSA3D_FORUM_LANG_TABLE[LANG_NUM];
@@ -383,6 +403,8 @@ extern const char* const MSG_RESUME_PRINT_LANG_TABLE[LANG_NUM];
 #define MSG_RESUME_PRINT LANG_TABLE_SELECT(MSG_RESUME_PRINT_LANG_TABLE)
 extern const char* const MSG_RESUMING_LANG_TABLE[LANG_NUM];
 #define MSG_RESUMING LANG_TABLE_SELECT(MSG_RESUMING_LANG_TABLE)
+extern const char* const MSG_RESUMING_PRINT_LANG_TABLE[LANG_NUM];
+#define MSG_RESUMING_PRINT LANG_TABLE_SELECT(MSG_RESUMING_PRINT_LANG_TABLE)
 extern const char* const MSG_SD_CANT_ENTER_SUBDIR_LANG_TABLE[1];
 #define MSG_SD_CANT_ENTER_SUBDIR LANG_TABLE_SELECT_EXPLICIT(MSG_SD_CANT_ENTER_SUBDIR_LANG_TABLE, 0)
 extern const char* const MSG_SD_CANT_OPEN_SUBDIR_LANG_TABLE[1];
@@ -477,6 +499,8 @@ extern const char* const MSG_SET_HOME_OFFSETS_LANG_TABLE[1];
 #define MSG_SET_HOME_OFFSETS LANG_TABLE_SELECT_EXPLICIT(MSG_SET_HOME_OFFSETS_LANG_TABLE, 0)
 extern const char* const MSG_SET_ORIGIN_LANG_TABLE[1];
 #define MSG_SET_ORIGIN LANG_TABLE_SELECT_EXPLICIT(MSG_SET_ORIGIN_LANG_TABLE, 0)
+extern const char* const MSG_SET_TEMPERATURE_LANG_TABLE[LANG_NUM];
+#define MSG_SET_TEMPERATURE LANG_TABLE_SELECT(MSG_SET_TEMPERATURE_LANG_TABLE)
 extern const char* const MSG_SHOW_END_STOPS_LANG_TABLE[LANG_NUM];
 #define MSG_SHOW_END_STOPS LANG_TABLE_SELECT(MSG_SHOW_END_STOPS_LANG_TABLE)
 extern const char* const MSG_SILENT_MODE_OFF_LANG_TABLE[LANG_NUM];
@@ -517,6 +541,14 @@ extern const char* const MSG_TAKE_EFFECT_LANG_TABLE[LANG_NUM];
 #define MSG_TAKE_EFFECT LANG_TABLE_SELECT(MSG_TAKE_EFFECT_LANG_TABLE)
 extern const char* const MSG_TEMPERATURE_LANG_TABLE[LANG_NUM];
 #define MSG_TEMPERATURE LANG_TABLE_SELECT(MSG_TEMPERATURE_LANG_TABLE)
+extern const char* const MSG_TEMP_CALIBRATION_LANG_TABLE[LANG_NUM];
+#define MSG_TEMP_CALIBRATION LANG_TABLE_SELECT(MSG_TEMP_CALIBRATION_LANG_TABLE)
+extern const char* const MSG_TEMP_CALIBRATION_DONE_LANG_TABLE[LANG_NUM];
+#define MSG_TEMP_CALIBRATION_DONE LANG_TABLE_SELECT(MSG_TEMP_CALIBRATION_DONE_LANG_TABLE)
+extern const char* const MSG_TEMP_CALIBRATION_OFF_LANG_TABLE[LANG_NUM];
+#define MSG_TEMP_CALIBRATION_OFF LANG_TABLE_SELECT(MSG_TEMP_CALIBRATION_OFF_LANG_TABLE)
+extern const char* const MSG_TEMP_CALIBRATION_ON_LANG_TABLE[LANG_NUM];
+#define MSG_TEMP_CALIBRATION_ON LANG_TABLE_SELECT(MSG_TEMP_CALIBRATION_ON_LANG_TABLE)
 extern const char* const MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF_LANG_TABLE[LANG_NUM];
 #define MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF LANG_TABLE_SELECT(MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF_LANG_TABLE)
 extern const char* const MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_ON_LANG_TABLE[LANG_NUM];
diff --git a/Firmware/language_cz.h b/Firmware/language_cz.h
index 9dda46c22..b2267ad8c 100644
--- a/Firmware/language_cz.h
+++ b/Firmware/language_cz.h
@@ -264,4 +264,21 @@
 #define MSG_FILAMENT_CLEAN								"Je barva cista?" 
 #define MSG_UNLOADING_FILAMENT							"Vysouvam filament"
 
-#define MSG_PAPER										"Umistete list papiru na podlozku a udrzujte jej pod tryskou behem mereni prvnich 4 bodu. Pokud tryska zachyti papir, vypnete tiskarnu."
\ No newline at end of file
+#define MSG_PAPER										"Umistete list papiru na podlozku a udrzujte jej pod tryskou behem mereni prvnich 4 bodu. Pokud tryska zachyti papir, vypnete tiskarnu."
+
+#define MSG_FINISHING_MOVEMENTS							"Dokoncovani pohybu"
+#define MSG_PRINT_PAUSED								"Tisk pozastaven"
+#define MSG_RESUMING_PRINT								"Obnovovani tisku"
+#define MSG_PID_EXTRUDER								"PID kalibrace"
+#define MSG_SET_TEMPERATURE								"Nastavte teplotu:"
+#define MSG_PID_FINISHED								"PID kal. ukoncena"
+#define MSG_PID_RUNNING									"PID kal.           "
+
+#define MSG_CALIBRATE_PINDA								"Zkalibrovat"
+#define MSG_CALIBRATION_PINDA_MENU						"Teplotni kalibrace"
+#define MSG_PINDA_NOT_CALIBRATED						"Tiskarna nebyla teplotne zkalibrovana"
+#define MSG_PINDA_PREHEAT								"Predehrivani"
+#define MSG_TEMP_CALIBRATION							"Tepl. kal.          "
+#define MSG_TEMP_CALIBRATION_DONE						"Teplotni kalibrace dokoncena. Pokracujte stiskem tlacitka."
+#define MSG_TEMP_CALIBRATION_ON							"Tepl. kal. [ON]"
+#define MSG_TEMP_CALIBRATION_OFF						"Tepl. kal. [OFF]"
\ No newline at end of file
diff --git a/Firmware/language_en.h b/Firmware/language_en.h
index f5e881b72..8c5e71d8a 100644
--- a/Firmware/language_en.h
+++ b/Firmware/language_en.h
@@ -208,6 +208,7 @@
 #define(length=14) MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2	" of 9"
 #define(length=60) MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE1	"Measuring reference height of calibration point"
 #define(length=14) MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2	" of 9"
+#define(length=20) MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION	"Iteration "
 
 #define(length=20,lines=8) MSG_BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND			"XYZ calibration failed. Bed calibration point was not found."
 #define(length=20,lines=8) MSG_BED_SKEW_OFFSET_DETECTION_FITTING_FAILED				"XYZ calibration failed. Please consult the manual."
@@ -265,4 +266,21 @@
 #define MSG_MENU_CALIBRATION									"Calibration"
 #define MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF					"SD card [normal]"
 #define MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_ON					"SD card [FlshAir]"
-#define MSG_PRINTER_DISCONNECTED								"Printer disconnected"
\ No newline at end of file
+#define MSG_PRINTER_DISCONNECTED								"Printer disconnected"
+#define(length=20, lines=1) MSG_FINISHING_MOVEMENTS				"Finishing movements"
+#define(length=20, lines=1) MSG_PRINT_PAUSED					"Print paused"
+#define(length=20, lines=1) MSG_RESUMING_PRINT					"Resuming print"
+#define(length=17, lines=1) MSG_PID_EXTRUDER					"PID calibration"
+#define(length=19, lines=1) MSG_SET_TEMPERATURE					"Set temperature:"
+#define(length=20, lines=1) MSG_PID_FINISHED					"PID cal. finished"
+#define(length=20, lines=1) MSG_PID_RUNNING						"PID cal.           "
+
+#define(length=17, lines=1) MSG_CALIBRATE_PINDA					"Calibrate"
+#define(length=17, lines=1) MSG_CALIBRATION_PINDA_MENU			"Temp. calibration"
+#define(length=20, lines=4) MSG_PINDA_NOT_CALIBRATED			"Temperature calibration has not been run yet"
+#define(length=20, lines=1) MSG_PINDA_PREHEAT					"Preheating"
+#define(length=20, lines=1) MSG_TEMP_CALIBRATION				"Temp. cal.          "
+#define(length=20, lines=4) MSG_TEMP_CALIBRATION_DONE			"Temperature calibration is finished. Click to continue."
+#define(length=20, lines=1) MSG_TEMP_CALIBRATION_ON				"Temp. cal. [ON]"
+#define(length=20, lines=1) MSG_TEMP_CALIBRATION_OFF			"Temp. cal. [OFF]"
+
diff --git a/Firmware/language_it.h b/Firmware/language_it.h
index dc9e2a3a2..fcb40120d 100644
--- a/Firmware/language_it.h
+++ b/Firmware/language_it.h
@@ -248,4 +248,5 @@
 #define MSG_WAITING_TEMP								"In attesa del raffreddamento della testina e del piatto"
 #define MSG_FILAMENT_CLEAN								"Il colore e' nitido?"
 #define MSG_UNLOADING_FILAMENT							"Rilasc. filamento"
-#define MSG_PAPER										"Porre un foglio sotto l'ugello durante la calibrazione dei primi 4 punti. In caso l'ugello muova il foglio spegnere prontamente la stampante."
\ No newline at end of file
+#define MSG_PAPER										"Porre un foglio sotto l'ugello durante la calibrazione dei primi 4 punti. In caso l'ugello muova il foglio spegnere prontamente la stampante."
+
diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp
index 38eb41c69..1ebc12658 100644
--- a/Firmware/mesh_bed_calibration.cpp
+++ b/Firmware/mesh_bed_calibration.cpp
@@ -383,6 +383,10 @@ BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
             MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
             SERIAL_ECHOPGM("), error: ");
             MYSERIAL.print(err);
+			SERIAL_ECHOPGM(", error X: ");
+			MYSERIAL.print(errX);
+			SERIAL_ECHOPGM(", error Y: ");
+			MYSERIAL.print(errY);
             SERIAL_ECHOLNPGM("");
         }
     }
@@ -1586,11 +1590,51 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
     float *vec_y = vec_x + 2;
     float *cntr  = vec_y + 2;
     memset(pts, 0, sizeof(float) * 7 * 7);
+	uint8_t iteration = 0; 
+	BedSkewOffsetDetectionResultType result;
 
 //    SERIAL_ECHOLNPGM("find_bed_offset_and_skew verbosity level: ");
 //    SERIAL_ECHO(int(verbosity_level));
 //    SERIAL_ECHOPGM("");
 
+	while (iteration < 3) {
+
+		SERIAL_ECHOPGM("Iteration: ");
+		MYSERIAL.println(int(iteration + 1));
+
+		if (iteration > 0) {
+			// Cache the current correction matrix.
+			world2machine_initialize();
+			vec_x[0] = world2machine_rotation_and_skew[0][0];
+			vec_x[1] = world2machine_rotation_and_skew[1][0];
+			vec_y[0] = world2machine_rotation_and_skew[0][1];
+			vec_y[1] = world2machine_rotation_and_skew[1][1];
+			cntr[0] = world2machine_shift[0];
+			cntr[1] = world2machine_shift[1];
+			if (verbosity_level >= 20) {
+				SERIAL_ECHOPGM("vec_x[0]:");
+				MYSERIAL.print(vec_x[0], 5);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOPGM("vec_x[1]:");
+				MYSERIAL.print(vec_x[1], 5);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOPGM("vec_y[0]:");
+				MYSERIAL.print(vec_y[0], 5);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOPGM("vec_y[1]:");
+				MYSERIAL.print(vec_y[1], 5);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOPGM("cntr[0]:");
+				MYSERIAL.print(cntr[0], 5);
+				SERIAL_ECHOLNPGM("");
+				SERIAL_ECHOPGM("cntr[1]:");
+				MYSERIAL.print(cntr[1], 5);
+				SERIAL_ECHOLNPGM("");
+			}
+			// and reset the correction matrix, so the planner will not do anything.
+			world2machine_reset();
+		}
+
 #ifdef MESH_BED_CALIBRATION_SHOW_LCD
     uint8_t next_line;
     lcd_display_message_fullscreen_P(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1, next_line);
@@ -1600,144 +1644,192 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
 
     // Collect the rear 2x3 points.
     current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-    for (int k = 0; k < 4; ++ k) {
-        // Don't let the manage_inactivity() function remove power from the motors.
-        refresh_cmd_timeout();
+	for (int k = 0; k < 4; ++k) {
+		// Don't let the manage_inactivity() function remove power from the motors.
+		refresh_cmd_timeout();
 #ifdef MESH_BED_CALIBRATION_SHOW_LCD
-        lcd_implementation_print_at(0, next_line, k+1);
-        lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2);
-#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
-        float *pt = pts + k * 2;
-        // Go up to z_initial.
-        go_to_current(homing_feedrate[Z_AXIS] / 60.f);
-        if (verbosity_level >= 20) {
-            // Go to Y0, wait, then go to Y-4.
-            current_position[Y_AXIS] = 0.f;
-            go_to_current(homing_feedrate[X_AXIS] / 60.f);
-            SERIAL_ECHOLNPGM("At Y0");
-            delay_keep_alive(5000);
-            current_position[Y_AXIS] = Y_MIN_POS;
-            go_to_current(homing_feedrate[X_AXIS] / 60.f);
-            SERIAL_ECHOLNPGM("At Y-4");
-            delay_keep_alive(5000);
-        }
-        // Go to the measurement point position.
-        current_position[X_AXIS] = pgm_read_float(bed_ref_points_4+k*2);
-        current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4+k*2+1);
-        go_to_current(homing_feedrate[X_AXIS] / 60.f);
-        if (verbosity_level >= 10)
-            delay_keep_alive(3000);
-        if (! find_bed_induction_sensor_point_xy())
-            return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
-#if 1
-        if (k == 0) {
-            // Improve the position of the 1st row sensor points by a zig-zag movement.
-            find_bed_induction_sensor_point_z();
-            int8_t i = 4;
-            for (;;) {
-                if (improve_bed_induction_sensor_point3(verbosity_level))
-                    break;
-                if (-- i == 0)
-                    return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
-                // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
-                current_position[Z_AXIS] -= 0.025f;
-                enable_endstops(false);
-                enable_z_endstop(false);
-                go_to_current(homing_feedrate[Z_AXIS]);
-            }
-            if (i == 0)
-                // not found
-                return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
-        }
-#endif
-        if (verbosity_level >= 10)
-            delay_keep_alive(3000);
-        // Save the detected point position and then clamp the Y coordinate, which may have been estimated
-        // to lie outside the machine working space.
-        pt[0] = current_position[X_AXIS];
-        pt[1] = current_position[Y_AXIS];
-        if (current_position[Y_AXIS] < Y_MIN_POS)
-            current_position[Y_AXIS] = Y_MIN_POS;
-        // Start searching for the other points at 3mm above the last point.
-        current_position[Z_AXIS] += 3.f;
-        cntr[0] += pt[0];
-        cntr[1] += pt[1];
-        if (verbosity_level >= 10 && k == 0) {
-            // Show the zero. Test, whether the Y motor skipped steps.
-            current_position[Y_AXIS] = MANUAL_Y_HOME_POS;
-            go_to_current(homing_feedrate[X_AXIS] / 60.f);
-            delay_keep_alive(3000);
-        }
-    }
+		lcd_implementation_print_at(0, next_line, k + 1);
+		lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2);
 
-    if (verbosity_level >= 20) {
-        // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
-        delay_keep_alive(3000);
-        for (int8_t mesh_point = 0; mesh_point < 4; ++ mesh_point) {
-            // Don't let the manage_inactivity() function remove power from the motors.
-            refresh_cmd_timeout();
-            // Go to the measurement point.
-            // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-            current_position[X_AXIS] = pts[mesh_point*2];
-            current_position[Y_AXIS] = pts[mesh_point*2+1];
-            go_to_current(homing_feedrate[X_AXIS]/60);
-            delay_keep_alive(3000);
-        }
-    }
-
-    BedSkewOffsetDetectionResultType result = calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
-    if (result >= 0) {
-        world2machine_update(vec_x, vec_y, cntr);
-    #if 1
-        // Fearlessly store the calibration values into the eeprom.
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
-    #endif
-		if (verbosity_level >= 10) {
-			// Length of the vec_x
-			float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
-			SERIAL_ECHOLNPGM("X vector length:");
-			MYSERIAL.println(l);
-
-			// Length of the vec_y
-			l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
-			SERIAL_ECHOLNPGM("Y vector length:");
-			MYSERIAL.println(l);
-			// Zero point correction
-			l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
-			SERIAL_ECHOLNPGM("Zero point correction:");
-			MYSERIAL.println(l);
-
-			// vec_x and vec_y shall be nearly perpendicular.
-			l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
-			SERIAL_ECHOLNPGM("Perpendicularity");
-			MYSERIAL.println(fabs(l));
-			SERIAL_ECHOLNPGM("Saving bed calibration vectors to EEPROM");
+		if (iteration > 0) {
+			lcd_print_at_PGM(0, next_line + 1, MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION);
+			lcd_implementation_print(int(iteration + 1));
 		}
-        // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
-        world2machine_update_current();
+#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
+		float *pt = pts + k * 2;
+		// Go up to z_initial.
 
-        if (verbosity_level >= 20) {
-            // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
-            delay_keep_alive(3000);
-            for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
-                // Don't let the manage_inactivity() function remove power from the motors.
-                refresh_cmd_timeout();
-                // Go to the measurement point.
-                // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-                current_position[X_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2);
-                current_position[Y_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2+1);
-                go_to_current(homing_feedrate[X_AXIS]/60);
-                delay_keep_alive(3000);
-            }
-        }
-    }
+		go_to_current(homing_feedrate[Z_AXIS] / 60.f);
+		if (verbosity_level >= 20) {
+			// Go to Y0, wait, then go to Y-4.
+			current_position[Y_AXIS] = 0.f;
+			go_to_current(homing_feedrate[X_AXIS] / 60.f);
+			SERIAL_ECHOLNPGM("At Y0");
+			delay_keep_alive(5000);
+			current_position[Y_AXIS] = Y_MIN_POS;
+			go_to_current(homing_feedrate[X_AXIS] / 60.f);
+			SERIAL_ECHOLNPGM("At Y-4");
+			delay_keep_alive(5000);
+		}
+		// Go to the measurement point position.
+		if (iteration == 0) {
+			current_position[X_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2);
+			current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2 + 1);
+		}
+		else {
+			// if first iteration failed, count corrected point coordinates as initial
+			// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+			
+			current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[0] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[0];
+			current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[1] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[1];
 
-    return result;
+			// The calibration points are very close to the min Y.
+			if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
+				current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
+
+		}
+		if (verbosity_level >= 20) {
+			SERIAL_ECHOPGM("corrected current_position[X_AXIS]:");
+			MYSERIAL.print(current_position[X_AXIS], 5);
+			SERIAL_ECHOLNPGM("");
+			SERIAL_ECHOPGM("corrected current_position[Y_AXIS]:");
+			MYSERIAL.print(current_position[Y_AXIS], 5);
+			SERIAL_ECHOLNPGM("");
+		}
+
+
+		go_to_current(homing_feedrate[X_AXIS] / 60.f);
+		if (verbosity_level >= 10)
+			delay_keep_alive(3000);
+		if (!find_bed_induction_sensor_point_xy())
+			return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
+#if 1
+		
+			if (k == 0) {
+				// Improve the position of the 1st row sensor points by a zig-zag movement.
+				find_bed_induction_sensor_point_z();
+				int8_t i = 4;
+				for (;;) {
+					if (improve_bed_induction_sensor_point3(verbosity_level))
+						break;
+					if (--i == 0)
+						return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
+					// Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
+					current_position[Z_AXIS] -= 0.025f;
+					enable_endstops(false);
+					enable_z_endstop(false);
+					go_to_current(homing_feedrate[Z_AXIS]);
+				}
+				if (i == 0)
+					// not found
+					return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
+			}
+#endif
+			if (verbosity_level >= 10)
+				delay_keep_alive(3000);
+			// Save the detected point position and then clamp the Y coordinate, which may have been estimated
+			// to lie outside the machine working space.
+			if (verbosity_level >= 20) {
+				SERIAL_ECHOLNPGM("Measured:");
+				MYSERIAL.println(current_position[X_AXIS]);
+				MYSERIAL.println(current_position[Y_AXIS]);
+			}
+			//pt[0] = (pt[0] * iteration) / (iteration + 1);
+			//pt[0] += (current_position[X_AXIS]/(iteration + 1)); //count average
+			//pt[1] = (pt[1] * iteration) / (iteration + 1);
+			//pt[1] += (current_position[Y_AXIS] / (iteration + 1));
+			
+			
+			pt[0] += current_position[X_AXIS];
+			if(iteration > 0) pt[0] = pt[0] / 2;
+			
+			pt[1] += current_position[Y_AXIS];
+			if (iteration > 0) pt[1] = pt[1] / 2;
+
+			if (current_position[Y_AXIS] < Y_MIN_POS)
+				current_position[Y_AXIS] = Y_MIN_POS;
+			// Start searching for the other points at 3mm above the last point.
+			current_position[Z_AXIS] += 3.f;
+			//cntr[0] += pt[0];
+			//cntr[1] += pt[1];
+			if (verbosity_level >= 10 && k == 0) {
+				// Show the zero. Test, whether the Y motor skipped steps.
+				current_position[Y_AXIS] = MANUAL_Y_HOME_POS;
+				go_to_current(homing_feedrate[X_AXIS] / 60.f);
+				delay_keep_alive(3000);
+			}
+		}
+
+		if (verbosity_level >= 20) {
+			// Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
+			delay_keep_alive(3000);
+			for (int8_t mesh_point = 0; mesh_point < 4; ++mesh_point) {
+				// Don't let the manage_inactivity() function remove power from the motors.
+				refresh_cmd_timeout();
+				// Go to the measurement point.
+				// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+				current_position[X_AXIS] = pts[mesh_point * 2];
+				current_position[Y_AXIS] = pts[mesh_point * 2 + 1];
+				go_to_current(homing_feedrate[X_AXIS] / 60);
+				delay_keep_alive(3000);
+			}
+		}
+
+		result = calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
+		if (result >= 0) {
+			world2machine_update(vec_x, vec_y, cntr);
+#if 1
+			// Fearlessly store the calibration values into the eeprom.
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 0), cntr[0]);
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 4), cntr[1]);
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 0), vec_x[0]);
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 4), vec_x[1]);
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 0), vec_y[0]);
+			eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 4), vec_y[1]);
+#endif
+			if (verbosity_level >= 10) {
+				// Length of the vec_x
+				float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
+				SERIAL_ECHOLNPGM("X vector length:");
+				MYSERIAL.println(l);
+
+				// Length of the vec_y
+				l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
+				SERIAL_ECHOLNPGM("Y vector length:");
+				MYSERIAL.println(l);
+				// Zero point correction
+				l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
+				SERIAL_ECHOLNPGM("Zero point correction:");
+				MYSERIAL.println(l);
+
+				// vec_x and vec_y shall be nearly perpendicular.
+				l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
+				SERIAL_ECHOLNPGM("Perpendicularity");
+				MYSERIAL.println(fabs(l));
+				SERIAL_ECHOLNPGM("Saving bed calibration vectors to EEPROM");
+			}
+			// Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
+			world2machine_update_current();
+
+			if (verbosity_level >= 20) {
+				// Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
+				delay_keep_alive(3000);
+				for (int8_t mesh_point = 0; mesh_point < 9; ++mesh_point) {
+					// Don't let the manage_inactivity() function remove power from the motors.
+					refresh_cmd_timeout();
+					// Go to the measurement point.
+					// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+					current_position[X_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2);
+					current_position[Y_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2 + 1);
+					go_to_current(homing_feedrate[X_AXIS] / 60);
+					delay_keep_alive(3000);
+				}
+			}
+			return result;
+		}
+		iteration++;
+	}
+	return result;    
 }
 
 BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask)
@@ -2217,8 +2309,8 @@ static int babystepLoadZ = 0;
 void babystep_apply()
 {
     // Apply Z height correction aka baby stepping before mesh bed leveling gets activated.
-    if(calibration_status() == CALIBRATION_STATUS_CALIBRATED)
-    {
+    if(calibration_status() <= CALIBRATION_STATUS_PINDA)
+	{
 		check_babystep(); //checking if babystep is in allowed range, otherwise setting babystep to 0
 		
 		// End of G80: Apply the baby stepping value.
diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp
index d2f6e3f88..754262192 100644
--- a/Firmware/temperature.cpp
+++ b/Firmware/temperature.cpp
@@ -51,7 +51,12 @@ float current_temperature_bed = 0.0;
   int redundant_temperature_raw = 0;
   float redundant_temperature = 0.0;
 #endif
+  
+
 #ifdef PIDTEMP
+  float _Kp, _Ki, _Kd;
+  int pid_cycle, pid_number_of_cycles;
+  bool pid_tuning_finished = false;
   float Kp=DEFAULT_Kp;
   float Ki=(DEFAULT_Ki*PID_dT);
   float Kd=(DEFAULT_Kd/PID_dT);
@@ -181,10 +186,12 @@ unsigned long watchmillis[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0);
 //=============================   functions      ============================
 //===========================================================================
 
-void PID_autotune(float temp, int extruder, int ncycles)
-{
+  void PID_autotune(float temp, int extruder, int ncycles)
+  {
+  pid_number_of_cycles = ncycles;
+  pid_tuning_finished = false;
   float input = 0.0;
-  int cycles=0;
+  pid_cycle=0;
   bool heating = true;
 
   unsigned long temp_millis = millis();
@@ -195,7 +202,6 @@ void PID_autotune(float temp, int extruder, int ncycles)
 
   long bias, d;
   float Ku, Tu;
-  float Kp, Ki, Kd;
   float max = 0, min = 10000;
 
 #if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1) || \
@@ -210,6 +216,8 @@ void PID_autotune(float temp, int extruder, int ncycles)
   #endif
        ){
           SERIAL_ECHOLN("PID Autotune failed. Bad extruder number.");
+		  pid_tuning_finished = true;
+		  pid_cycle = 0;
           return;
         }
 	
@@ -267,7 +275,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
           heating=true;
           t2=millis();
           t_low=t2 - t1;
-          if(cycles > 0) {
+          if(pid_cycle > 0) {
             bias += (d*(t_high - t_low))/(t_low + t_high);
             bias = constrain(bias, 20 ,(extruder<0?(MAX_BED_POWER):(PID_MAX))-20);
             if(bias > (extruder<0?(MAX_BED_POWER):(PID_MAX))/2) d = (extruder<0?(MAX_BED_POWER):(PID_MAX)) - 1 - bias;
@@ -277,33 +285,33 @@ void PID_autotune(float temp, int extruder, int ncycles)
             SERIAL_PROTOCOLPGM(" d: "); SERIAL_PROTOCOL(d);
             SERIAL_PROTOCOLPGM(" min: "); SERIAL_PROTOCOL(min);
             SERIAL_PROTOCOLPGM(" max: "); SERIAL_PROTOCOLLN(max);
-            if(cycles > 2) {
+            if(pid_cycle > 2) {
               Ku = (4.0*d)/(3.14159*(max-min)/2.0);
               Tu = ((float)(t_low + t_high)/1000.0);
               SERIAL_PROTOCOLPGM(" Ku: "); SERIAL_PROTOCOL(Ku);
               SERIAL_PROTOCOLPGM(" Tu: "); SERIAL_PROTOCOLLN(Tu);
-              Kp = 0.6*Ku;
-              Ki = 2*Kp/Tu;
-              Kd = Kp*Tu/8;
+              _Kp = 0.6*Ku;
+              _Ki = 2*_Kp/Tu;
+              _Kd = _Kp*Tu/8;
               SERIAL_PROTOCOLLNPGM(" Classic PID ");
-              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(Kp);
-              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(Ki);
-              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(Kd);
+              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(_Kp);
+              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(_Ki);
+              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(_Kd);
               /*
-              Kp = 0.33*Ku;
-              Ki = Kp/Tu;
-              Kd = Kp*Tu/3;
+              _Kp = 0.33*Ku;
+              _Ki = _Kp/Tu;
+              _Kd = _Kp*Tu/3;
               SERIAL_PROTOCOLLNPGM(" Some overshoot ");
-              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(Kp);
-              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(Ki);
-              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(Kd);
-              Kp = 0.2*Ku;
-              Ki = 2*Kp/Tu;
-              Kd = Kp*Tu/3;
+              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(_Kp);
+              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(_Ki);
+              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(_Kd);
+              _Kp = 0.2*Ku;
+              _Ki = 2*_Kp/Tu;
+              _Kd = _Kp*Tu/3;
               SERIAL_PROTOCOLLNPGM(" No overshoot ");
-              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(Kp);
-              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(Ki);
-              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(Kd);
+              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(_Kp);
+              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(_Ki);
+              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(_Kd);
               */
             }
           }
@@ -311,13 +319,15 @@ void PID_autotune(float temp, int extruder, int ncycles)
             soft_pwm_bed = (bias + d) >> 1;
           else
             soft_pwm[extruder] = (bias + d) >> 1;
-          cycles++;
+          pid_cycle++;
           min=temp;
         }
       } 
     }
     if(input > (temp + 20)) {
       SERIAL_PROTOCOLLNPGM("PID Autotune failed! Temperature too high");
+	  pid_tuning_finished = true;
+	  pid_cycle = 0;
       return;
     }
     if(millis() - temp_millis > 2000) {
@@ -338,10 +348,14 @@ void PID_autotune(float temp, int extruder, int ncycles)
     }
     if(((millis() - t1) + (millis() - t2)) > (10L*60L*1000L*2L)) {
       SERIAL_PROTOCOLLNPGM("PID Autotune failed! timeout");
+	  pid_tuning_finished = true;
+	  pid_cycle = 0;
       return;
     }
-    if(cycles > ncycles) {
+    if(pid_cycle > ncycles) {
       SERIAL_PROTOCOLLNPGM("PID Autotune finished! Put the last Kp, Ki and Kd constants from above into Configuration.h");
+	  pid_tuning_finished = true;
+	  pid_cycle = 0;
       return;
     }
     lcd_update();
@@ -1056,11 +1070,10 @@ void temp_runaway_check(int _heater_id, float _target_temperature, float _curren
 	float __hysteresis = 0;
 	int __timeout = 0;
 	bool temp_runaway_check_active = false;
-	static float __preheat_start = 0;
-	static int __preheat_counter = 0;
-	static int __preheat_errors = 0;
-
-	_heater_id = (_isbed) ? _heater_id++ : _heater_id;
+	static float __preheat_start[2] = { 0,0}; //currently just bed and one extruder
+	static int __preheat_counter[2] = { 0,0};
+	static int __preheat_errors[2] = { 0,0};
+		
 
 #ifdef 	TEMP_RUNAWAY_BED_TIMEOUT
 	if (_isbed)
@@ -1093,8 +1106,8 @@ void temp_runaway_check(int _heater_id, float _target_temperature, float _curren
 			{
 				temp_runaway_status[_heater_id] = TempRunaway_PREHEAT;
 				temp_runaway_target[_heater_id] = _target_temperature;
-				__preheat_start = _current_temperature;
-				__preheat_counter = 0;
+				__preheat_start[_heater_id] = _current_temperature;
+				__preheat_counter[_heater_id] = 0;
 			}
 			else
 			{
@@ -1105,26 +1118,36 @@ void temp_runaway_check(int _heater_id, float _target_temperature, float _curren
 
 		if (temp_runaway_status[_heater_id] == TempRunaway_PREHEAT)
 		{
-			if (_current_temperature < 150)
+			if (_current_temperature < ((_isbed) ? (0.8 * _target_temperature) : 150)) //check only in area where temperature is changing fastly for heater, check to 0.8 x target temperature for bed
 			{
-				__preheat_counter++;
-				if (__preheat_counter > 8)
+				__preheat_counter[_heater_id]++;
+				if (__preheat_counter[_heater_id] > ((_isbed) ? 16 : 8)) // periodicaly check if current temperature changes
 				{
-					if (_current_temperature - __preheat_start < 2) {
-						__preheat_errors++;
+					/*SERIAL_ECHOPGM("Heater:");
+					MYSERIAL.print(_heater_id);
+					SERIAL_ECHOPGM(" T:");
+					MYSERIAL.print(_current_temperature);
+					SERIAL_ECHOPGM(" Tstart:");
+					MYSERIAL.print(__preheat_start[_heater_id]);*/
+					
+					if (_current_temperature - __preheat_start[_heater_id] < 2) {
+						__preheat_errors[_heater_id]++;
+						/*SERIAL_ECHOPGM(" Preheat errors:");
+						MYSERIAL.println(__preheat_errors[_heater_id]);*/
 					}
 					else {
-						__preheat_errors = 0;
+						//SERIAL_ECHOLNPGM("");
+						__preheat_errors[_heater_id] = 0;
 					}
 
-					if (__preheat_errors > 5)
+					if (__preheat_errors[_heater_id] > ((_isbed) ? 2 : 5)) 
 					{
 						if (farm_mode) { prusa_statistics(0); }
-						temp_runaway_stop(true);
+						temp_runaway_stop(true, _isbed);
 						if (farm_mode) { prusa_statistics(91); }
 					}
-					__preheat_start = _current_temperature;
-					__preheat_counter = 0;
+					__preheat_start[_heater_id] = _current_temperature;
+					__preheat_counter[_heater_id] = 0;
 				}
 			}
 		}
@@ -1142,7 +1165,7 @@ void temp_runaway_check(int _heater_id, float _target_temperature, float _curren
 
 
 		if (temp_runaway_check_active)
-		{
+		{			
 			//	we are in range
 			if (_target_temperature - __hysteresis < _current_temperature && _current_temperature < _target_temperature + __hysteresis)
 			{
@@ -1157,7 +1180,7 @@ void temp_runaway_check(int _heater_id, float _target_temperature, float _curren
 					if (temp_runaway_error_counter[_heater_id] * 2 > __timeout)
 					{
 						if (farm_mode) { prusa_statistics(0); }
-						temp_runaway_stop(false);
+						temp_runaway_stop(false, _isbed);
 						if (farm_mode) { prusa_statistics(90); }
 					}
 				}
@@ -1167,7 +1190,7 @@ void temp_runaway_check(int _heater_id, float _target_temperature, float _curren
 	}
 }
 
-void temp_runaway_stop(bool isPreheat)
+void temp_runaway_stop(bool isPreheat, bool isBed)
 {
 	cancel_heatup = true;
 	quickStop();
@@ -1193,9 +1216,9 @@ void temp_runaway_stop(bool isPreheat)
 	if (isPreheat)
 	{
 		Stop();
-		LCD_ALERTMESSAGEPGM("   PREHEAT ERROR");
+		isBed ? LCD_ALERTMESSAGEPGM("BED PREHEAT ERROR") : LCD_ALERTMESSAGEPGM("PREHEAT ERROR");
 		SERIAL_ERROR_START;
-		SERIAL_ERRORLNPGM(": THERMAL RUNAWAY ( PREHEAT )");
+		isBed ? SERIAL_ERRORLNPGM(" THERMAL RUNAWAY ( PREHEAT HEATBED)") : SERIAL_ERRORLNPGM(" THERMAL RUNAWAY ( PREHEAT HOTEND)");
 		SET_OUTPUT(EXTRUDER_0_AUTO_FAN_PIN);
 		SET_OUTPUT(FAN_PIN);
 		WRITE(EXTRUDER_0_AUTO_FAN_PIN, 1);
@@ -1205,9 +1228,9 @@ void temp_runaway_stop(bool isPreheat)
 	}
 	else
 	{
-		LCD_ALERTMESSAGEPGM("THERMAL RUNAWAY");
+		isBed ? LCD_ALERTMESSAGEPGM("BED THERMAL RUNAWAY") : LCD_ALERTMESSAGEPGM("THERMAL RUNAWAY");
 		SERIAL_ERROR_START;
-		SERIAL_ERRORLNPGM(": THERMAL RUNAWAY");
+		isBed ? SERIAL_ERRORLNPGM(" HEATBED THERMAL RUNAWAY") : SERIAL_ERRORLNPGM(" HOTEND THERMAL RUNAWAY");
 	}
 }
 #endif
diff --git a/Firmware/temperature.h b/Firmware/temperature.h
index 4a2e8bf96..9e2a03cd7 100644
--- a/Firmware/temperature.h
+++ b/Firmware/temperature.h
@@ -58,7 +58,9 @@ extern float current_temperature_bed;
 #endif
 
 #ifdef PIDTEMP
-  extern float Kp,Ki,Kd,Kc;
+  extern int pid_cycle, pid_number_of_cycles;
+  extern float Kp,Ki,Kd,Kc,_Kp,_Ki,_Kd;
+  extern bool pid_tuning_finished;
   float scalePID_i(float i);
   float scalePID_d(float d);
   float unscalePID_i(float i);
@@ -180,7 +182,7 @@ static float temp_runaway_timer[4];
 static int temp_runaway_error_counter[4];
 
 void temp_runaway_check(int _heater_id, float _target_temperature, float _current_temperature, float _output, bool _isbed);
-void temp_runaway_stop(bool isPreheat);
+void temp_runaway_stop(bool isPreheat, bool isBed);
 #endif
 
 int getHeaterPower(int heater);
diff --git a/Firmware/thermistortables.h b/Firmware/thermistortables.h
index ac1dea736..faa7e5667 100644
--- a/Firmware/thermistortables.h
+++ b/Firmware/thermistortables.h
@@ -1033,7 +1033,9 @@ const short temptable_12[][2] PROGMEM = {
 
 #define PtA 3.9083E-3
 #define PtB -5.775E-7
+#define PtC -4.183E-12
 #define PtRt(T,R0) ((R0)*(1.0+(PtA)*(T)+(PtB)*(T)*(T)))
+#define PtRtNew(T,R0) ((R0)*(1.0+(PtA)*(T)+(PtB)*(T)*(T) + (T-100)*PtC*(T)*(T)*(T)))
 #define PtAdVal(T,R0,Rup) (short)(1024/(Rup/PtRt(T,R0)+1))
 #define PtLine(T,R0,Rup) { PtAdVal(T,R0,Rup)*OVERSAMPLENR, T },
 
@@ -1061,6 +1063,124 @@ const short temptable_147[][2] PROGMEM = {
   PtLine(300,100,4700)
 };
 #endif
+// E3D Pt100 with 4k7 MiniRambo pullup, no Amp on the MiniRambo v1.3a
+#if (THERMISTORHEATER_0 == 148) || (THERMISTORHEATER_1 == 148) || (THERMISTORHEATER_2 == 148) || (THERMISTORBED == 148)
+const short temptable_148[][2] PROGMEM = {
+// These values have been calculated and tested over many days.  See https://docs.google.com/spreadsheets/d/1MJXa6feEe0mGVCT2TrBwLxVOMoLDkJlvfQ4JXhAdV_E
+// Values that are missing from the 5C gap are missing due to resolution limits.
+{19.00000 * OVERSAMPLENR,  0},
+{19.25000 * OVERSAMPLENR,  5},
+{19.50000 * OVERSAMPLENR, 10},
+{19.87500 * OVERSAMPLENR, 15},
+{20.25000 * OVERSAMPLENR, 20},
+{21.00000 * OVERSAMPLENR, 25},
+{21.75000 * OVERSAMPLENR, 35},
+{22.00000 * OVERSAMPLENR, 40},
+{23.00000 * OVERSAMPLENR, 50},  // 55C is more commonly used.
+{23.75000 * OVERSAMPLENR, 60},
+{24.00000 * OVERSAMPLENR, 65},
+{24.06250 * OVERSAMPLENR, 70},
+{25.00000 * OVERSAMPLENR, 75},
+{25.50000 * OVERSAMPLENR, 85},
+{26.00000 * OVERSAMPLENR, 90},
+{26.93750 * OVERSAMPLENR,100},
+{27.00000 * OVERSAMPLENR,105},
+{27.37500 * OVERSAMPLENR,110},
+{28.00000 * OVERSAMPLENR,115},
+{29.00000 * OVERSAMPLENR,125},
+{29.25000 * OVERSAMPLENR,135},
+{30.00000 * OVERSAMPLENR,140},
+{35.50000 * OVERSAMPLENR,150},
+{31.00000 * OVERSAMPLENR,155},
+{32.00000 * OVERSAMPLENR,165},
+{32.18750 * OVERSAMPLENR,175},
+{33.00000 * OVERSAMPLENR,180},
+{33.62500 * OVERSAMPLENR,190},
+{34.00000 * OVERSAMPLENR,195},
+{35.00000 * OVERSAMPLENR,205},
+{35.50000 * OVERSAMPLENR,215},
+{36.00000 * OVERSAMPLENR,220},
+{36.75000 * OVERSAMPLENR,230},
+{37.00000 * OVERSAMPLENR,235},
+{37.75000 * OVERSAMPLENR,245},
+{38.00000 * OVERSAMPLENR,250},
+{38.12500 * OVERSAMPLENR,255},
+{39.00000 * OVERSAMPLENR,260},
+{40.00000 * OVERSAMPLENR,275},
+{40.25000 * OVERSAMPLENR,285},
+{41.00000 * OVERSAMPLENR,290},
+{41.25000 * OVERSAMPLENR,300},
+{42.00000 * OVERSAMPLENR,305},
+{43.00000 * OVERSAMPLENR,315},
+{43.25000 * OVERSAMPLENR,325},
+{44.00000 * OVERSAMPLENR,330},
+{44.18750 * OVERSAMPLENR,340},
+{45.00000 * OVERSAMPLENR,345},
+{45.25000 * OVERSAMPLENR,355},
+{46.00000 * OVERSAMPLENR,360},
+{46.62500 * OVERSAMPLENR,370},
+{47.00000 * OVERSAMPLENR,375},
+{47.25000 * OVERSAMPLENR,385},
+{48.00000 * OVERSAMPLENR,390},
+{48.75000 * OVERSAMPLENR,400},
+{49.00000 * OVERSAMPLENR,405},
+};
+#endif
+#if (THERMISTORHEATER_0 == 247) || (THERMISTORHEATER_1 == 247) || (THERMISTORHEATER_2 == 247) || (THERMISTORBED == 247) // Pt100 with 4k7 MiniRambo pullup & PT100 Amplifier
+const short temptable_247[][2] PROGMEM = {
+// Calculated from Bob-the-Kuhn's PT100 calculator listed in https://github.com/MarlinFirmware/Marlin/issues/5543
+// and the table provided by E3D at http://wiki.e3d-online.com/wiki/E3D_PT100_Amplifier_Documentation#Output_Characteristics.
+{  0 * OVERSAMPLENR,    0},
+{241 * OVERSAMPLENR,    1},
+{249 * OVERSAMPLENR,   10},
+{259 * OVERSAMPLENR,   20},
+{267 * OVERSAMPLENR,   30},
+{275 * OVERSAMPLENR,   40},
+{283 * OVERSAMPLENR,   50},
+{291 * OVERSAMPLENR,   60},
+{299 * OVERSAMPLENR,   70},
+{307 * OVERSAMPLENR,   80},
+{315 * OVERSAMPLENR,   90},
+{323 * OVERSAMPLENR,  100},
+{331 * OVERSAMPLENR,  110},
+{340 * OVERSAMPLENR,  120},
+{348 * OVERSAMPLENR,  130},
+{354 * OVERSAMPLENR,  140},
+{362 * OVERSAMPLENR,  150},
+{370 * OVERSAMPLENR,  160},
+{378 * OVERSAMPLENR,  170},
+{386 * OVERSAMPLENR,  180},
+{394 * OVERSAMPLENR,  190},
+{402 * OVERSAMPLENR,  200},
+{410 * OVERSAMPLENR,  210},
+{418 * OVERSAMPLENR,  220},
+{426 * OVERSAMPLENR,  230},
+{432 * OVERSAMPLENR,  240},
+{440 * OVERSAMPLENR,  250},
+{448 * OVERSAMPLENR,  260},
+{454 * OVERSAMPLENR,  270},
+{462 * OVERSAMPLENR,  280},
+{469 * OVERSAMPLENR,  290},
+{475 * OVERSAMPLENR,  300},
+{483 * OVERSAMPLENR,  310},
+{491 * OVERSAMPLENR,  320},
+{499 * OVERSAMPLENR,  330},
+{505 * OVERSAMPLENR,  340},
+{513 * OVERSAMPLENR,  350},
+{519 * OVERSAMPLENR,  360},
+{527 * OVERSAMPLENR,  370},
+{533 * OVERSAMPLENR,  380},
+{541 * OVERSAMPLENR,  390},
+{549 * OVERSAMPLENR,  400},
+{616 * OVERSAMPLENR,  500},
+{682 * OVERSAMPLENR,  600},
+{741 * OVERSAMPLENR,  700},
+{801 * OVERSAMPLENR,  800},
+{856 * OVERSAMPLENR,  900},
+{910 * OVERSAMPLENR, 1000},
+{960 * OVERSAMPLENR, 1100},
+};
+#endif
 #if (THERMISTORHEATER_0 == 1010) || (THERMISTORHEATER_1 == 1010) || (THERMISTORHEATER_2 == 1010) || (THERMISTORBED == 1010) // Pt1000 with 1k0 pullup
 const short temptable_1010[][2] PROGMEM = {
   PtLine(0,1000,1000)
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index aa22fe265..6314a75ab 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -106,6 +106,13 @@ int farm_status = 0;
 unsigned long allert_timer = millis();
 bool printer_connected = true;
 
+unsigned long display_time; //just timer for showing pid finished message on lcd;
+float pid_temp = DEFAULT_PID_TEMP;
+
+bool long_press_active = false;
+long long_press_timer = millis();
+long button_blanking_time = millis();
+bool button_pressed = false;
 
 bool long_press_active = false;
 long long_press_timer = millis();
@@ -509,11 +516,88 @@ static void lcd_status_screen()
 #ifdef ULTIPANEL
 
 void lcd_commands()
-{
+{	
+	char cmd1[25];
+	if (lcd_commands_type == LCD_COMMAND_LONG_PAUSE)
+	{
+		if(lcd_commands_step == 0) { 
+			card.pauseSDPrint();
+			lcd_setstatuspgm(MSG_FINISHING_MOVEMENTS);
+
+			lcdDrawUpdate = 3;
+			lcd_commands_step = 1;
+		}
+		if (lcd_commands_step == 1 && !blocks_queued()) {
+			lcd_setstatuspgm(MSG_PRINT_PAUSED);
+			isPrintPaused = true;
+			long_pause();
+			lcd_commands_type = 0;
+			lcd_commands_step = 0;
+		}
+
+	}
+
+	if (lcd_commands_type == LCD_COMMAND_LONG_PAUSE_RESUME) {
+		char cmd1[30];
+		if (lcd_commands_step == 0) {
+
+			lcdDrawUpdate = 3;
+			lcd_commands_step = 4;
+		}
+		if (lcd_commands_step == 1 && !blocks_queued()) {	//recover feedmultiply, current 
+			
+			sprintf_P(cmd1, PSTR("M220 S%d"), saved_feedmultiply);
+			enquecommand(cmd1);
+			isPrintPaused = false;
+			card.startFileprint();
+			lcd_commands_step = 0;
+			lcd_commands_type = 0;
+		}
+		if (lcd_commands_step == 2 && !blocks_queued()) {	//turn on fan, move Z and unretract
+			
+			sprintf_P(cmd1, PSTR("M106 S%d"), fanSpeedBckp);
+			enquecommand(cmd1);
+			strcpy(cmd1, "G1 Z");
+			strcat(cmd1, ftostr32(pause_lastpos[Z_AXIS]));
+			enquecommand(cmd1);
+			if (axis_relative_modes[3] == true) enquecommand_P(PSTR("M83")); // set extruder to relative mode.
+			else enquecommand_P(PSTR("M82")); // set extruder to absolute mode
+			enquecommand_P(PSTR("G1 E"  STRINGIFY(PAUSE_RETRACT))); //unretract
+			enquecommand_P(PSTR("G90")); //absolute positioning
+			lcd_commands_step = 1;
+		}
+		if (lcd_commands_step == 3 && !blocks_queued()) {	//wait for nozzle to reach target temp
+			
+			strcpy(cmd1, "M109 S");
+			strcat(cmd1, ftostr3(HotendTempBckp));
+			enquecommand(cmd1);			
+			lcd_commands_step = 2;
+		}
+		if (lcd_commands_step == 4 && !blocks_queued()) {	//set temperature back and move xy
+			
+			strcpy(cmd1, "M104 S");
+			strcat(cmd1, ftostr3(HotendTempBckp));
+			enquecommand(cmd1);
+			
+			strcpy(cmd1, "G1 X");
+			strcat(cmd1, ftostr32(pause_lastpos[X_AXIS]));
+			strcat(cmd1, " Y");
+			strcat(cmd1, ftostr32(pause_lastpos[Y_AXIS]));
+			enquecommand(cmd1);
+
+			lcd_setstatuspgm(MSG_RESUMING_PRINT);
+			lcd_commands_step = 3;
+		}
+	}
+
 	if (lcd_commands_type == LCD_COMMAND_STOP_PRINT)   /// stop print
 	{
 
-		if (lcd_commands_step == 0) { lcd_commands_step = 6; custom_message = true;	}
+		if (lcd_commands_step == 0) 
+		{ 
+			lcd_commands_step = 6; 
+			custom_message = true;	
+		}
 
 		if (lcd_commands_step == 1 && !blocks_queued())
 		{
@@ -697,6 +781,47 @@ void lcd_commands()
 		}
 
 	}
+	if (lcd_commands_type == LCD_COMMAND_PID_EXTRUDER) {
+		char cmd1[30];
+		
+		if (lcd_commands_step == 0) {
+			custom_message_type = 3;
+			custom_message_state = 1;
+			custom_message = true;
+			lcdDrawUpdate = 3;
+			lcd_commands_step = 3;
+		}
+		if (lcd_commands_step == 3 && !blocks_queued()) { //PID calibration
+			strcpy(cmd1, "M303 E0 S");
+			strcat(cmd1, ftostr3(pid_temp));
+			enquecommand(cmd1);
+			lcd_setstatuspgm(MSG_PID_RUNNING);
+			lcd_commands_step = 2;
+		}
+		if (lcd_commands_step == 2 && pid_tuning_finished) { //saving to eeprom
+			pid_tuning_finished = false;
+			custom_message_state = 0;
+			lcd_setstatuspgm(MSG_PID_FINISHED);
+			strcpy(cmd1, "M301 P");
+			strcat(cmd1, ftostr32(_Kp));
+			strcat(cmd1, " I");
+			strcat(cmd1, ftostr32(_Ki));
+			strcat(cmd1, " D");
+			strcat(cmd1, ftostr32(_Kd));
+			enquecommand(cmd1);
+			enquecommand_P(PSTR("M500"));
+			display_time = millis();
+			lcd_commands_step = 1;
+		}
+		if ((lcd_commands_step == 1) && ((millis()- display_time)>2000)) { //calibration finished message
+			lcd_setstatuspgm(WELCOME_MSG);
+			custom_message_type = 0;
+			custom_message = false;
+			pid_temp = DEFAULT_PID_TEMP;
+			lcd_commands_step = 0;
+			lcd_commands_type = 0;
+		}
+	}
 
 
 }
@@ -711,16 +836,16 @@ static void lcd_return_to_status() {
     lcd_goto_menu(lcd_status_screen, 0, false);
 }
 
+
 static void lcd_sdcard_pause() {
-  card.pauseSDPrint();
-  isPrintPaused = true;
-  lcdDrawUpdate = 3;
+	lcd_return_to_status();
+	lcd_commands_type = LCD_COMMAND_LONG_PAUSE;
+
 }
 
 static void lcd_sdcard_resume() {
-	card.startFileprint();
-	isPrintPaused = false;
-	lcdDrawUpdate = 3;
+	lcd_return_to_status();
+	lcd_commands_type = LCD_COMMAND_LONG_PAUSE_RESUME;
 }
 
 float move_menu_scale;
@@ -1433,6 +1558,25 @@ static void lcd_adjust_bed()
     END_MENU();
 }
 
+void pid_extruder() {
+
+	lcd_implementation_clear();
+	lcd.setCursor(1, 0);
+	lcd_printPGM(MSG_SET_TEMPERATURE);
+	pid_temp += int(encoderPosition);
+	if (pid_temp > HEATER_0_MAXTEMP) pid_temp = HEATER_0_MAXTEMP;
+	if (pid_temp < HEATER_0_MINTEMP) pid_temp = HEATER_0_MINTEMP;
+	encoderPosition = 0;
+	lcd.setCursor(1, 2);
+	lcd.print(ftostr3(pid_temp));
+	if (lcd_clicked()) {
+		lcd_commands_type = LCD_COMMAND_PID_EXTRUDER;
+		lcd_return_to_status();
+		lcd_update(2);
+	}
+
+}
+
 void lcd_adjust_z() {
   int enc_dif = 0;
   int cursor_pos = 1;
@@ -2291,6 +2435,33 @@ void lcd_mesh_calibration_z()
   lcd_return_to_status();
 }
 
+void lcd_pinda_calibration_menu()
+{
+	START_MENU();
+		MENU_ITEM(back, MSG_MENU_CALIBRATION, lcd_calibration_menu);
+		MENU_ITEM(submenu, MSG_CALIBRATE_PINDA, lcd_calibrate_pinda);
+		//MENU_ITEM(back, MSG_SETTINGS, lcd_settings_menu);
+		if (temp_cal_active == false) {
+			MENU_ITEM(function, MSG_TEMP_CALIBRATION_OFF, lcd_temp_calibration_set);
+		}
+		else {
+			MENU_ITEM(function, MSG_TEMP_CALIBRATION_ON, lcd_temp_calibration_set);
+		}
+	END_MENU();
+}
+
+void lcd_temp_calibration_set() {
+	temp_cal_active = !temp_cal_active;
+	eeprom_update_byte((unsigned char *)EEPROM_TEMP_CAL_ACTIVE, temp_cal_active);
+	digipot_init();
+	lcd_goto_menu(lcd_pinda_calibration_menu, 2);
+}
+
+void lcd_calibrate_pinda() {
+	enquecommand_P(PSTR("G76"));
+	lcd_return_to_status();
+}
+
 #ifndef SNMM
 
 /*void lcd_calibrate_extruder() {
@@ -2454,6 +2625,7 @@ static void lcd_calibration_menu()
 MENU_ITEM(function, MSG_CALIBRATE_BED, lcd_mesh_calibration);
     // "Calibrate Z" with storing the reference values to EEPROM.
     MENU_ITEM(submenu, MSG_HOMEYZ, lcd_mesh_calibration_z);
+	
 #ifndef SNMM
 	//MENU_ITEM(function, MSG_CALIBRATE_E, lcd_calibrate_extruder);
 #endif
@@ -2462,6 +2634,8 @@ MENU_ITEM(function, MSG_CALIBRATE_BED, lcd_mesh_calibration);
 #endif
     MENU_ITEM(gcode, MSG_AUTO_HOME, PSTR("G28 W"));
     MENU_ITEM(submenu, MSG_BED_CORRECTION_MENU, lcd_adjust_bed);
+	MENU_ITEM(submenu, MSG_CALIBRATION_PINDA_MENU, lcd_pinda_calibration_menu);
+	MENU_ITEM(submenu, MSG_PID_EXTRUDER, pid_extruder);
     MENU_ITEM(submenu, MSG_SHOW_END_STOPS, menu_show_end_stops);
     MENU_ITEM(gcode, MSG_CALIBRATE_BED_RESET, PSTR("M44"));
 #ifndef SNMM
@@ -3213,9 +3387,7 @@ static void lcd_main_menu()
         
     }*/
     
-    
-    
-  if ( ( IS_SD_PRINTING || is_usb_printing ) && (current_position[Z_AXIS] < Z_HEIGHT_HIDE_LIVE_ADJUST_MENU) && !homing_flag) 
+  if ( ( IS_SD_PRINTING || is_usb_printing ) && (current_position[Z_AXIS] < Z_HEIGHT_HIDE_LIVE_ADJUST_MENU) && !homing_flag && !mesh_bed_leveling_flag)
   {
 	MENU_ITEM(submenu, MSG_BABYSTEP_Z, lcd_babystep_z);//8
   }
@@ -3234,15 +3406,17 @@ static void lcd_main_menu()
   {
     if (card.isFileOpen())
     {
-		if (card.sdprinting)
-		{
-			MENU_ITEM(function, MSG_PAUSE_PRINT, lcd_sdcard_pause);
+		if (mesh_bed_leveling_flag == false && homing_flag == false) {
+			if (card.sdprinting)
+			{
+				MENU_ITEM(function, MSG_PAUSE_PRINT, lcd_sdcard_pause);
+			}
+			else
+			{
+				MENU_ITEM(function, MSG_RESUME_PRINT, lcd_sdcard_resume);
+			}
+			MENU_ITEM(submenu, MSG_STOP_PRINT, lcd_sdcard_stop);
 		}
-		else
-		{
-			MENU_ITEM(function, MSG_RESUME_PRINT, lcd_sdcard_resume);
-		}
-		MENU_ITEM(submenu, MSG_STOP_PRINT, lcd_sdcard_stop);
 	}
 	else
 	{
@@ -3451,7 +3625,8 @@ void lcd_sdcard_stop()
 				card.closefile();
 
 				stoptime = millis();
-				unsigned long t = (stoptime - starttime) / 1000; //time in s
+				unsigned long t = (stoptime - starttime - pause_time) / 1000; //time in s
+				pause_time = 0;
 				save_statistics(total_filament_used, t);
 
 				lcd_return_to_status();
@@ -3731,6 +3906,7 @@ static void lcd_selftest()
 		_progress = lcd_selftest_screen(4, _progress, 3, true, 1500);
 		_result = lcd_selfcheck_axis(2, Z_MAX_POS);
 		enquecommand_P(PSTR("G28 W"));
+		enquecommand_P(PSTR("G1 Z15"));
 	}
 
 	if (_result)
@@ -3959,7 +4135,7 @@ static bool lcd_selfcheck_check_heater(bool _isbed)
 
 	do {
 		_counter++;
-		(_counter < _cycles) ? _docycle = true : _docycle = false;
+		_docycle = (_counter < _cycles) ? true : false;
 
 		manage_heater();
 		manage_inactivity(true);
@@ -3974,9 +4150,9 @@ static bool lcd_selfcheck_check_heater(bool _isbed)
 	int _checked_result = (_isbed) ? degBed() - _checked_snapshot : degHotend(0) - _checked_snapshot;
 	int _opposite_result = (_isbed) ? degHotend(0) - _opposite_snapshot : degBed() - _opposite_snapshot;
 
-	if (_opposite_result < (_isbed) ? 10 : 3)
+	if (_opposite_result < ((_isbed) ? 10 : 3))
 	{
-		if (_checked_result >= (_isbed) ? 3 : 10)
+		if (_checked_result >= ((_isbed) ? 3 : 10))
 		{
 			_stepresult = true;
 		}
@@ -4121,15 +4297,9 @@ static bool lcd_selftest_fan_dialog(int _fan)
 	lcd.setCursor(0, 3); lcd.print(">");
 	lcd.setCursor(1, 3); lcd_printPGM(MSG_SELFTEST_FAN_NO);
 
-
-
-
-
 	int8_t enc_dif = 0;
-	bool _response = false;
 	do
 	{
-
 		switch (_fan)
 		{
 		case 1:
@@ -4143,8 +4313,6 @@ static bool lcd_selftest_fan_dialog(int _fan)
 			analogWrite(FAN_PIN, 255);
 			break;
 		}
-
-
 		if (abs((enc_dif - encoderDiff)) > 2) {
 			if (enc_dif > encoderDiff) {
 				_result = true;
@@ -4169,13 +4337,7 @@ static bool lcd_selftest_fan_dialog(int _fan)
 		manage_heater();
 		delay(100);
 
-		if (lcd_clicked())
-		{
-			_response = true;
-		}
-
-
-	} while (!_response);
+	} while (!lcd_clicked());
 
 	SET_OUTPUT(EXTRUDER_0_AUTO_FAN_PIN);
 	WRITE(EXTRUDER_0_AUTO_FAN_PIN, 0);
@@ -4275,7 +4437,7 @@ static void lcd_selftest_screen_step(int _row, int _col, int _state, const char
 static void lcd_quick_feedback()
 {
   lcdDrawUpdate = 2;
-  button_pressed = false;
+  button_pressed = false;  
   lcd_implementation_quick_feedback();
 }
 
@@ -4630,35 +4792,40 @@ void lcd_buttons_update()
 #if BTN_ENC > 0
   if (lcd_update_enabled == true) { //if we are in non-modal mode, long press can be used and short press triggers with button release
 	  if (READ(BTN_ENC) == 0) { //button is pressed	  
-
-		  if (button_pressed == false && long_press_active == false) {
-			  if (currentMenu != lcd_move_z) {
-				  savedMenu = currentMenu;
-				  savedEncoderPosition = encoderPosition;
+		  if (millis() > button_blanking_time) {
+			  button_blanking_time = millis() + BUTTON_BLANKING_TIME;
+			  if (button_pressed == false && long_press_active == false) {
+				  if (currentMenu != lcd_move_z) {
+					  savedMenu = currentMenu;
+					  savedEncoderPosition = encoderPosition;
+				  }
+				  long_press_timer = millis();
+				  button_pressed = true;
 			  }
-			  long_press_timer = millis();
-			  button_pressed = true;
-		  }
-		  else {
-			  if (millis() - long_press_timer > LONG_PRESS_TIME) { //long press activated
-				   
-				  long_press_active = true;
-				  move_menu_scale = 1.0;
-				  lcd_goto_menu(lcd_move_z);
+			  else {
+				  if (millis() - long_press_timer > LONG_PRESS_TIME) { //long press activated
+
+					  long_press_active = true;
+					  move_menu_scale = 1.0;
+					  lcd_goto_menu(lcd_move_z);
+				  }
 			  }
 		  }
 	  }
 	  else { //button not pressed
 		  if (button_pressed) { //button was released
+			  button_blanking_time = millis() + BUTTON_BLANKING_TIME;
+
 			  if (long_press_active == false) { //button released before long press gets activated
 				  if (currentMenu == lcd_move_z) {
 					  //return to previously active menu and previous encoder position
-					  lcd_goto_menu(savedMenu, savedEncoderPosition);
+					  lcd_goto_menu(savedMenu, savedEncoderPosition);					  
 				  }
 				  else {
 					  newbutton |= EN_C;
 				  }
 			  }
+			  else if (currentMenu == lcd_move_z) lcd_quick_feedback(); 
 			  //button_pressed is set back to false via lcd_quick_feedback function
 		  }
 		  else {			  
@@ -4761,7 +4928,9 @@ void lcd_buzz(long duration, uint16_t freq)
 
 bool lcd_clicked()
 {
-  return LCD_CLICKED;
+	bool clicked = LCD_CLICKED;
+	if(clicked) button_pressed = false;
+    return clicked;
 }
 #endif//ULTIPANEL
 
diff --git a/Firmware/ultralcd.h b/Firmware/ultralcd.h
index 76136ff8f..9ce724009 100644
--- a/Firmware/ultralcd.h
+++ b/Firmware/ultralcd.h
@@ -91,6 +91,9 @@ void lcd_mylang();
   #define LCD_COMMAND_LOAD_FILAMENT 1
   #define LCD_COMMAND_STOP_PRINT 2
   #define LCD_COMMAND_FARM_MODE_CONFIRM 4
+  #define LCD_COMMAND_LONG_PAUSE 5
+  #define LCD_COMMAND_LONG_PAUSE_RESUME 6
+  #define LCD_COMMAND_PID_EXTRUDER 7 
 
   extern unsigned long lcd_timeoutToStatus;
   extern int lcd_commands_type;
@@ -101,6 +104,7 @@ void lcd_mylang();
   extern int farm_status;
 
   extern bool cancel_heatup;
+  extern bool isPrintPaused;
   
   #ifdef FILAMENT_LCD_DISPLAY
         extern unsigned long message_millis;
@@ -234,4 +238,8 @@ void lcd_extr_cal_reset();
 union MenuData;
 
 char reset_menu();
+
+void lcd_pinda_calibration_menu();
+void lcd_calibrate_pinda();
+void lcd_temp_calibration_set();
 #endif //ULTRALCD_H
\ No newline at end of file
diff --git a/Firmware/ultralcd_implementation_hitachi_HD44780.h b/Firmware/ultralcd_implementation_hitachi_HD44780.h
index 21f77c739..da393a66f 100644
--- a/Firmware/ultralcd_implementation_hitachi_HD44780.h
+++ b/Firmware/ultralcd_implementation_hitachi_HD44780.h
@@ -792,7 +792,7 @@ static void lcd_implementation_status_screen()
     lcd.print(LCD_STR_CLOCK[0]);
     if(starttime != 0)
     {
-        uint16_t time = millis()/60000 - starttime/60000;
+		uint16_t time = millis() / 60000 - starttime / 60000;
         lcd.print(itostr2(time/60));
         lcd.print(':');
         lcd.print(itostr2(time%60));
@@ -948,6 +948,33 @@ static void lcd_implementation_status_screen()
 			{
 				lcd.print(lcd_status_message);
 			}
+			// PID tuning in progress
+			if (custom_message_type == 3) {
+				lcd.print(lcd_status_message);
+				if (pid_cycle <= pid_number_of_cycles && custom_message_state > 0) {
+					lcd.setCursor(10, 3);
+					lcd.print(itostr3(pid_cycle));
+					
+					lcd.print('/');
+					lcd.print(itostr3left(pid_number_of_cycles));
+				}
+			}
+			// PINDA temp calibration in progress
+			if (custom_message_type == 4) {
+				char progress[4];
+				lcd.setCursor(0, 3);
+				lcd_printPGM(MSG_TEMP_CALIBRATION);
+				lcd.setCursor(12, 3);
+				sprintf(progress, "%d/6", custom_message_state);
+				lcd.print(progress);
+			}
+			// temp compensation preheat
+			if (custom_message_type == 5) {
+				lcd.setCursor(0, 3);
+				lcd_printPGM(MSG_PINDA_PREHEAT);
+			}
+
+
 		}
 	else
 		{
diff --git a/Firmware/variants/1_75mm_MK2-RAMBo10a-E3Dv6full.h b/Firmware/variants/1_75mm_MK2-RAMBo10a-E3Dv6full.h
index 23d612d38..15a66337c 100644
--- a/Firmware/variants/1_75mm_MK2-RAMBo10a-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK2-RAMBo10a-E3Dv6full.h
@@ -18,6 +18,12 @@ GENERAL SETTINGS
 // Electronics
 #define MOTHERBOARD BOARD_RAMBO_MINI_1_0
 
+// Uncomment the below for the E3D PT100 temperature sensor (with or without PT100 Amplifier)
+//#define E3D_PT100_EXTRUDER_WITH_AMP
+//#define E3D_PT100_EXTRUDER_NO_AMP
+//#define E3D_PT100_BED_WITH_AMP
+//#define E3D_PT100_BED_NO_AMP
+
 
 /*------------------------------------
 AXIS SETTINGS
@@ -48,6 +54,11 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define X_CANCEL_POS 50
 #define Y_CANCEL_POS 190
 
+//Pause print position
+#define X_PAUSE_POS 50
+#define Y_PAUSE_POS 190
+#define Z_PAUSE_LIFT 20
+
 #define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
 #define HOMING_FEEDRATE {3000, 3000, 800, 0}  // set the homing speeds (mm/min)
 
@@ -73,15 +84,26 @@ EXTRUDER SETTINGS
 #define BED_MINTEMP 15
 
 // Maxtemps
+#if defined(E3D_PT100_EXTRUDER_WITH_AMP) || defined(E3D_PT100_EXTRUDER_NO_AMP)
+#define HEATER_0_MAXTEMP 410
+#else
 #define HEATER_0_MAXTEMP 305
+#endif
 #define HEATER_1_MAXTEMP 305
 #define HEATER_2_MAXTEMP 305
 #define BED_MAXTEMP 150
 
+#if defined(E3D_PT100_EXTRUDER_WITH_AMP) || defined(E3D_PT100_EXTRUDER_NO_AMP)
+// Define PID constants for extruder with PT100
+#define  DEFAULT_Kp 21.70
+#define  DEFAULT_Ki 1.60
+#define  DEFAULT_Kd 73.76
+#else
 // Define PID constants for extruder
 #define  DEFAULT_Kp 40.925
 #define  DEFAULT_Ki 4.875
 #define  DEFAULT_Kd 86.085
+#endif
 
 // Extrude mintemp
 #define EXTRUDE_MINTEMP 130
@@ -106,6 +128,9 @@ EXTRUDER SETTINGS
 
 #endif
 
+//#define DIS //for measuring bed heigth and PINDa detection heigth relative to auto home point, experimental function
+
+
 /*------------------------------------
 CHANGE FILAMENT SETTINGS
 *------------------------------------*/
@@ -143,8 +168,8 @@ ADDITIONAL FEATURES SETTINGS
 #endif
 
 // temperature runaway
-//#define TEMP_RUNAWAY_BED_HYSTERESIS 5
-//#define TEMP_RUNAWAY_BED_TIMEOUT 360
+#define TEMP_RUNAWAY_BED_HYSTERESIS 5
+#define TEMP_RUNAWAY_BED_TIMEOUT 360
 
 #define TEMP_RUNAWAY_EXTRUDER_HYSTERESIS 15
 #define TEMP_RUNAWAY_EXTRUDER_TIMEOUT 45
@@ -188,7 +213,7 @@ BED SETTINGS
 #define MESH_MEAS_NUM_Y_POINTS 3
 
 #define MESH_HOME_Z_CALIB 0.2
-#define MESH_HOME_Z_SEARCH 5
+#define MESH_HOME_Z_SEARCH 5 //Z lift for homing, mesh bed leveling etc.
 
 #define X_PROBE_OFFSET_FROM_EXTRUDER 23     // Z probe to nozzle X offset: -left  +right
 #define Y_PROBE_OFFSET_FROM_EXTRUDER 9     // Z probe to nozzle Y offset: -front +behind
@@ -224,9 +249,16 @@ BED SETTINGS
 #ifdef PIDTEMPBED
 //120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+)
 //from FOPDT model - kp=.39 Tp=405 Tdead=66, Tc set to 79.2, aggressive factor of .15 (vs .1, 1, 10)
+#if defined(E3D_PT100_BED_WITH_AMP) || defined(E3D_PT100_BED_NO_AMP)
+// Define PID constants for extruder with PT100
+#define  DEFAULT_bedKp 21.70
+#define  DEFAULT_bedKi 1.60
+#define  DEFAULT_bedKd 73.76
+#else
 #define  DEFAULT_bedKp 126.13
 #define  DEFAULT_bedKi 4.30
 #define  DEFAULT_bedKd 924.76
+#endif
 
 //120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+)
 //from pidautotune
@@ -303,12 +335,26 @@ THERMISTORS SETTINGS
 // 1047 is Pt1000 with 4k7 pullup
 // 1010 is Pt1000 with 1k pullup (non standard)
 // 147 is Pt100 with 4k7 pullup
+// 148 is Pt100 with 4k7 pullup and no PT100 Amplifier (in case type 147 doesn't work)
+// 247 is Pt100 with 4k7 pullup and PT100 Amplifier
 // 110 is Pt100 with 1k pullup (non standard)
 
+#if defined(E3D_PT100_EXTRUDER_WITH_AMP)
+#define TEMP_SENSOR_0 247
+#elif defined(E3D_PT100_EXTRUDER_NO_AMP)
+#define TEMP_SENSOR_0 148
+#else
 #define TEMP_SENSOR_0 5
+#endif
 #define TEMP_SENSOR_1 0
 #define TEMP_SENSOR_2 0
+#if defined(E3D_PT100_BED_WITH_AMP)
+#define TEMP_SENSOR_BED 247
+#elif defined(E3D_PT100_BED_NO_AMP)
+#define TEMP_SENSOR_BED 148
+#else
 #define TEMP_SENSOR_BED 1
+#endif
 
 #define STACK_GUARD_TEST_VALUE 0xA2A2
 
@@ -324,10 +370,25 @@ THERMISTORS SETTINGS
 #define Z_BABYSTEP_MIN -3999
 #define Z_BABYSTEP_MAX 0
 
+#define PINDA_PREHEAT_X 75
+#define PINDA_PREHEAT_Y 75
+#define PINDA_HEAT_T 120 //time in s
+
+#define PINDA_MIN_T 50
+#define PINDA_STEP_T 10
+#define PINDA_MAX_T 100
+
 #define PING_TIME 60 //time in s
 #define PING_TIME_LONG 600 //10 min; used when length of commands buffer > 0 to avoid false triggering when dealing with long gcodes
 #define PING_ALLERT_PERIOD 60 //time in s
 
-#define LONG_PRESS_TIME 1000 //time in ms for button long press
+#define LONG_PRESS_TIME 1000 //time in ms for button long press 
+#define BUTTON_BLANKING_TIME 200 //time in ms for blanking after button release
+
+#define PAUSE_RETRACT 2 
+
+#define DEFAULT_PID_TEMP 210
+
+#define DEFAULT_RETRACTION 1 //used for PINDA temp compensation
 
 #endif //__CONFIGURATION_PRUSA_H
diff --git a/Firmware/variants/1_75mm_MK2-RAMBo13a-E3Dv6full.h b/Firmware/variants/1_75mm_MK2-RAMBo13a-E3Dv6full.h
index 852d2c620..c45c8ec74 100644
--- a/Firmware/variants/1_75mm_MK2-RAMBo13a-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK2-RAMBo13a-E3Dv6full.h
@@ -18,6 +18,12 @@ GENERAL SETTINGS
 // Electronics
 #define MOTHERBOARD BOARD_RAMBO_MINI_1_3
 
+// Uncomment the below for the E3D PT100 temperature sensor (with or without PT100 Amplifier)
+//#define E3D_PT100_EXTRUDER_WITH_AMP
+//#define E3D_PT100_EXTRUDER_NO_AMP
+//#define E3D_PT100_BED_WITH_AMP
+//#define E3D_PT100_BED_NO_AMP
+
 
 /*------------------------------------
 AXIS SETTINGS
@@ -48,6 +54,11 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define X_CANCEL_POS 50
 #define Y_CANCEL_POS 190
 
+//Pause print position
+#define X_PAUSE_POS 50
+#define Y_PAUSE_POS 190
+#define Z_PAUSE_LIFT 20
+
 #define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
 #define HOMING_FEEDRATE {3000, 3000, 800, 0}  // set the homing speeds (mm/min)
 
@@ -73,15 +84,26 @@ EXTRUDER SETTINGS
 #define BED_MINTEMP 15
 
 // Maxtemps
+#if defined(E3D_PT100_EXTRUDER_WITH_AMP) || defined(E3D_PT100_EXTRUDER_NO_AMP)
+#define HEATER_0_MAXTEMP 410
+#else
 #define HEATER_0_MAXTEMP 305
+#endif
 #define HEATER_1_MAXTEMP 305
 #define HEATER_2_MAXTEMP 305
 #define BED_MAXTEMP 150
 
+#if defined(E3D_PT100_EXTRUDER_WITH_AMP) || defined(E3D_PT100_EXTRUDER_NO_AMP)
+// Define PID constants for extruder with PT100
+#define  DEFAULT_Kp 21.70
+#define  DEFAULT_Ki 1.60
+#define  DEFAULT_Kd 73.76
+#else
 // Define PID constants for extruder
 #define  DEFAULT_Kp 40.925
 #define  DEFAULT_Ki 4.875
 #define  DEFAULT_Kd 86.085
+#endif
 
 // Extrude mintemp
 #define EXTRUDE_MINTEMP 130
@@ -93,6 +115,9 @@ EXTRUDER SETTINGS
 #define EXTRUDER_AUTO_FAN_TEMPERATURE 50
 #define EXTRUDER_AUTO_FAN_SPEED   255  // == full speed
 
+
+
+
 // Prusa Single extruder multiple material suport
 //#define SNMM
 
@@ -106,6 +131,9 @@ EXTRUDER SETTINGS
 
 #endif
 
+//#define DIS //for measuring bed heigth and PINDa detection heigth relative to auto home point, experimental function
+
+
 /*------------------------------------
 CHANGE FILAMENT SETTINGS
 *------------------------------------*/
@@ -143,8 +171,8 @@ ADDITIONAL FEATURES SETTINGS
 #endif
 
 // temperature runaway
-//#define TEMP_RUNAWAY_BED_HYSTERESIS 5
-//#define TEMP_RUNAWAY_BED_TIMEOUT 360
+#define TEMP_RUNAWAY_BED_HYSTERESIS 5
+#define TEMP_RUNAWAY_BED_TIMEOUT 360
 
 #define TEMP_RUNAWAY_EXTRUDER_HYSTERESIS 15
 #define TEMP_RUNAWAY_EXTRUDER_TIMEOUT 45
@@ -188,7 +216,7 @@ BED SETTINGS
 #define MESH_MEAS_NUM_Y_POINTS 3
 
 #define MESH_HOME_Z_CALIB 0.2
-#define MESH_HOME_Z_SEARCH 5
+#define MESH_HOME_Z_SEARCH 5 //Z lift for homing, mesh bed leveling etc.
 
 #define X_PROBE_OFFSET_FROM_EXTRUDER 23     // Z probe to nozzle X offset: -left  +right
 #define Y_PROBE_OFFSET_FROM_EXTRUDER 9     // Z probe to nozzle Y offset: -front +behind
@@ -224,9 +252,16 @@ BED SETTINGS
 #ifdef PIDTEMPBED
 //120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+)
 //from FOPDT model - kp=.39 Tp=405 Tdead=66, Tc set to 79.2, aggressive factor of .15 (vs .1, 1, 10)
+#if defined(E3D_PT100_BED_WITH_AMP) || defined(E3D_PT100_BED_NO_AMP)
+// Define PID constants for extruder with PT100
+#define  DEFAULT_bedKp 21.70
+#define  DEFAULT_bedKi 1.60
+#define  DEFAULT_bedKd 73.76
+#else
 #define  DEFAULT_bedKp 126.13
 #define  DEFAULT_bedKi 4.30
 #define  DEFAULT_bedKd 924.76
+#endif
 
 //120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+)
 //from pidautotune
@@ -303,12 +338,26 @@ THERMISTORS SETTINGS
 // 1047 is Pt1000 with 4k7 pullup
 // 1010 is Pt1000 with 1k pullup (non standard)
 // 147 is Pt100 with 4k7 pullup
+// 148 is E3D Pt100 with 4k7 pullup and no PT100 Amplifier on a MiniRambo 1.3a
+// 247 is Pt100 with 4k7 pullup and PT100 Amplifier
 // 110 is Pt100 with 1k pullup (non standard)
 
+#if defined(E3D_PT100_EXTRUDER_WITH_AMP)
+#define TEMP_SENSOR_0 247
+#elif defined(E3D_PT100_EXTRUDER_NO_AMP)
+#define TEMP_SENSOR_0 148
+#else
 #define TEMP_SENSOR_0 5
+#endif
 #define TEMP_SENSOR_1 0
 #define TEMP_SENSOR_2 0
+#if defined(E3D_PT100_BED_WITH_AMP)
+#define TEMP_SENSOR_BED 247
+#elif defined(E3D_PT100_BED_NO_AMP)
+#define TEMP_SENSOR_BED 148
+#else
 #define TEMP_SENSOR_BED 1
+#endif
 
 #define STACK_GUARD_TEST_VALUE 0xA2A2
 
@@ -324,10 +373,25 @@ THERMISTORS SETTINGS
 #define Z_BABYSTEP_MIN -3999
 #define Z_BABYSTEP_MAX 0
 
+#define PINDA_PREHEAT_X 75
+#define PINDA_PREHEAT_Y 75
+#define PINDA_HEAT_T 120 //time in s
+
+#define PINDA_MIN_T 50
+#define PINDA_STEP_T 10
+#define PINDA_MAX_T 100
+
 #define PING_TIME 60 //time in s
 #define PING_TIME_LONG 600 //10 min; used when length of commands buffer > 0 to avoid false triggering when dealing with long gcodes
 #define PING_ALLERT_PERIOD 60 //time in s
 
-#define LONG_PRESS_TIME 1000 //time in ms for button long press
+#define LONG_PRESS_TIME 1000 //time in ms for button long press 
+#define BUTTON_BLANKING_TIME 200 //time in ms for blanking after button release
+
+#define PAUSE_RETRACT 2 
+
+#define DEFAULT_PID_TEMP 210
+
+#define DEFAULT_RETRACTION 1 //used for PINDA temp compensation
 
 #endif //__CONFIGURATION_PRUSA_H