diff --git a/Firmware/Configuration_prusa.h b/Firmware/Configuration_prusa.h
index 5654506e7..46c751046 100644
--- a/Firmware/Configuration_prusa.h
+++ b/Firmware/Configuration_prusa.h
@@ -60,7 +60,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define X_MIN_POS 0
 #define Y_MAX_POS 210
 #define Y_MIN_POS -4
-#define Z_MAX_POS 200
+#define Z_MAX_POS 210
 #define Z_MIN_POS 0.15
 
 // Canceled home position
@@ -468,6 +468,6 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 
 #define M600_TIMEOUT 600  //seconds
 
-//#define SUPPORT_VERBOSITY
+#define SUPPORT_VERBOSITY
 
 #endif //__CONFIGURATION_PRUSA_H
diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index a576a1f26..4b2271eec 100644
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -435,6 +435,7 @@ void force_high_power_mode(bool start_high_power_section);
 
 // G-codes
 bool gcode_M45(bool onlyZ, int8_t verbosity_level);
+void gcode_M114();
 void gcode_M701();
 
 #define UVLO !(PINE & (1<<4))
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 3b51a67c4..d33ff0200 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -2228,8 +2228,12 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level)
 				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] / 40, active_extruder);
 				st_synchronize();
+				
 				if (result >= 0)
 				{
+					#ifdef HEATBED_V2
+					sample_z();
+					#else //HEATBED_V2
 					point_too_far_mask = 0;
 					// Second half: The fine adjustment.
 					// Let the planner use the uncorrected coordinates.
@@ -2244,8 +2248,10 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level)
 					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] / 40, active_extruder);
 					st_synchronize();
-					// if (result >= 0) babystep_apply();
+					// if (result >= 0) babystep_apply();					
+					#endif //HEATBED_V2
 				}
+				
 				lcd_bed_calibration_show_result(result, point_too_far_mask);
 				if (result >= 0)
 				{
@@ -2276,6 +2282,29 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level)
 	return final_result;
 }
 
+void gcode_M114()
+{
+	SERIAL_PROTOCOLPGM("X:");
+	SERIAL_PROTOCOL(current_position[X_AXIS]);
+	SERIAL_PROTOCOLPGM(" Y:");
+	SERIAL_PROTOCOL(current_position[Y_AXIS]);
+	SERIAL_PROTOCOLPGM(" Z:");
+	SERIAL_PROTOCOL(current_position[Z_AXIS]);
+	SERIAL_PROTOCOLPGM(" E:");
+	SERIAL_PROTOCOL(current_position[E_AXIS]);
+
+	SERIAL_PROTOCOLRPGM(MSG_COUNT_X);
+	SERIAL_PROTOCOL(float(st_get_position(X_AXIS)) / axis_steps_per_unit[X_AXIS]);
+	SERIAL_PROTOCOLPGM(" Y:");
+	SERIAL_PROTOCOL(float(st_get_position(Y_AXIS)) / axis_steps_per_unit[Y_AXIS]);
+	SERIAL_PROTOCOLPGM(" Z:");
+	SERIAL_PROTOCOL(float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS]);
+	SERIAL_PROTOCOLPGM(" E:");
+	SERIAL_PROTOCOL(float(st_get_position(E_AXIS)) / axis_steps_per_unit[E_AXIS]);
+
+	SERIAL_PROTOCOLLN("");
+}
+
 void gcode_M701()
 {
 #ifdef SNMM
@@ -4908,25 +4937,7 @@ Sigma_Exit:
       lcd_setstatus(strchr_pointer + 5);
       break;*/
     case 114: // M114
-      SERIAL_PROTOCOLPGM("X:");
-      SERIAL_PROTOCOL(current_position[X_AXIS]);
-      SERIAL_PROTOCOLPGM(" Y:");
-      SERIAL_PROTOCOL(current_position[Y_AXIS]);
-      SERIAL_PROTOCOLPGM(" Z:");
-      SERIAL_PROTOCOL(current_position[Z_AXIS]);
-      SERIAL_PROTOCOLPGM(" E:");
-      SERIAL_PROTOCOL(current_position[E_AXIS]);
-
-      SERIAL_PROTOCOLRPGM(MSG_COUNT_X);
-      SERIAL_PROTOCOL(float(st_get_position(X_AXIS))/axis_steps_per_unit[X_AXIS]);
-      SERIAL_PROTOCOLPGM(" Y:");
-      SERIAL_PROTOCOL(float(st_get_position(Y_AXIS))/axis_steps_per_unit[Y_AXIS]);
-      SERIAL_PROTOCOLPGM(" Z:");
-      SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);
-      SERIAL_PROTOCOLPGM(" E:");
-      SERIAL_PROTOCOL(float(st_get_position(E_AXIS))/axis_steps_per_unit[E_AXIS]);
-
-      SERIAL_PROTOCOLLN("");
+		gcode_M114();
       break;
     case 120: // M120
       enable_endstops(false) ;
diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp
index 15e4743bf..2a942da1b 100644
--- a/Firmware/mesh_bed_calibration.cpp
+++ b/Firmware/mesh_bed_calibration.cpp
@@ -104,10 +104,17 @@ const float bed_ref_points[] PROGMEM = {
 
 static inline float sqr(float x) { return x * x; }
 
+#ifdef HEATBED_V2
 static inline bool point_on_1st_row(const uint8_t i)
 {
-	return (i < 2);
+	return false;
 }
+#else //HEATBED_V2
+static inline bool point_on_1st_row(const uint8_t i)
+{
+	return (i < 3);
+}
+#endif //HEATBED_V2
 
 // Weight of a point coordinate in a least squares optimization.
 // The first row of points may not be fully reachable
@@ -904,22 +911,29 @@ error:
 #define FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS (8.f)
 #define FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS (4.f)
 #define FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP  (1.f)
+#ifdef HEATBED_V2
+#define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP   (2.f)
+#define FIND_BED_INDUCTION_SENSOR_POINT_MAX_Z_ERROR  (0.01f)
+#else //HEATBED_V2
 #define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP   (0.2f)
+#endif //HEATBED_V2
+
+#ifdef HEATBED_V2
 inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
 {
 	#ifdef SUPPORT_VERBOSITY
-	if(verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
+	if (verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
 	#endif // SUPPORT_VERBOSITY
 	float feedrate = homing_feedrate[X_AXIS] / 60.f;
-    bool found = false;
+	bool found = false;
 
-    {
-        float x0 = current_position[X_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
-        float x1 = current_position[X_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
-        float y0 = current_position[Y_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
-        float y1 = current_position[Y_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
-        uint8_t nsteps_y;
-        uint8_t i;
+	{
+		float x0 = current_position[X_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
+		float x1 = current_position[X_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
+		float y0 = current_position[Y_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
+		float y1 = current_position[Y_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
+		uint8_t nsteps_y;
+		uint8_t i;
 		if (x0 < X_MIN_POS) {
 			x0 = X_MIN_POS;
 			#ifdef SUPPORT_VERBOSITY
@@ -944,163 +958,421 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
 			if (verbosity_level >= 20) SERIAL_ECHOLNPGM("Y searching radius higher than X_MAX. Clamping was done.");
 			#endif // SUPPORT_VERBOSITY
 		}
-        nsteps_y = int(ceil((y1 - y0) / FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP));
+		nsteps_y = int(ceil((y1 - y0) / FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP));
 
-        enable_endstops(false);
-        bool  dir_positive = true;
+		enable_endstops(false);
+		bool  dir_positive = true;
+		float z_error = 2 * FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP;
+		float find_bed_induction_sensor_point_z_step = FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP;
+		float initial_z_position = current_position[Z_AXIS];
 
-//        go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
-        go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
-        // Continously lower the Z axis.
-        endstops_hit_on_purpose();
-        enable_z_endstop(true);
-        while (current_position[Z_AXIS] > -10.f) {
-            // Do nsteps_y zig-zag movements.
-            current_position[Y_AXIS] = y0;
-            for (i = 0; i < (nsteps_y - 1); current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i) {
-                // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
-                current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
-                go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
-                dir_positive = ! dir_positive;
-                if (endstop_z_hit_on_purpose())
-                    goto endloop;
-            }
-            for (i = 0; i < (nsteps_y - 1); current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i) {
-                // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
-                current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
-                go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
-                dir_positive = ! dir_positive;
-                if (endstop_z_hit_on_purpose())
-                    goto endloop;
-            }
-        }
-        endloop:
-//        SERIAL_ECHOLN("First hit");
+		//        go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
+		go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
+		// Continously lower the Z axis.
+		endstops_hit_on_purpose();
+		enable_z_endstop(true);
+		while (current_position[Z_AXIS] > -10.f && z_error > FIND_BED_INDUCTION_SENSOR_POINT_MAX_Z_ERROR) {
+			// Do nsteps_y zig-zag movements.
 
-        // we have to let the planner know where we are right now as it is not where we said to go.
-        update_current_position_xyz();
+			//SERIAL_ECHOPGM("z_error: ");
+			//MYSERIAL.println(z_error);
+			current_position[Y_AXIS] = y0;
+			initial_z_position = current_position[Z_AXIS];
+			for (i = 0; i < (nsteps_y - 1); current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++i) {
+				// Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
+				current_position[Z_AXIS] -= find_bed_induction_sensor_point_z_step / float(nsteps_y - 1);
+				go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
+				dir_positive = !dir_positive;
+				if (endstop_z_hit_on_purpose()) {
+					update_current_position_xyz();
+					z_error = 2 * (initial_z_position - current_position[Z_AXIS]);
+					if (z_error > FIND_BED_INDUCTION_SENSOR_POINT_MAX_Z_ERROR) {
+						find_bed_induction_sensor_point_z_step = z_error / 2;
+						current_position[Z_AXIS] += z_error;
+						enable_z_endstop(false);
+						go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
+						enable_z_endstop(true);
+					}
+					goto endloop;
+				}
+			}
+			initial_z_position = current_position[Z_AXIS];
+			for (i = 0; i < (nsteps_y - 1); current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++i) {
+				// Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
+				current_position[Z_AXIS] -= find_bed_induction_sensor_point_z_step / float(nsteps_y - 1);
+				go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
+				dir_positive = !dir_positive;
+				if (endstop_z_hit_on_purpose()) {
+					update_current_position_xyz();
+					z_error = 2 * (initial_z_position - current_position[Z_AXIS]);
+					if (z_error > FIND_BED_INDUCTION_SENSOR_POINT_MAX_Z_ERROR) {
+						find_bed_induction_sensor_point_z_step = z_error / 2;
+						current_position[Z_AXIS] += z_error;
+						enable_z_endstop(false);
+						go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
+						enable_z_endstop(true);
+					}
+					goto endloop;
+				}
+			}
+		endloop:;
+		}
+		#ifdef SUPPORT_VERBOSITY
+		if (verbosity_level >= 20) {
+			SERIAL_ECHO("First hit");
+			SERIAL_ECHO("- X: ");
+			MYSERIAL.print(current_position[X_AXIS]);
+			SERIAL_ECHO("; Y: ");
+			MYSERIAL.print(current_position[Y_AXIS]);
+			SERIAL_ECHO("; Z: ");
+			MYSERIAL.println(current_position[Z_AXIS]);
+		}
+		#endif //SUPPORT_VERBOSITY
+		//lcd_show_fullscreen_message_and_wait_P(PSTR("First hit"));
+		//lcd_update_enable(true);
 
-        // Search in this plane for the first hit. Zig-zag first in X, then in Y axis.
-        for (int8_t iter = 0; iter < 3; ++ iter) {
-            if (iter > 0) {
-                // Slightly lower the Z axis to get a reliable trigger.
-                current_position[Z_AXIS] -= 0.02f;
-                go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
-            }
+		float init_x_position = current_position[X_AXIS];
+		float init_y_position = current_position[Y_AXIS];
 
-            // Do nsteps_y zig-zag movements.
-            float a, b;
-            enable_endstops(false);
-            enable_z_endstop(false);
-            current_position[Y_AXIS] = y0;
-            go_xy(x0, current_position[Y_AXIS], feedrate);
-            enable_z_endstop(true);
-            found = false;
-            for (i = 0, dir_positive = true; i < (nsteps_y - 1); current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
-                go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
-                if (endstop_z_hit_on_purpose()) {
-                    found = true;
-                    break;
-                }
-            }
-            update_current_position_xyz();
-            if (! found) {
-//                SERIAL_ECHOLN("Search in Y - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search in Y - found");
-            a = current_position[Y_AXIS];
+		// we have to let the planner know where we are right now as it is not where we said to go.
+		update_current_position_xyz();
+		enable_z_endstop(false);
+		
+		for (int8_t iter = 0; iter < 2; ++iter) {
+			/*SERIAL_ECHOPGM("iter: ");
+			MYSERIAL.println(iter);
+			SERIAL_ECHOPGM("1 - current_position[Z_AXIS]: ");
+			MYSERIAL.println(current_position[Z_AXIS]);*/
 
-            enable_z_endstop(false);
-            current_position[Y_AXIS] = y1;
-            go_xy(x0, current_position[Y_AXIS], feedrate);
-            enable_z_endstop(true);
-            found = false;
-            for (i = 0, dir_positive = true; i < (nsteps_y - 1); current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
-                go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
-                if (endstop_z_hit_on_purpose()) {
-                    found = true;
-                    break;
-                }
-            }
-            update_current_position_xyz();
-            if (! found) {
-//                SERIAL_ECHOLN("Search in Y2 - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search in Y2 - found");
-            b = current_position[Y_AXIS];
-            current_position[Y_AXIS] = 0.5f * (a + b);
+			// Slightly lower the Z axis to get a reliable trigger.
+			current_position[Z_AXIS] -= 0.1f;
+			go_xyz(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], homing_feedrate[Z_AXIS] / (60 * 10));
 
-            // Search in the X direction along a cross.
-            found = false;
-            enable_z_endstop(false);
-            go_xy(x0, current_position[Y_AXIS], feedrate);
-            enable_z_endstop(true);
-            go_xy(x1, current_position[Y_AXIS], feedrate);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-//                SERIAL_ECHOLN("Search X span 0 - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search X span 0 - found");
-            a = current_position[X_AXIS];
-            enable_z_endstop(false);
-            go_xy(x1, current_position[Y_AXIS], feedrate);
-            enable_z_endstop(true);
-            go_xy(x0, current_position[Y_AXIS], feedrate);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-//                SERIAL_ECHOLN("Search X span 1 - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search X span 1 - found");
-            b = current_position[X_AXIS];
-            // Go to the center.
-            enable_z_endstop(false);
-            current_position[X_AXIS] = 0.5f * (a + b);
-            go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
-            found = true;
+			SERIAL_ECHOPGM("2 - current_position[Z_AXIS]: ");
+			MYSERIAL.println(current_position[Z_AXIS]);
+			// Do nsteps_y zig-zag movements.
+			float a, b;
+			float avg[2] = { 0,0 };
+
+			for (int iteration = 0; iteration < 8; iteration++) {
+
+				found = false;
+				invert_z_endstop(true);
+				enable_z_endstop(true);
+				go_xy(x0, current_position[Y_AXIS], feedrate / 5);
+				update_current_position_xyz();
+				if (!endstop_z_hit_on_purpose()) {
+					//                SERIAL_ECHOLN("Search X span 0 - not found");
+					continue;
+				}
+
+				//lcd_show_fullscreen_message_and_wait_P(PSTR("X1 found"));
+				//lcd_update_enable(true);
+				//            SERIAL_ECHOLN("Search X span 0 - found");
+				a = current_position[X_AXIS];
+				enable_z_endstop(false);
+				go_xy(init_x_position, current_position[Y_AXIS], feedrate / 5);
+				enable_z_endstop(true);
+				go_xy(x1, current_position[Y_AXIS], feedrate / 5);
+				update_current_position_xyz();
+				if (!endstop_z_hit_on_purpose()) {
+					//                SERIAL_ECHOLN("Search X span 1 - not found");
+					continue;
+				}
+				//lcd_show_fullscreen_message_and_wait_P(PSTR("X2 found"));
+				//lcd_update_enable(true);
+				//            SERIAL_ECHOLN("Search X span 1 - found");
+				b = current_position[X_AXIS];
+				// Go to the center.
+				enable_z_endstop(false);
+				current_position[X_AXIS] = 0.5f * (a + b);
+				go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate / 5);
+				found = true;
+				
+				// Search in the Y direction along a cross.
+				found = false;
+				enable_z_endstop(true);
+				go_xy(current_position[X_AXIS], y0, feedrate / 5);
+				update_current_position_xyz();
+				if (!endstop_z_hit_on_purpose()) {
+					//                SERIAL_ECHOLN("Search Y2 span 0 - not found");
+					continue;
+				}
+				//lcd_show_fullscreen_message_and_wait_P(PSTR("Y1 found"));
+				//lcd_update_enable(true);
+				//            SERIAL_ECHOLN("Search Y2 span 0 - found");
+				a = current_position[Y_AXIS];
+				enable_z_endstop(false);
+				go_xy(current_position[X_AXIS], init_y_position, feedrate / 5);
+				enable_z_endstop(true);
+				go_xy(current_position[X_AXIS], y1, feedrate / 5);
+				update_current_position_xyz();
+				if (!endstop_z_hit_on_purpose()) {
+					//                SERIAL_ECHOLN("Search Y2 span 1 - not found");
+					continue;
+				}
+				//            SERIAL_ECHOLN("Search Y2 span 1 - found");
+				b = current_position[Y_AXIS];
+				//lcd_show_fullscreen_message_and_wait_P(PSTR("Y2 found"));
+				//lcd_update_enable(true);
+
+				// Go to the center.
+				enable_z_endstop(false);
+				invert_z_endstop(false);
+				current_position[Y_AXIS] = 0.5f * (a + b);
+				go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate / 5);
+
+				#ifdef SUPPORT_VERBOSITY
+				if (verbosity_level >= 20) {
+					SERIAL_ECHOPGM("ITERATION: ");
+					MYSERIAL.println(iteration);
+					SERIAL_ECHOPGM("CURRENT POSITION X: ");
+					MYSERIAL.println(current_position[X_AXIS]);
+					SERIAL_ECHOPGM("CURRENT POSITION Y: ");
+					MYSERIAL.println(current_position[Y_AXIS]);
+				}
+				#endif //SUPPORT_VERBOSITY
+
+				if (iteration > 0) {
+					// Average the last 7 measurements.
+					avg[X_AXIS] += current_position[X_AXIS];
+					avg[Y_AXIS] += current_position[Y_AXIS];
+				}
+
+				init_x_position = current_position[X_AXIS];
+				init_y_position = current_position[Y_AXIS];
+
+				found = true;
+
+			}
+			avg[X_AXIS] *= (1.f / 7.f);
+			avg[Y_AXIS] *= (1.f / 7.f);
+
+			current_position[X_AXIS] = avg[X_AXIS];
+			current_position[Y_AXIS] = avg[Y_AXIS];
+			#ifdef SUPPORT_VERBOSITY
+			if (verbosity_level >= 20) {
+				SERIAL_ECHOPGM("AVG CURRENT POSITION X: ");
+				MYSERIAL.println(current_position[X_AXIS]);
+				SERIAL_ECHOPGM("AVG CURRENT POSITION Y: ");
+				MYSERIAL.println(current_position[Y_AXIS]);
+			}
+			#endif // SUPPORT_VERBOSITY
+			go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
+			#ifdef SUPPORT_VERBOSITY
+			if (verbosity_level >= 20) {
+				lcd_show_fullscreen_message_and_wait_P(PSTR("Final position"));
+				lcd_update_enable(true);
+			}
+			#endif //SUPPORT_VERBOSITY
+
+			break;
+		}
+	}
+
+	enable_z_endstop(false);
+	return found;
+
+}
+#else //HEATBED_V2
+inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
+{
+	#ifdef SUPPORT_VERBOSITY
+	if (verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
+	#endif // SUPPORT_VERBOSITY
+	float feedrate = homing_feedrate[X_AXIS] / 60.f;
+	bool found = false;
+
+	{
+		float x0 = current_position[X_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
+		float x1 = current_position[X_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
+		float y0 = current_position[Y_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
+		float y1 = current_position[Y_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
+		uint8_t nsteps_y;
+		uint8_t i;
+		if (x0 < X_MIN_POS) {
+			x0 = X_MIN_POS;
+			#ifdef SUPPORT_VERBOSITY
+			if (verbosity_level >= 20) SERIAL_ECHOLNPGM("X searching radius lower than X_MIN. Clamping was done.");
+			#endif // SUPPORT_VERBOSITY
+		}
+		if (x1 > X_MAX_POS) {
+			x1 = X_MAX_POS;
+			#ifdef SUPPORT_VERBOSITY
+			if (verbosity_level >= 20) SERIAL_ECHOLNPGM("X searching radius higher than X_MAX. Clamping was done.");
+			#endif // SUPPORT_VERBOSITY
+		}
+		if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION) {
+			y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
+			#ifdef SUPPORT_VERBOSITY
+			if (verbosity_level >= 20) SERIAL_ECHOLNPGM("Y searching radius lower than Y_MIN. Clamping was done.");
+			#endif // SUPPORT_VERBOSITY
+		}
+		if (y1 > Y_MAX_POS) {
+			y1 = Y_MAX_POS;
+			#ifdef SUPPORT_VERBOSITY
+			if (verbosity_level >= 20) SERIAL_ECHOLNPGM("Y searching radius higher than X_MAX. Clamping was done.");
+			#endif // SUPPORT_VERBOSITY
+		}
+		nsteps_y = int(ceil((y1 - y0) / FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP));
+
+		enable_endstops(false);
+		bool  dir_positive = true;
+
+		//        go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
+		go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
+		// Continously lower the Z axis.
+		endstops_hit_on_purpose();
+		enable_z_endstop(true);
+		while (current_position[Z_AXIS] > -10.f) {
+			// Do nsteps_y zig-zag movements.
+			current_position[Y_AXIS] = y0;
+			for (i = 0; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++i) {
+				// Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
+				current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
+				go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
+				dir_positive = !dir_positive;
+				if (endstop_z_hit_on_purpose())
+					goto endloop;
+			}
+			for (i = 0; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++i) {
+				// Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
+				current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
+				go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
+				dir_positive = !dir_positive;
+				if (endstop_z_hit_on_purpose())
+					goto endloop;
+			}
+		}
+	endloop:
+		//        SERIAL_ECHOLN("First hit");
+
+		// we have to let the planner know where we are right now as it is not where we said to go.
+		update_current_position_xyz();
+
+		// Search in this plane for the first hit. Zig-zag first in X, then in Y axis.
+		for (int8_t iter = 0; iter < 3; ++iter) {
+			if (iter > 0) {
+				// Slightly lower the Z axis to get a reliable trigger.
+				current_position[Z_AXIS] -= 0.02f;
+				go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS] / 60);
+			}
+
+			// Do nsteps_y zig-zag movements.
+			float a, b;
+			enable_endstops(false);
+			enable_z_endstop(false);
+			current_position[Y_AXIS] = y0;
+			go_xy(x0, current_position[Y_AXIS], feedrate);
+			enable_z_endstop(true);
+			found = false;
+			for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++i, dir_positive = !dir_positive) {
+				go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
+				if (endstop_z_hit_on_purpose()) {
+					found = true;
+					break;
+				}
+			}
+			update_current_position_xyz();
+			if (!found) {
+				//                SERIAL_ECHOLN("Search in Y - not found");
+				continue;
+			}
+			//            SERIAL_ECHOLN("Search in Y - found");
+			a = current_position[Y_AXIS];
+
+			enable_z_endstop(false);
+			current_position[Y_AXIS] = y1;
+			go_xy(x0, current_position[Y_AXIS], feedrate);
+			enable_z_endstop(true);
+			found = false;
+			for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++i, dir_positive = !dir_positive) {
+				go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
+				if (endstop_z_hit_on_purpose()) {
+					found = true;
+					break;
+				}
+			}
+			update_current_position_xyz();
+			if (!found) {
+				//                SERIAL_ECHOLN("Search in Y2 - not found");
+				continue;
+			}
+			//            SERIAL_ECHOLN("Search in Y2 - found");
+			b = current_position[Y_AXIS];
+			current_position[Y_AXIS] = 0.5f * (a + b);
+
+			// Search in the X direction along a cross.
+			found = false;
+			enable_z_endstop(false);
+			go_xy(x0, current_position[Y_AXIS], feedrate);
+			enable_z_endstop(true);
+			go_xy(x1, current_position[Y_AXIS], feedrate);
+			update_current_position_xyz();
+			if (!endstop_z_hit_on_purpose()) {
+				//                SERIAL_ECHOLN("Search X span 0 - not found");
+				continue;
+			}
+			//            SERIAL_ECHOLN("Search X span 0 - found");
+			a = current_position[X_AXIS];
+			enable_z_endstop(false);
+			go_xy(x1, current_position[Y_AXIS], feedrate);
+			enable_z_endstop(true);
+			go_xy(x0, current_position[Y_AXIS], feedrate);
+			update_current_position_xyz();
+			if (!endstop_z_hit_on_purpose()) {
+				//                SERIAL_ECHOLN("Search X span 1 - not found");
+				continue;
+			}
+			//            SERIAL_ECHOLN("Search X span 1 - found");
+			b = current_position[X_AXIS];
+			// Go to the center.
+			enable_z_endstop(false);
+			current_position[X_AXIS] = 0.5f * (a + b);
+			go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
+			found = true;
 
 #if 1
-            // Search in the Y direction along a cross.
-            found = false;
-            enable_z_endstop(false);
-            go_xy(current_position[X_AXIS], y0, feedrate);
-            enable_z_endstop(true);
-            go_xy(current_position[X_AXIS], y1, feedrate);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-//                SERIAL_ECHOLN("Search Y2 span 0 - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search Y2 span 0 - found");
-            a = current_position[Y_AXIS];
-            enable_z_endstop(false);
-            go_xy(current_position[X_AXIS], y1, feedrate);
-            enable_z_endstop(true);
-            go_xy(current_position[X_AXIS], y0, feedrate);
-            update_current_position_xyz();
-            if (! endstop_z_hit_on_purpose()) {
-//                SERIAL_ECHOLN("Search Y2 span 1 - not found");
-                continue;
-            }
-//            SERIAL_ECHOLN("Search Y2 span 1 - found");
-            b = current_position[Y_AXIS];
-            // Go to the center.
-            enable_z_endstop(false);
-            current_position[Y_AXIS] = 0.5f * (a + b);
-            go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
-            found = true;
+			// Search in the Y direction along a cross.
+			found = false;
+			enable_z_endstop(false);
+			go_xy(current_position[X_AXIS], y0, feedrate);
+			enable_z_endstop(true);
+			go_xy(current_position[X_AXIS], y1, feedrate);
+			update_current_position_xyz();
+			if (!endstop_z_hit_on_purpose()) {
+				//                SERIAL_ECHOLN("Search Y2 span 0 - not found");
+				continue;
+			}
+			//            SERIAL_ECHOLN("Search Y2 span 0 - found");
+			a = current_position[Y_AXIS];
+			enable_z_endstop(false);
+			go_xy(current_position[X_AXIS], y1, feedrate);
+			enable_z_endstop(true);
+			go_xy(current_position[X_AXIS], y0, feedrate);
+			update_current_position_xyz();
+			if (!endstop_z_hit_on_purpose()) {
+				//                SERIAL_ECHOLN("Search Y2 span 1 - not found");
+				continue;
+			}
+			//            SERIAL_ECHOLN("Search Y2 span 1 - found");
+			b = current_position[Y_AXIS];
+			// Go to the center.
+			enable_z_endstop(false);
+			current_position[Y_AXIS] = 0.5f * (a + b);
+			go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
+			found = true;
 #endif
-            break;
-        }
-    }
+			break;
+		}
+	}
 
-    enable_z_endstop(false);
-    return found;
+	enable_z_endstop(false);
+	return found;
 }
 
+#endif //HEATBED_V2
+
 // Search around the current_position[X,Y,Z].
 // It is expected, that the induction sensor is switched on at the current position.
 // Look around this center point by painting a star around the point.
@@ -1368,7 +1640,7 @@ canceled:
 // Searching in a zig-zag movement in a plane for the maximum width of the response.
 // This function may set the current_position[Y_AXIS] below Y_MIN_POS, if the function succeeded.
 // If this function failed, the Y coordinate will never be outside the working space.
-#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (4.f)
+#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (8.f)
 #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f)
 inline bool improve_bed_induction_sensor_point3(int verbosity_level)
 {	
@@ -1855,7 +2127,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
 		#endif // SUPPORT_VERBOSITY
 		if (!find_bed_induction_sensor_point_xy(verbosity_level))
 			return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
-#if 1
+#ifndef HEATBED_V2
 		
 			if (k == 0 || k == 1) {
 				// Improve the position of the 1st row sensor points by a zig-zag movement.
@@ -1876,7 +2148,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
 					// not found
 					return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
 			}
-#endif
+#endif //HEATBED_V2
 			#ifdef SUPPORT_VERBOSITY
 			if (verbosity_level >= 10)
 				delay_keep_alive(3000);
@@ -2292,16 +2564,7 @@ BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8
     }
 	#endif // SUPPORT_VERBOSITY
 
-	//make space
-	current_position[Z_AXIS] += 150;
-	go_to_current(homing_feedrate[Z_AXIS] / 60);
-	//plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate, active_extruder););
-
-	lcd_show_fullscreen_message_and_wait_P(MSG_PLACE_STEEL_SHEET);
-
-    // Sample Z heights for the mesh bed leveling.
-    // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
-    if (! sample_mesh_and_store_reference())
+	if(!sample_z())
         goto canceled;
 
     enable_endstops(endstops_enabled);
@@ -2323,6 +2586,22 @@ canceled:
     return result;
 }
 
+bool sample_z() {
+	bool sampled = true;
+	//make space
+	current_position[Z_AXIS] += 150;
+	go_to_current(homing_feedrate[Z_AXIS] / 60);
+	//plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate, active_extruder););
+
+	lcd_show_fullscreen_message_and_wait_P(MSG_PLACE_STEEL_SHEET);
+
+	// Sample Z heights for the mesh bed leveling.
+	// In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
+	if (!sample_mesh_and_store_reference()) sampled = false;
+
+	return sampled;
+}
+
 void go_home_with_z_lift()
 {
     // Don't let the manage_inactivity() function remove power from the motors.
@@ -2508,7 +2787,7 @@ bool scan_bed_induction_points(int8_t verbosity_level)
             current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
         go_to_current(homing_feedrate[X_AXIS]/60);
         find_bed_induction_sensor_point_z();
-        scan_bed_induction_sensor_point();
+		scan_bed_induction_sensor_point();
     }
     // Don't let the manage_inactivity() function remove power from the motors.
     refresh_cmd_timeout();
diff --git a/Firmware/mesh_bed_calibration.h b/Firmware/mesh_bed_calibration.h
index 4f6ebd724..5f5a98aa2 100644
--- a/Firmware/mesh_bed_calibration.h
+++ b/Firmware/mesh_bed_calibration.h
@@ -187,5 +187,6 @@ extern void babystep_undo();
 // Reset the current babystep counter without moving the axes.
 extern void babystep_reset();
 extern void count_xyz_details();
+extern bool sample_z();
 
 #endif /* MESH_BED_CALIBRATION_H */
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 8510daf9b..28aee746e 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -98,6 +98,7 @@ static bool old_z_max_endstop=false;
 static bool check_endstops = true;
 
 static bool check_z_endstop = false;
+static bool z_endstop_invert = false;
 
 int8_t SilentMode = 0;
 
@@ -282,10 +283,15 @@ bool enable_endstops(bool check)
 
 bool enable_z_endstop(bool check)
 {
-  bool old = check_z_endstop;
-  check_z_endstop = check;
-  endstop_z_hit=false;
-  return old;
+	bool old = check_z_endstop;
+	check_z_endstop = check;
+	endstop_z_hit = false;
+	return old;
+}
+
+void invert_z_endstop(bool endstop_invert)
+{
+  z_endstop_invert = endstop_invert;
 }
 
 //         __________________________
@@ -603,9 +609,9 @@ void isr() {
         // Good for searching for the center of an induction target.
             #ifdef TMC2130_SG_HOMING
             // Stall guard homing turned on
-                z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0);
+                z_min_endstop = (READ(Z_MIN_PIN) != z_endstop_invert) || (READ(Z_TMC2130_DIAG) != 0);
             #else
-				z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
+                z_min_endstop = (READ(Z_MIN_PIN) != z_endstop_invert);
             #endif //TMC2130_SG_HOMING
         if(z_min_endstop && old_z_min_endstop) {
           endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
diff --git a/Firmware/stepper.h b/Firmware/stepper.h
index 2fdec2d6c..3c1e6ffed 100644
--- a/Firmware/stepper.h
+++ b/Firmware/stepper.h
@@ -93,6 +93,7 @@ bool endstop_z_hit_on_purpose();
 
 bool enable_endstops(bool check); // Enable/disable endstop checking. Return the old value.
 bool enable_z_endstop(bool check);
+void invert_z_endstop(bool endstop_invert);
 
 void checkStepperErrors(); //Print errors detected by the stepper