diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 4f22e13e2ec0..b0c024c0edba 100755
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -22,7 +22,6 @@
 #include "pins.h"
 #include "Timer.h"
 #include "mmu2.h"
-extern uint8_t mbl_z_probe_nr;
 
 #ifndef AT90USB
 #define  HardwareSerial_h // trick to disable the standard HWserial
@@ -236,7 +235,7 @@ enum class HeatingStatus : uint8_t
 extern HeatingStatus heating_status;
 
 extern bool fans_check_enabled;
-extern float homing_feedrate[];
+constexpr float homing_feedrate[] = HOMING_FEEDRATE;
 extern uint8_t axis_relative_modes;
 extern float feedrate;
 extern int feedmultiply;
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 4eb76c660804..7c362a3d1df5 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -148,8 +148,6 @@
 CardReader card;
 #endif
 
-uint8_t mbl_z_probe_nr = 3; //numer of Z measurements for each point in mesh bed leveling calibration
-
 //used for PINDA temp calibration and pause print
 #define DEFAULT_RETRACTION    1
 #define DEFAULT_RETRACTION_MM 4 //MM
@@ -157,8 +155,6 @@ uint8_t mbl_z_probe_nr = 3; //numer of Z measurements for each point in mesh bed
 float default_retraction = DEFAULT_RETRACTION;
 
 
-float homing_feedrate[] = HOMING_FEEDRATE;
-
 //Although this flag and many others like this could be represented with a struct/bitfield for each axis (more readable and efficient code), the implementation
 //would not be standard across all platforms. That being said, the code will continue to use bitmasks for independent axis.
 //Moreover, according to C/C++ standard, the ordering of bits is platform/compiler dependent and the compiler is allowed to align the bits arbitrarily,
@@ -2791,23 +2787,16 @@ static void gcode_G28(bool home_x_axis, bool home_y_axis, bool home_z_axis)
 // G80 - Automatic mesh bed leveling
 static void gcode_G80()
 {
+    constexpr float XY_AXIS_FEEDRATE = (homing_feedrate[X_AXIS] * 3) / 60;
+    constexpr float Z_LIFT_FEEDRATE = homing_feedrate[Z_AXIS] / 60;
+    constexpr float Z_CALIBRATION_THRESHOLD = 0.35f;
+    constexpr float MESH_HOME_Z_SEARCH_FAST = 0.35f;
     st_synchronize();
     if (planner_aborted)
         return;
 
     mesh_bed_leveling_flag = true;
-#ifndef PINDA_THERMISTOR
-    static bool run = false; // thermistor-less PINDA temperature compensation is running
-#endif // ndef PINDA_THERMISTOR
 
-#ifdef SUPPORT_VERBOSITY
-    int8_t verbosity_level = 0;
-    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();
-    }
-#endif //SUPPORT_VERBOSITY
     // 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!
@@ -2818,30 +2807,8 @@ static void gcode_G80()
         return;
     }
 
-    uint8_t nMeasPoints = MESH_MEAS_NUM_X_POINTS;
-    if (code_seen('N')) {
-        nMeasPoints = code_value_uint8();
-        if (nMeasPoints != 7) {
-            nMeasPoints = 3;
-        }
-    }
-    else {
-        nMeasPoints = eeprom_read_byte((uint8_t*)EEPROM_MBL_POINTS_NR);
-    }
-
-    uint8_t nProbeRetry = 3;
-    if (code_seen('R')) {
-        nProbeRetry = code_value_uint8();
-        if (nProbeRetry > 10) {
-            nProbeRetry = 10;
-        }
-    }
-    else {
-        nProbeRetry = eeprom_read_byte((uint8_t*)EEPROM_MBL_PROBE_NR);
-    }
-    bool magnet_elimination = (eeprom_read_byte((uint8_t*)EEPROM_MBL_MAGNET_ELIMINATION) > 0);
-
 #ifndef PINDA_THERMISTOR
+    static bool run = false; // thermistor-less PINDA temperature compensation is running
     if (run == false && eeprom_read_byte((uint8_t *)EEPROM_TEMP_CAL_ACTIVE) && calibration_status_pinda() == true && target_temperature_bed >= 50)
     {
         temp_compensation_start();
@@ -2852,113 +2819,110 @@ static void gcode_G80()
     }
     run = false;
 #endif //PINDA_THERMISTOR
-    // Save custom message state, set a new custom message state to display: Calibrating point 9.
-    CustomMsg custom_message_type_old = custom_message_type;
-    uint8_t custom_message_state_old = custom_message_state;
-    custom_message_type = CustomMsg::MeshBedLeveling;
-    custom_message_state = (nMeasPoints * nMeasPoints) + 10;
-    lcd_update(1);
+
+    uint8_t nMeasPoints = code_seen('N') ? code_value_uint8() : eeprom_read_byte((uint8_t*)EEPROM_MBL_POINTS_NR);
+    if (nMeasPoints != 7) {
+        nMeasPoints = 3;
+    }
+
+    uint8_t nProbeRetry = code_seen('R') ? code_value_uint8() : eeprom_read_byte((uint8_t*)EEPROM_MBL_PROBE_NR);
+    if (nProbeRetry > 10) {
+        nProbeRetry = 10;
+    }
+
+    const float area_min_x = code_seen('X') ? code_value() - x_mesh_density - X_PROBE_OFFSET_FROM_EXTRUDER : -INFINITY;
+    const float area_min_y = code_seen('Y') ? code_value() - y_mesh_density - Y_PROBE_OFFSET_FROM_EXTRUDER : -INFINITY;
+    const float area_max_x = code_seen('W') ? area_min_x + code_value() + 2 * x_mesh_density : INFINITY;
+    const float area_max_y = code_seen('H') ? area_min_y + code_value() + 2 * y_mesh_density : INFINITY;
 
     mbl.reset(); //reset mesh bed leveling
+    mbl.z_values[0][0] = min_pos[Z_AXIS];
 
     // Reset baby stepping to zero, if the babystepping has already been loaded before.
     babystep_undo();
 
-    // 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_curposXYZE(homing_feedrate[Z_AXIS] / 60);
-    // The move to the first calibration point.
-    current_position[X_AXIS] = BED_X0;
-    current_position[Y_AXIS] = BED_Y0;
+    // Initialize the default mesh from eeprom and calculate how many points are to be probed
+    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)
+    uint8_t meshPointsToProbe = 0;
+    for (uint8_t row = 0; row < MESH_NUM_Y_POINTS; row++) {
+        for (uint8_t col = 0; col < MESH_NUM_X_POINTS; col++) {
+            bool isOn3x3Mesh = ((row % 3 == 0) && (col % 3 == 0));
+            if (isOn3x3Mesh) {
+                if (has_z && (row || col)) {
+                    // Reconstruct the mesh saved in eeprom
+                    uint16_t z_offset_u = eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER + 2 * ((col/3) + row - 1)));
+                    const float z0 = mbl.z_values[0][0] + *reinterpret_cast<int16_t*>(&z_offset_u) * 0.01;
+                    mbl.set_z(col, row, z0);
+                }
+            } else {
+                mbl.set_z(col, row, NAN);
+            }
 
-#ifdef SUPPORT_VERBOSITY
-    if (verbosity_level >= 1)
-    {
-        bool clamped = world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-        clamped ? SERIAL_PROTOCOLPGM("First calibration point clamped.\n") : SERIAL_PROTOCOLPGM("No clamping for first calibration point.\n");
-    }
-#else //SUPPORT_VERBOSITY
-    world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-#endif //SUPPORT_VERBOSITY
+            // check for points that are skipped
+            if (nMeasPoints == 3) {
+                if (!isOn3x3Mesh)
+                    continue;
+            } else {
+                const float x_pos = BED_X(col);
+                const float y_pos = BED_Y(row);
+                if ((x_pos < area_min_x || x_pos > area_max_x || y_pos < area_min_y || y_pos > area_max_y) && (!isOn3x3Mesh || has_z)) {
+                    continue;
+                }
+            }
 
-    int XY_AXIS_FEEDRATE = homing_feedrate[X_AXIS] / 20;
-    plan_buffer_line_curposXYZE(XY_AXIS_FEEDRATE);
-    // Wait until the move is finished.
-    st_synchronize();
-    if (planner_aborted)
-    {
-        custom_message_type = custom_message_type_old;
-        custom_message_state = custom_message_state_old;
-        return;
+            // increment the total point counter if the points are not skipped
+            meshPointsToProbe++;
+        }
     }
+    mbl.upsample_3x3(); //upsample the default mesh
 
-    uint8_t mesh_point = 0; //index number of calibration point
-    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)
-#ifdef SUPPORT_VERBOSITY
-    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");
-    }
-#endif // SUPPORT_VERBOSITY
+    // Save custom message state, set a new custom message state to display: Calibrating point 9.
+    CustomMsg custom_message_type_old = custom_message_type;
+    uint8_t custom_message_state_old = custom_message_state;
+    custom_message_type = CustomMsg::MeshBedLeveling;
+    custom_message_state = meshPointsToProbe + 10;
+    lcd_update(1);
+
+    // Lift Z to a safe position before probing the first point
+    current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+    plan_buffer_line_curposXYZE(Z_LIFT_FEEDRATE);
+
+    // Cycle through all points and probe them
     int l_feedmultiply = setup_for_endstop_move(false); //save feedrate and feedmultiply, sets feedmultiply to 100
-    while (mesh_point != nMeasPoints * nMeasPoints) {
+    uint8_t mesh_point = 0; //index number of calibration point
+    while (mesh_point != MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS) {
         // Get coords of a measuring point.
-        uint8_t ix = mesh_point % nMeasPoints; // from 0 to MESH_NUM_X_POINTS - 1
-        uint8_t iy = mesh_point / nMeasPoints;
-        /*if (!mbl_point_measurement_valid(ix, iy, nMeasPoints, true)) {
-          printf_P(PSTR("Skipping point [%d;%d] \n"), ix, iy);
-          custom_message_state--;
-          mesh_point++;
-          continue; //skip
-          }*/
-        if (iy & 1) ix = (nMeasPoints - 1) - ix; // Zig zag
-        if (nMeasPoints == 7) //if we have 7x7 mesh, compare with Z-calibration for points which are in 3x3 mesh
-        {
-            has_z = ((ix % 3 == 0) && (iy % 3 == 0)) && is_bed_z_jitter_data_valid();
-        }
-        float z0 = 0.f;
-        if (has_z && (mesh_point > 0)) {
-            uint16_t z_offset_u = 0;
-            if (nMeasPoints == 7) {
-                z_offset_u = eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER + 2 * ((ix/3) + iy - 1)));
-            }
-            else {
-                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;
-#ifdef SUPPORT_VERBOSITY
-            if (verbosity_level >= 1) {
-                printf_P(PSTR("Bed leveling, point: %d, calibration Z stored in eeprom: %d, calibration z: %f \n"), mesh_point, z_offset_u, z0);
-            }
-#endif // SUPPORT_VERBOSITY
+        uint8_t ix = mesh_point % MESH_NUM_X_POINTS; // from 0 to MESH_NUM_X_POINTS - 1
+        uint8_t iy = mesh_point / MESH_NUM_X_POINTS;
+        if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag
+        bool isOn3x3Mesh = ((ix % 3 == 0) && (iy % 3 == 0));
+        float x_pos = BED_X(ix);
+        float y_pos = BED_Y(iy);
+
+        if ((nMeasPoints == 3) && !isOn3x3Mesh) {
+            mesh_point++;
+            mbl.set_z(ix, iy, NAN);
+            continue; //skip
+        } else if ((x_pos < area_min_x || x_pos > area_max_x || y_pos < area_min_y || y_pos > area_max_y) && (!isOn3x3Mesh || has_z)) {
+            mesh_point++;
+            continue; //skip
         }
 
-        // Move Z up to MESH_HOME_Z_SEARCH.
-        if((ix == 0) && (iy == 0)) current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-        else current_position[Z_AXIS] += 2.f / nMeasPoints; //use relative movement from Z coordinate where PINDa triggered on previous point. This makes calibration faster.
-        float init_z_bckp = current_position[Z_AXIS];
-        plan_buffer_line_curposXYZE(Z_LIFT_FEEDRATE);
-        st_synchronize();
+        // Move Z up to the probe height of the current Z point.
+        const float z0 = mbl.z_values[iy][ix];
+        const float init_z_bckp = !has_z ? MESH_HOME_Z_SEARCH : z0 + MESH_HOME_Z_SEARCH_FAST;
+        if (init_z_bckp > current_position[Z_AXIS]) {
+            current_position[Z_AXIS] = init_z_bckp;
+            plan_buffer_line_curposXYZE(Z_LIFT_FEEDRATE);
+            st_synchronize();
+        }
 
         // Move to XY position of the sensor point.
-        current_position[X_AXIS] = BED_X(ix, nMeasPoints);
-        current_position[Y_AXIS] = BED_Y(iy, nMeasPoints);
-
-        //printf_P(PSTR("[%f;%f]\n"), current_position[X_AXIS], current_position[Y_AXIS]);
+        current_position[X_AXIS] = x_pos;
+        current_position[Y_AXIS] = y_pos;
 
-
-#ifdef SUPPORT_VERBOSITY
-        if (verbosity_level >= 1) {
-            bool clamped = world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-            SERIAL_PROTOCOL(mesh_point);
-            clamped ? SERIAL_PROTOCOLPGM(": xy clamped.\n") : SERIAL_PROTOCOLPGM(": no xy clamping\n");
-        }
-#else //SUPPORT_VERBOSITY
         world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-#endif // SUPPORT_VERBOSITY
 
-        //printf_P(PSTR("after clamping: [%f;%f]\n"), current_position[X_AXIS], current_position[Y_AXIS]);
         plan_buffer_line_curposXYZE(XY_AXIS_FEEDRATE);
         st_synchronize();
         if (planner_aborted)
@@ -2969,72 +2933,48 @@ static void gcode_G80()
         }
 
         // 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, nProbeRetry)) { //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
+        if (!find_bed_induction_sensor_point_z(has_z ? z0 - Z_CALIBRATION_THRESHOLD : -10.f, nProbeRetry)) { //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
             printf_P(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
             break;
         }
-        if (init_z_bckp - current_position[Z_AXIS] < 0.1f) { //broken cable or initial Z coordinate too low. Go to MESH_HOME_Z_SEARCH and repeat last step (z-probe) again to distinguish between these two cases.
-            //printf_P(PSTR("Another attempt! Current Z position: %f\n"), current_position[Z_AXIS]);
+        if (init_z_bckp - current_position[Z_AXIS] < 0.f) { //broken cable or initial Z coordinate too low. Go to MESH_HOME_Z_SEARCH and repeat last step (z-probe) again to distinguish between these two cases.
             current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
             plan_buffer_line_curposXYZE(Z_LIFT_FEEDRATE);
             st_synchronize();
 
-            if (!find_bed_induction_sensor_point_z((has_z && mesh_point > 0) ? z0 - Z_CALIBRATION_THRESHOLD : -10.f, nProbeRetry)) { //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
+            if (!find_bed_induction_sensor_point_z(has_z ? z0 - Z_CALIBRATION_THRESHOLD : -10.f, nProbeRetry)) { //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
                 printf_P(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
                 break;
             }
             if (MESH_HOME_Z_SEARCH - current_position[Z_AXIS] < 0.1f) {
-                puts_P(PSTR("Bed leveling failed. Sensor disconnected or cable broken."));
+                puts_P(PSTR("Bed leveling failed. Sensor triggered too soon"));
                 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
-            puts_P(PSTR("Bed leveling failed. Sensor triggered too high."));
+            puts_P(PSTR("Bed leveling failed. Too much variation from eeprom mesh"));
             break;
         }
-#ifdef SUPPORT_VERBOSITY
-        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");
-        }
-#endif // SUPPORT_VERBOSITY
-        float offset_z = 0;
 
 #ifdef PINDA_THERMISTOR
-        offset_z = temp_compensation_pinda_thermistor_offset(current_temperature_pinda);
-#endif //PINDA_THERMISTOR
-        //			#ifdef SUPPORT_VERBOSITY
-        /*			if (verbosity_level >= 1)
-                    {
-                    SERIAL_ECHOPGM("mesh bed leveling: ");
-                    MYSERIAL.print(current_position[Z_AXIS], 5);
-                    SERIAL_ECHOPGM(" offset: ");
-                    MYSERIAL.print(offset_z, 5);
-                    SERIAL_ECHOLNPGM("");
-                    }*/
-        //			#endif // SUPPORT_VERBOSITY
+        float offset_z = temp_compensation_pinda_thermistor_offset(current_temperature_pinda);
         mbl.set_z(ix, iy, current_position[Z_AXIS] - offset_z); //store measured z values z_values[iy][ix] = z - offset_z;
+#else
+        mbl.set_z(ix, iy, current_position[Z_AXIS]); //store measured z values z_values[iy][ix] = z;
+#endif //PINDA_THERMISTOR
 
         custom_message_state--;
         mesh_point++;
         lcd_update(1);
     }
     current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-#ifdef SUPPORT_VERBOSITY
-    if (verbosity_level >= 20) {
-        SERIAL_ECHOLNPGM("Mesh bed leveling while loop finished.");
-        SERIAL_ECHOLNPGM("MESH_HOME_Z_SEARCH: ");
-        MYSERIAL.print(current_position[Z_AXIS], 5);
-    }
-#endif // SUPPORT_VERBOSITY
     plan_buffer_line_curposXYZE(Z_LIFT_FEEDRATE);
     st_synchronize();
-    if (mesh_point != nMeasPoints * nMeasPoints) {
+    static uint8_t g80_fail_cnt = 0;
+    if (mesh_point != MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS) {
+        if (g80_fail_cnt++ >= 2) {
+            kill(PSTR("Mesh bed leveling failed. Please run Z calibration."));
+        }
         Sound_MakeSound(e_SOUND_TYPE_StandardAlert);
         bool bState;
         do   {                             // repeat until Z-leveling o.k.
@@ -3054,14 +2994,13 @@ static void gcode_G80()
             tmc2130_home_enter(Z_AXIS_MASK);
 #endif // TMC2130
             current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-            plan_buffer_line_curposXYZE(homing_feedrate[Z_AXIS] / 40);
+            plan_buffer_line_curposXYZE(Z_LIFT_FEEDRATE);
             st_synchronize();
 #ifdef TMC2130
             tmc2130_home_exit();
 #endif // TMC2130
             enable_z_endstop(bState);
         } while (st_get_position_mm(Z_AXIS) > MESH_HOME_Z_SEARCH); // i.e. Z-leveling not o.k.
-        //               plan_set_z_position(MESH_HOME_Z_SEARCH); // is not necessary ('do-while' loop always ends at the expected Z-position)
 
         custom_message_type = custom_message_type_old;
         custom_message_state = custom_message_state_old;
@@ -3070,101 +3009,63 @@ static void gcode_G80()
         repeatcommand_front();             // re-run (i.e. of "G80")
         return;
     }
+    g80_fail_cnt = 0; // no fail was detected. Reset the error counter.
+
     clean_up_after_endstop_move(l_feedmultiply);
-    //		SERIAL_ECHOLNPGM("clean up finished ");
 
 #ifndef PINDA_THERMISTOR
     if(eeprom_read_byte((uint8_t *)EEPROM_TEMP_CAL_ACTIVE) && calibration_status_pinda() == true) temp_compensation_apply(); //apply PINDA temperature compensation
 #endif
     babystep_apply(); // Apply Z height correction aka baby stepping before mesh bed leveing gets activated.
-    //		SERIAL_ECHOLNPGM("babystep applied");
+    
+    // Apply the bed level correction to the mesh
     bool eeprom_bed_correction_valid = eeprom_read_byte((unsigned char*)EEPROM_BED_CORRECTION_VALID) == 1;
-#ifdef SUPPORT_VERBOSITY
-    if (verbosity_level >= 1) {
-        eeprom_bed_correction_valid ? SERIAL_PROTOCOLPGM("Bed correction data valid\n") : SERIAL_PROTOCOLPGM("Bed correction data not valid\n");
-    }
-#endif // SUPPORT_VERBOSITY
-    const constexpr uint8_t sides = 4;
-    int8_t correction[sides] = {0};
-    for (uint8_t i = 0; i < sides; ++i) {
-        static const char codes[sides] PROGMEM = { 'L', 'R', 'F', 'B' };
-        static uint8_t *const eep_addresses[sides] PROGMEM = {
-          (uint8_t*)EEPROM_BED_CORRECTION_LEFT,
-          (uint8_t*)EEPROM_BED_CORRECTION_RIGHT,
-          (uint8_t*)EEPROM_BED_CORRECTION_FRONT,
-          (uint8_t*)EEPROM_BED_CORRECTION_REAR,
-        };
-        if (code_seen(pgm_read_byte(&codes[i])))
-        { // Verify value is within allowed range
-            int32_t temp = code_value_long();
-            if (labs(temp) > BED_ADJUSTMENT_UM_MAX) {
-                SERIAL_ERROR_START;
-                SERIAL_ECHOPGM("Excessive bed leveling correction: ");
-                SERIAL_ECHO(temp);
-                SERIAL_ECHOLNPGM(" microns");
-                correction[i] = 0;
+    auto bedCorrectHelper = [eeprom_bed_correction_valid] (char code, uint8_t *eep_address) -> int8_t {
+        if (code_seen(code)) {
+            // Verify value is within allowed range
+            int16_t temp = code_value_short();
+            if (abs(temp) > BED_ADJUSTMENT_UM_MAX) {
+                printf_P(PSTR("%SExcessive bed leveling correction: %i microns\n"), errormagic, temp);
             } else {
-              // Value is valid, save it
-              correction[i] = (int8_t)temp;
+                return (int8_t)temp; // Value is valid, use it
             }
-        } else if (eeprom_bed_correction_valid)
-            correction[i] = (int8_t)eeprom_read_byte((uint8_t*)pgm_read_ptr(&eep_addresses[i]));
-        if (correction[i] == 0)
-            continue;
-    }
-    for (uint8_t row = 0; row < nMeasPoints; ++row) {
-        for (uint8_t col = 0; col < nMeasPoints; ++col) {
-            mbl.z_values[row][col] +=0.001f * (
-              + correction[0] * (nMeasPoints - 1 - col)
+        } else if (eeprom_bed_correction_valid) {
+            return (int8_t)eeprom_read_byte(eep_address);
+        }
+        return 0;
+    };
+    const int8_t correction[4] = {
+        bedCorrectHelper('L', (uint8_t*)EEPROM_BED_CORRECTION_LEFT),
+        bedCorrectHelper('R', (uint8_t*)EEPROM_BED_CORRECTION_RIGHT),
+        bedCorrectHelper('F', (uint8_t*)EEPROM_BED_CORRECTION_FRONT),
+        bedCorrectHelper('B', (uint8_t*)EEPROM_BED_CORRECTION_REAR),
+    };
+    for (uint8_t row = 0; row < MESH_NUM_Y_POINTS; row++) {
+        for (uint8_t col = 0; col < MESH_NUM_X_POINTS; col++) {
+            constexpr float scaler = 0.001f / (MESH_NUM_X_POINTS - 1);
+            mbl.z_values[row][col] += scaler * (
+              + correction[0] * (MESH_NUM_X_POINTS - 1 - col)
               + correction[1] * col
-              + correction[2] * (nMeasPoints - 1 - row)
-              + correction[3] * row) / (float)(nMeasPoints - 1);
+              + correction[2] * (MESH_NUM_Y_POINTS - 1 - row)
+              + correction[3] * row);
         }
     }
-    //		SERIAL_ECHOLNPGM("Bed leveling correction finished");
-    if (nMeasPoints == 3) {
-        mbl.upsample_3x3(); //interpolation from 3x3 to 7x7 points using largrangian polynomials while using the same array z_values[iy][ix] for storing (just coppying measured data to new destination and interpolating between them)
-    }
-    /*
-      SERIAL_PROTOCOLPGM("Num X,Y: ");
-      SERIAL_PROTOCOL(MESH_NUM_X_POINTS);
-      SERIAL_PROTOCOLPGM(",");
-      SERIAL_PROTOCOL(MESH_NUM_Y_POINTS);
-      SERIAL_PROTOCOLPGM("\nZ search height: ");
-      SERIAL_PROTOCOL(MESH_HOME_Z_SEARCH);
-      SERIAL_PROTOCOLLNPGM("\nMeasured points:");
-      for (int y = MESH_NUM_Y_POINTS-1; y >= 0; y--) {
-      for (int x = 0; x < MESH_NUM_X_POINTS; x++) {
-      SERIAL_PROTOCOLPGM("  ");
-      SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
-      }
-      SERIAL_PROTOCOLPGM("\n");
-      }
-    */
-    if (nMeasPoints == 7 && magnet_elimination) {
-        mbl_interpolation(nMeasPoints);
-    }
-    /*
-      SERIAL_PROTOCOLPGM("Num X,Y: ");
-      SERIAL_PROTOCOL(MESH_NUM_X_POINTS);
-      SERIAL_PROTOCOLPGM(",");
-      SERIAL_PROTOCOL(MESH_NUM_Y_POINTS);
-      SERIAL_PROTOCOLPGM("\nZ search height: ");
-      SERIAL_PROTOCOL(MESH_HOME_Z_SEARCH);
-      SERIAL_PROTOCOLLNPGM("\nMeasured points:");
-      for (int y = MESH_NUM_Y_POINTS-1; y >= 0; y--) {
-      for (int x = 0; x < MESH_NUM_X_POINTS; x++) {
-      SERIAL_PROTOCOLPGM("  ");
-      SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
-      }
-      SERIAL_PROTOCOLPGM("\n");
-      }
-    */
-    //		SERIAL_ECHOLNPGM("Upsample finished");
+
+    mbl.upsample_3x3(); //interpolation from 3x3 to 7x7 points using largrangian polynomials while using the same array z_values[iy][ix] for storing (just coppying measured data to new destination and interpolating between them)
+
+    if (nMeasPoints == 7 && eeprom_read_byte((uint8_t*)EEPROM_MBL_MAGNET_ELIMINATION)) {
+        mbl_magnet_elimination();
+    }
+
     mbl.active = 1; //activate mesh bed leveling
-    //		SERIAL_ECHOLNPGM("Mesh bed leveling activated");
-    go_home_with_z_lift();
-    //		SERIAL_ECHOLNPGM("Go home finished");
+
+    if (code_seen('O') && !code_value_uint8()) {
+        // Don't let the manage_inactivity() function remove power from the motors.
+        refresh_cmd_timeout();
+    } else {
+        go_home_with_z_lift();
+    }
+
 #ifndef PINDA_THERMISTOR
     //unretract (after PINDA preheat retraction)
     if (temp_compensation_retracted) {
@@ -4981,12 +4882,12 @@ void process_commands()
     Default 3x3 grid can be changed on MK2.5/s and MK3/s to 7x7 grid.
     #### Usage
 	  
-          G80 [ N | R | V | L | R | F | B ]
+          G80 [ N | R | L | R | F | B | X | Y | W | H ]
       
 	#### Parameters
       - `N` - Number of mesh points on x axis. Default is 3. Valid values are 3 and 7.
       - `R` - Probe retries. Default 3 max. 10
-      - `V` - Verbosity level 1=low, 10=mid, 20=high. It only can be used if the firmware has been compiled with SUPPORT_VERBOSITY active.
+      - `O` - Return to origin. Default 1 (true)
       
       Using the following parameters enables additional "manual" bed leveling correction. Valid values are -100 microns to 100 microns.
     #### Additional Parameters
@@ -4994,16 +4895,13 @@ void process_commands()
       - `R` - Right Bed Level correct value in um.
       - `F` - Front Bed Level correct value in um.
       - `B` - Back Bed Level correct value in um.
+
+      The following parameters are used to define the area used by the print:
+      - `X` - area lower left point X coordinate
+      - `Y` - area lower left point Y coordinate
+      - `W` - area width (on X axis)
+      - `H` - area height (on Y axis)
     */
-  
-	/*
-    * Probes a grid and produces a mesh to compensate for variable bed height
-	* The S0 report the points as below
-	*  +----> X-axis
-	*  |
-	*  |
-	*  v Y-axis
-	*/
 
 	case 80: {
         gcode_G80();
@@ -5016,20 +4914,7 @@ void process_commands()
         */
         case 81:
             if (mbl.active) {
-                SERIAL_PROTOCOLPGM("Num X,Y: ");
-                SERIAL_PROTOCOL(MESH_NUM_X_POINTS);
-                SERIAL_PROTOCOL(',');
-                SERIAL_PROTOCOL(MESH_NUM_Y_POINTS);
-                SERIAL_PROTOCOLPGM("\nZ search height: ");
-                SERIAL_PROTOCOL(MESH_HOME_Z_SEARCH);
-                SERIAL_PROTOCOLLNPGM("\nMeasured points:");
-                for (uint8_t y = MESH_NUM_Y_POINTS; y-- > 0;) {
-                    for (uint8_t x = 0; x < MESH_NUM_X_POINTS; x++) {
-                        SERIAL_PROTOCOLPGM("  ");
-                        SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
-                    }
-                    SERIAL_PROTOCOLLN();
-                }
+                mbl.print();
             }
             else
                 SERIAL_PROTOCOLLNPGM("Mesh bed leveling not active.");
diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp
index 853e88a03e02..ca444fcbe610 100644
--- a/Firmware/mesh_bed_calibration.cpp
+++ b/Firmware/mesh_bed_calibration.cpp
@@ -685,11 +685,11 @@ bool is_bed_z_jitter_data_valid()
 // offsets of the Z heiths of the calibration points from the first point are saved as 16bit signed int, scaled to tenths of microns
 // if at least one 16bit integer has different value then -1 (0x0FFFF), data are considered valid and function returns true, otherwise it returns false
 {	
-	bool data_valid = false;
 	for (int8_t i = 0; i < 8; ++i) {
-		if (eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER + i * 2)) != 0x0FFFF) data_valid = true;
+		if (eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER + i * 2)) != 0x0FFFF)
+            return true;
 	}
-    return data_valid;
+    return false;
 }
 
 static void world2machine_update(const float vec_x[2], const float vec_y[2], const float cntr[2])
@@ -776,7 +776,7 @@ void world2machine_revert_to_uncorrected()
 static inline bool vec_undef(const float v[2])
 {
     const uint32_t *vx = (const uint32_t*)v;
-    return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
+    return vx[0] == 0xFFFFFFFF || vx[1] == 0xFFFFFFFF;
 }
 
 
@@ -2184,6 +2184,16 @@ inline void scan_bed_induction_sensor_point()
 
 #define MESH_BED_CALIBRATION_SHOW_LCD
 
+float __attribute__((noinline)) BED_X(const uint8_t col)
+{
+    return ((float)col * x_mesh_density + BED_X0);
+}
+
+float __attribute__((noinline)) BED_Y(const uint8_t row)
+{
+    return ((float)row * y_mesh_density + BED_Y0);
+}
+
 BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask)
 {	
     // Don't let the manage_inactivity() function remove power from the motors.
@@ -2481,8 +2491,8 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
 					uint8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS; // from 0 to MESH_NUM_X_POINTS - 1
 					uint8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
 					if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix;
-					current_position[X_AXIS] = BED_X(ix, MESH_MEAS_NUM_X_POINTS);
-					current_position[Y_AXIS] = BED_Y(iy, MESH_MEAS_NUM_Y_POINTS);
+					current_position[X_AXIS] = BED_X(ix * 3);
+					current_position[Y_AXIS] = BED_Y(iy * 3);
 					go_to_current(homing_feedrate[X_AXIS] / 60);
 					delay_keep_alive(3000);
 				}
@@ -2820,16 +2830,16 @@ void go_home_with_z_lift()
     // Go home.
     // First move up to a safe height.
     current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-    go_to_current(homing_feedrate[Z_AXIS]/60);
+    go_to_current(homing_feedrate[Z_AXIS] / 60);
     // Second move to XY [0, 0].
-    current_position[X_AXIS] = X_MIN_POS+0.2;
-    current_position[Y_AXIS] = Y_MIN_POS+0.2;
+    current_position[X_AXIS] = X_MIN_POS + 0.2;
+    current_position[Y_AXIS] = Y_MIN_POS + 0.2;
     // Clamp to the physical coordinates.
     world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-	go_to_current(homing_feedrate[X_AXIS]/20);
+    go_to_current((3 * homing_feedrate[X_AXIS]) / 60);
     // Third move up to a safe height.
     current_position[Z_AXIS] = Z_MIN_POS;
-    go_to_current(homing_feedrate[Z_AXIS]/60);    
+    go_to_current(homing_feedrate[Z_AXIS] / 60);
 }
 
 // Sample the 9 points of the bed and store them into the EEPROM as a reference.
@@ -2890,8 +2900,8 @@ bool sample_mesh_and_store_reference()
 		uint8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
 		uint8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
 		if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
-		current_position[X_AXIS] = BED_X(ix, MESH_MEAS_NUM_X_POINTS);
-		current_position[Y_AXIS] = BED_Y(iy, MESH_MEAS_NUM_Y_POINTS);
+		current_position[X_AXIS] = BED_X(ix * 3);
+		current_position[Y_AXIS] = BED_Y(iy * 3);
         world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
         go_to_current(homing_feedrate[X_AXIS]/60);
 #ifdef MESH_BED_CALIBRATION_SHOW_LCD
@@ -2957,8 +2967,7 @@ bool sample_mesh_and_store_reference()
             }
     }
 
-    mbl.upsample_3x3();
-    mbl.active = true;
+    mbl.reset();
 
     go_home_with_z_lift();
 
@@ -3010,8 +3019,8 @@ bool scan_bed_induction_points(int8_t verbosity_level)
 		uint8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS; // from 0 to MESH_NUM_X_POINTS - 1
 		uint8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
 		if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix;
-		float bedX = BED_X(ix, MESH_MEAS_NUM_X_POINTS);
-		float bedY = BED_Y(iy, MESH_MEAS_NUM_Y_POINTS);
+		float bedX = BED_X(ix * 3);
+		float bedY = BED_Y(iy * 3);
         current_position[X_AXIS] = vec_x[0] * bedX + vec_y[0] * bedY + cntr[0];
         current_position[Y_AXIS] = vec_x[1] * bedX + vec_y[1] * bedY + cntr[1];
         // The calibration points are very close to the min Y.
@@ -3040,9 +3049,12 @@ bool scan_bed_induction_points(int8_t verbosity_level)
 // To replace loading of the babystep correction.
 static void shift_z(float delta)
 {
-    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] - delta, current_position[E_AXIS], homing_feedrate[Z_AXIS]/40);
+    const float curpos_z = current_position[Z_AXIS];
+    current_position[Z_AXIS] -= delta;
+    plan_buffer_line_curposXYZE(homing_feedrate[Z_AXIS] / 60);
     st_synchronize();
-    plan_set_z_position(current_position[Z_AXIS]);
+    current_position[Z_AXIS] = curpos_z;
+    plan_set_z_position(curpos_z);
 }
 
 // Number of baby steps applied
@@ -3118,20 +3130,19 @@ void mbl_settings_init() {
 //magnet elimination: use aaproximate Z-coordinate instead of measured values for points which are near magnets
 	eeprom_init_default_byte((uint8_t*)EEPROM_MBL_MAGNET_ELIMINATION, 1);
 	eeprom_init_default_byte((uint8_t*)EEPROM_MBL_POINTS_NR, 3);
-	mbl_z_probe_nr = eeprom_init_default_byte((uint8_t*)EEPROM_MBL_PROBE_NR, 3);
+	eeprom_init_default_byte((uint8_t*)EEPROM_MBL_PROBE_NR, 3);
 }
 
 //parameter ix: index of mesh bed leveling point in X-axis (for meas_points == 7 is valid range from 0 to 6; for meas_points == 3 is valid range from 0 to 2 )  
 //parameter iy: index of mesh bed leveling point in Y-axis (for meas_points == 7 is valid range from 0 to 6; for meas_points == 3 is valid range from 0 to 2 ) 
-//parameter meas_points: number of mesh bed leveling points in one axis; currently designed and tested for values 3 and 7
-//parameter zigzag: false if ix is considered 0 on left side of bed and ix rises with rising X coordinate; true if ix is considered 0 on the right side of heatbed for odd iy values (zig zag mesh bed leveling movements)  
 //function returns true if point is considered valid (typicaly in safe distance from magnet or another object which inflences PINDA measurements)
-bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy, uint8_t meas_points, bool zigzag) {
+bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy) {
 	    //"human readable" heatbed plan
 		//magnet proximity influence Z coordinate measurements significantly (40 - 100 um)
 		//0 - measurement point is above magnet and Z coordinate can be influenced negatively
 		//1 - we should be in safe distance from magnets, measurement should be accurate
-		if ((ix >= meas_points) || (iy >= meas_points)) return false;
+		if ((ix >= MESH_NUM_X_POINTS) || (iy >= MESH_NUM_Y_POINTS))
+            return false;
 
 		uint8_t valid_points_mask[7] = {
 					//[X_MAX,Y_MAX]
@@ -3145,36 +3156,26 @@ bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy, uint8_t meas_points, bo
 			0b1111111,//0
 		//[0,0]
 		};
-		if (meas_points == 3) {
-			ix *= 3;
-			iy *= 3;
-		}
-		if (zigzag) {
-			if ((iy % 2) == 0)	return (valid_points_mask[6 - iy] & (1 << (6 - ix)));
-			else return (valid_points_mask[6 - iy] & (1 << ix));
-		}
-		else {
-			return (valid_points_mask[6 - iy] & (1 << (6 - ix)));
-		}
+        return (valid_points_mask[6 - iy] & (1 << (6 - ix)));
 }
 
-void mbl_single_point_interpolation(uint8_t x, uint8_t y, uint8_t meas_points) {
+void mbl_single_point_interpolation(uint8_t x, uint8_t y) {
 	//printf_P(PSTR("x = %d; y = %d \n"), x, y);
 		uint8_t count = 0;
 		float z = 0;
-		if (mbl_point_measurement_valid(x, y + 1, meas_points, false)) { z += mbl.z_values[y + 1][x]; /*printf_P(PSTR("x; y+1: Z = %f \n"), mbl.z_values[y + 1][x]);*/ count++; }
-		if (mbl_point_measurement_valid(x, y - 1, meas_points, false)) { z += mbl.z_values[y - 1][x]; /*printf_P(PSTR("x; y-1: Z = %f \n"), mbl.z_values[y - 1][x]);*/ count++; }
-		if (mbl_point_measurement_valid(x + 1, y, meas_points, false)) { z += mbl.z_values[y][x + 1]; /*printf_P(PSTR("x+1; y: Z = %f \n"), mbl.z_values[y][x + 1]);*/ count++; }
-		if (mbl_point_measurement_valid(x - 1, y, meas_points, false)) { z += mbl.z_values[y][x - 1]; /*printf_P(PSTR("x-1; y: Z = %f \n"), mbl.z_values[y][x - 1]);*/ count++; }
+		if (mbl_point_measurement_valid(x, y + 1)) { z += mbl.z_values[y + 1][x]; /*printf_P(PSTR("x; y+1: Z = %f \n"), mbl.z_values[y + 1][x]);*/ count++; }
+		if (mbl_point_measurement_valid(x, y - 1)) { z += mbl.z_values[y - 1][x]; /*printf_P(PSTR("x; y-1: Z = %f \n"), mbl.z_values[y - 1][x]);*/ count++; }
+		if (mbl_point_measurement_valid(x + 1, y)) { z += mbl.z_values[y][x + 1]; /*printf_P(PSTR("x+1; y: Z = %f \n"), mbl.z_values[y][x + 1]);*/ count++; }
+		if (mbl_point_measurement_valid(x - 1, y)) { z += mbl.z_values[y][x - 1]; /*printf_P(PSTR("x-1; y: Z = %f \n"), mbl.z_values[y][x - 1]);*/ count++; }
 		if(count != 0) mbl.z_values[y][x] = z / count; //if we have at least one valid point in surrounding area use average value, otherwise use inaccurately measured Z-coordinate
 		//printf_P(PSTR("result: Z = %f \n\n"), mbl.z_values[y][x]);
 }
 
-void mbl_interpolation(uint8_t meas_points) {
-	for (uint8_t x = 0; x < meas_points; x++) {
-		for (uint8_t y = 0; y < meas_points; y++) {
-			if (!mbl_point_measurement_valid(x, y, meas_points, false)) {
-				mbl_single_point_interpolation(x, y, meas_points);
+void mbl_magnet_elimination() {
+    for (uint8_t y = 0; y < MESH_NUM_Y_POINTS; y++) {
+		for (uint8_t x = 0; x < MESH_NUM_X_POINTS; x++) {
+			if (!mbl_point_measurement_valid(x, y)) {
+				mbl_single_point_interpolation(x, y);
 			}
 		}
 	}
diff --git a/Firmware/mesh_bed_calibration.h b/Firmware/mesh_bed_calibration.h
index fcfa9527b342..651eaba71974 100644
--- a/Firmware/mesh_bed_calibration.h
+++ b/Firmware/mesh_bed_calibration.h
@@ -21,8 +21,8 @@
 
 #endif //not HEATBED_V2
 
-#define BED_X(i, n) ((float)i * (BED_Xn - BED_X0) / (n - 1) + BED_X0)
-#define BED_Y(i, n)  ((float)i * (BED_Yn - BED_Y0) / (n - 1) + BED_Y0)
+constexpr float x_mesh_density = (BED_Xn - BED_X0) / (MESH_NUM_X_POINTS - 1);
+constexpr float y_mesh_density = (BED_Yn - BED_Y0) / (MESH_NUM_Y_POINTS - 1);
 
 // Exact positions of the print head above the bed reference points, in the world coordinates.
 // The world coordinates match the machine coordinates only in case, when the machine
@@ -145,6 +145,17 @@ inline bool world2machine_clamp(float &x, float &y)
         machine2world(tmpx, tmpy, x, y);
     return clamped;
 }
+
+/// @brief For a given column on the mesh calculate the bed X coordinate
+/// @param col column index on mesh
+/// @return Bed X coordinate
+float BED_X(const uint8_t col);
+
+/// @brief For a given row on the mesh calculate the bed Y coordinate
+/// @param row row index on mesh
+/// @return Bed Y coordinate
+float BED_Y(const uint8_t row);
+
 /**
  * @brief Bed skew and offest detection result
  *
@@ -203,6 +214,5 @@ extern void count_xyz_details(float (&distanceMin)[2]);
 extern bool sample_z();
 
 extern void mbl_settings_init();
-
-extern bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy, uint8_t meas_points, bool zigzag);
-extern void mbl_interpolation(uint8_t meas_points);
+extern bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy);
+extern void mbl_magnet_elimination();
diff --git a/Firmware/mesh_bed_leveling.cpp b/Firmware/mesh_bed_leveling.cpp
index aa969340c3a5..9ffcbb59d6f6 100644
--- a/Firmware/mesh_bed_leveling.cpp
+++ b/Firmware/mesh_bed_leveling.cpp
@@ -6,23 +6,44 @@
 
 mesh_bed_leveling mbl;
 
-mesh_bed_leveling::mesh_bed_leveling() { reset(); }
-
 void mesh_bed_leveling::reset() {
     active = 0;
-    memset(z_values, 0, sizeof(float) * MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS);
+    memset(z_values, 0, sizeof(z_values));
 }
 
-static inline bool vec_undef(const float v[2])
-{
-    const uint32_t *vx = (const uint32_t*)v;
-    return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
-}
+float mesh_bed_leveling::get_z(float x, float y) {
+    int   i, j;
+    float s, t;
+    
+    i = int(floor((x - (BED_X0 + X_PROBE_OFFSET_FROM_EXTRUDER)) / x_mesh_density));
+    if (i < 0) {
+        i = 0;
+        s = (x - (BED_X0 + X_PROBE_OFFSET_FROM_EXTRUDER)) / x_mesh_density;
+    } else {
+        if (i > MESH_NUM_X_POINTS - 2) {
+            i = MESH_NUM_X_POINTS - 2;
+        }
+        s = (x - get_x(i)) / x_mesh_density;
+    }
 
-#if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1
+    j = int(floor((y - (BED_Y0 + Y_PROBE_OFFSET_FROM_EXTRUDER)) / y_mesh_density));
+    if (j < 0) {
+        j = 0;
+        t = (y - (BED_Y0 + Y_PROBE_OFFSET_FROM_EXTRUDER)) / y_mesh_density;
+    } else {
+        if (j > MESH_NUM_Y_POINTS - 2) {
+            j = MESH_NUM_Y_POINTS - 2;
+        }
+        t = (y - get_y(j)) / y_mesh_density;
+    }
+    
+    float si = 1.f-s;
+    float z0 = si * z_values[j  ][i] + s * z_values[j  ][i+1];
+    float z1 = si * z_values[j+1][i] + s * z_values[j+1][i+1];
+    return (1.f-t) * z0 + t * z1;
+}
 // Works for an odd number of MESH_NUM_X_POINTS and MESH_NUM_Y_POINTS
 
-// #define MBL_BILINEAR
 void mesh_bed_leveling::upsample_3x3()
 {
     int idx0 = 0;
@@ -30,76 +51,53 @@ void mesh_bed_leveling::upsample_3x3()
     int idx2 = MESH_NUM_X_POINTS - 1;
     {
         // First interpolate the points in X axis.
-        static const float x0 = MESH_MIN_X;
-        static const float x1 = 0.5f * float(MESH_MIN_X + MESH_MAX_X);
-        static const float x2 = MESH_MAX_X;
-        for (int j = 0; j < 3; ++ j) {
-            // 1) Copy the source points to their new destination.
-            z_values[j][idx2] = z_values[j][2];
-            z_values[j][idx1] = z_values[j][1];
-            // 2) Interpolate the remaining values by Largrangian polynomials.
-            for (int i = idx0 + 1; i < idx2; ++ i) {
-                if (i == idx1)
+        static const float x0 = (BED_X0 + X_PROBE_OFFSET_FROM_EXTRUDER);
+        static const float x1 = 0.5f * float(BED_X0 + BED_Xn) + X_PROBE_OFFSET_FROM_EXTRUDER;
+        static const float x2 = BED_Xn + X_PROBE_OFFSET_FROM_EXTRUDER;
+        for (int j = 0; j < MESH_NUM_Y_POINTS; ++ j) {
+            // Interpolate the remaining values by Largrangian polynomials.
+            for (int i = 0; i < MESH_NUM_X_POINTS; ++ i) {
+                if (!isnan(z_values[j][i]))
                     continue;
                 float x = get_x(i);
-                #ifdef MBL_BILINEAR
-                z_values[j][i] = (x < x1) ?
-                    ((z_values[j][idx0] * (x - x0) + z_values[j][idx1] * (x1 - x)) / (x1 - x0)) :
-                    ((z_values[j][idx1] * (x - x1) + z_values[j][idx2] * (x2 - x)) / (x2 - x1));
-                #else
                 z_values[j][i] = 
                     z_values[j][idx0] * (x - x1) * (x - x2) / ((x0 - x1) * (x0 - x2)) +
                     z_values[j][idx1] * (x - x0) * (x - x2) / ((x1 - x0) * (x1 - x2)) +
                     z_values[j][idx2] * (x - x0) * (x - x1) / ((x2 - x0) * (x2 - x1));
-                #endif
             }
         }
     }
     {
         // Second interpolate the points in Y axis.
-        static const float y0 = MESH_MIN_Y;
-        static const float y1 = 0.5f * float(MESH_MIN_Y + MESH_MAX_Y);
-        static const float y2 = MESH_MAX_Y;
+        static const float y0 = (BED_Y0 + Y_PROBE_OFFSET_FROM_EXTRUDER);
+        static const float y1 = 0.5f * float(BED_Y0 + BED_Yn) + Y_PROBE_OFFSET_FROM_EXTRUDER;
+        static const float y2 = BED_Yn + Y_PROBE_OFFSET_FROM_EXTRUDER;
         for (int i = 0; i < MESH_NUM_X_POINTS; ++ i) {
-            // 1) Copy the intermediate points to their new destination.
-            z_values[idx2][i] = z_values[2][i];
-            z_values[idx1][i] = z_values[1][i];
-            // 2) Interpolate the remaining values by Largrangian polynomials.
+            // Interpolate the remaining values by Largrangian polynomials.
             for (int j = 1; j + 1 < MESH_NUM_Y_POINTS; ++ j) {
-                if (j == idx1)
+                if (!isnan(z_values[j][i]))
                     continue;
                 float y = get_y(j);
-                #ifdef MBL_BILINEAR
-                z_values[j][i] = (y < y1) ? 
-                    ((z_values[idx0][i] * (y - y0) + z_values[idx1][i] * (y1 - y)) / (y1 - y0)) :
-                    ((z_values[idx1][i] * (y - y1) + z_values[idx2][i] * (y2 - y)) / (y2 - y1));
-                #else
                 z_values[j][i] = 
                     z_values[idx0][i] * (y - y1) * (y - y2) / ((y0 - y1) * (y0 - y2)) +
                     z_values[idx1][i] * (y - y0) * (y - y2) / ((y1 - y0) * (y1 - y2)) +
                     z_values[idx2][i] * (y - y0) * (y - y1) / ((y2 - y0) * (y2 - y1));
-                #endif
             }
         }
     }
+}
 
-/*
-    // Relax the non-measured points.
-    const float weight = 0.2f;
-    for (uint8_t iter = 0; iter < 20; ++ iter) {
-        for (int8_t j = 1; j < 6; ++ j) {
-            for (int8_t i = 1; i < 6; ++ i) {
-                if (i == 3 || j == 3)
-                    continue;
-                if ((i % 3) == 0 && (j % 3) == 0)
-                    continue;
-                float avg = 0.25f * (z_values[j][i-1]+z_values[j][i+1]+z_values[j-1][i]+z_values[j+1][i]);
-                z_values[j][i] = (1.f-weight)*z_values[j][i] + weight*avg;
-            }
+void mesh_bed_leveling::print() {
+    SERIAL_PROTOCOLLNPGM("Num X,Y: " STRINGIFY(MESH_NUM_X_POINTS) "," STRINGIFY(MESH_NUM_Y_POINTS));
+    SERIAL_PROTOCOLLNPGM("Z search height: " STRINGIFY(MESH_HOME_Z_SEARCH));
+    SERIAL_PROTOCOLLNPGM("Measured points:");
+    for (uint8_t y = MESH_NUM_Y_POINTS; y-- > 0;) {
+        for (uint8_t x = 0; x < MESH_NUM_X_POINTS; x++) {
+            SERIAL_PROTOCOLPGM("  ");
+            SERIAL_PROTOCOL_F(z_values[y][x], 5);
         }
+        SERIAL_PROTOCOLLN();
     }
-*/
 }
-#endif
 
 #endif  // MESH_BED_LEVELING
diff --git a/Firmware/mesh_bed_leveling.h b/Firmware/mesh_bed_leveling.h
index 7df04844ac77..7b3313a12b97 100644
--- a/Firmware/mesh_bed_leveling.h
+++ b/Firmware/mesh_bed_leveling.h
@@ -1,118 +1,23 @@
 #include "Marlin.h"
+#include "mesh_bed_calibration.h"
 
 #ifdef MESH_BED_LEVELING
 
-#define MEAS_NUM_X_DIST (float(MESH_MAX_X - MESH_MIN_X)/float(MESH_MEAS_NUM_X_POINTS - 1))
-#define MEAS_NUM_Y_DIST (float(MESH_MAX_Y - MESH_MIN_Y)/float(MESH_MEAS_NUM_Y_POINTS - 1))
-
-#define MESH_X_DIST (float(MESH_MAX_X - MESH_MIN_X)/float(MESH_NUM_X_POINTS - 1))
-#define MESH_Y_DIST (float(MESH_MAX_Y - MESH_MIN_Y)/float(MESH_NUM_Y_POINTS - 1))
-
 class mesh_bed_leveling {
 public:
     uint8_t active;
     float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
     
-    mesh_bed_leveling();
+    mesh_bed_leveling() { reset(); }
     
     void reset();
-    
-#if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1
-    void upsample_3x3();
-#endif
-    
-    static float get_x(int i) { return float(MESH_MIN_X) + float(MESH_X_DIST) * float(i); }
-    static float get_y(int i) { return float(MESH_MIN_Y) + float(MESH_Y_DIST) * float(i); }
-    
+
+    static float get_x(int i) { return BED_X(i) + X_PROBE_OFFSET_FROM_EXTRUDER; }
+    static float get_y(int i) { return BED_Y(i) + Y_PROBE_OFFSET_FROM_EXTRUDER; }
+    float get_z(float x, float y);
     void set_z(uint8_t ix, uint8_t iy, float z) { z_values[iy][ix] = z; }
-    
-    int select_x_index(float x) {
-        int i = 1;
-        while (x > get_x(i) && i < MESH_NUM_X_POINTS - 1) i++;
-        return i - 1;
-    }
-    
-    int select_y_index(float y) {
-        int i = 1;
-        while (y > get_y(i) && i < MESH_NUM_Y_POINTS - 1) i++;
-        return i - 1;
-    }
-    
-    float get_z(float x, float y) {
-        int   i, j;
-        float s, t;
-        
-#if MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3
-#define MESH_MID_X (0.5f*(MESH_MIN_X+MESH_MAX_X))
-#define MESH_MID_Y (0.5f*(MESH_MIN_Y+MESH_MAX_Y))
-        if (x < MESH_MID_X) {
-            i = 0;
-            s = (x - MESH_MIN_X) / MESH_X_DIST;
-            if (s > 1.f)
-                s = 1.f;
-        } else {
-            i = 1;
-            s = (x - MESH_MID_X) / MESH_X_DIST;
-            if (s < 0)
-                s = 0;
-        }
-        if (y < MESH_MID_Y) {
-            j = 0;
-            t = (y - MESH_MIN_Y) / MESH_Y_DIST;
-            if (t > 1.f)
-                t = 1.f;
-        } else {
-            j = 1;
-            t = (y - MESH_MID_Y) / MESH_Y_DIST;
-            if (t < 0)
-                t = 0;
-        }
-#else
-        i = int(floor((x - MESH_MIN_X) / MESH_X_DIST));
-        if (i < 0) {
-            i = 0;
-            s = (x - MESH_MIN_X) / MESH_X_DIST;
-            if (s > 1.f)
-                s = 1.f;
-        }
-        else if (i > MESH_NUM_X_POINTS - 2) {
-            i = MESH_NUM_X_POINTS - 2;
-            s = (x - get_x(i)) / MESH_X_DIST;
-            if (s < 0)
-                s = 0;
-        } else {
-            s = (x - get_x(i)) / MESH_X_DIST;
-            if (s < 0)
-                s = 0;
-            else if (s > 1.f)
-                s = 1.f;
-        }
-        j = int(floor((y - MESH_MIN_Y) / MESH_Y_DIST));
-        if (j < 0) {
-            j = 0;
-            t = (y - MESH_MIN_Y) / MESH_Y_DIST;
-            if (t > 1.f)
-                t = 1.f;
-        } else if (j > MESH_NUM_Y_POINTS - 2) {
-            j = MESH_NUM_Y_POINTS - 2;
-            t = (y - get_y(j)) / MESH_Y_DIST;
-            if (t < 0)
-                t = 0;
-        } else {
-            t = (y - get_y(j)) / MESH_Y_DIST;
-            if (t < 0)
-                t = 0;
-            else if (t > 1.f)
-                t = 1.f;
-        }
-#endif /* MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3 */
-        
-        float si = 1.f-s;
-        float z0 = si * z_values[j  ][i] + s * z_values[j  ][i+1];
-        float z1 = si * z_values[j+1][i] + s * z_values[j+1][i+1];
-        return (1.f-t) * z0 + t * z1;
-    }
-    
+    void upsample_3x3();
+    void print();
 };
 
 extern mesh_bed_leveling mbl;
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 18c6aa980bc3..be066530f9f8 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -5484,7 +5484,7 @@ static void mbl_mesh_toggle() {
 }
 
 static void mbl_probe_nr_toggle() {
-	mbl_z_probe_nr = eeprom_read_byte((uint8_t*)EEPROM_MBL_PROBE_NR);
+	uint8_t mbl_z_probe_nr = eeprom_read_byte((uint8_t*)EEPROM_MBL_PROBE_NR);
 	switch (mbl_z_probe_nr) {
 		case 1: mbl_z_probe_nr = 3; break;
 		case 3: mbl_z_probe_nr = 5; break;
@@ -5499,6 +5499,7 @@ static void lcd_mesh_bed_leveling_settings()
 
 	bool magnet_elimination = (eeprom_read_byte((uint8_t*)EEPROM_MBL_MAGNET_ELIMINATION) > 0);
 	uint8_t points_nr = eeprom_read_byte((uint8_t*)EEPROM_MBL_POINTS_NR);
+    uint8_t mbl_z_probe_nr = eeprom_read_byte((uint8_t*)EEPROM_MBL_PROBE_NR);
 	char sToggle[4]; //enough for nxn format
 
 	MENU_BEGIN();
diff --git a/Firmware/variants/MK25-RAMBo10a.h b/Firmware/variants/MK25-RAMBo10a.h
index 099ad329f10e..81289cbf90e0 100644
--- a/Firmware/variants/MK25-RAMBo10a.h
+++ b/Firmware/variants/MK25-RAMBo10a.h
@@ -273,12 +273,6 @@
 
 #define MBL_Z_STEP 0.01
 
-// Mesh definitions
-#define MESH_MIN_X 24
-#define MESH_MAX_X 228
-#define MESH_MIN_Y 6
-#define MESH_MAX_Y 210
-
 // Mesh upsample definition
 #define MESH_NUM_X_POINTS 7
 #define MESH_NUM_Y_POINTS 7
diff --git a/Firmware/variants/MK25-RAMBo13a.h b/Firmware/variants/MK25-RAMBo13a.h
index 7cce101be3c2..ecce197747f4 100644
--- a/Firmware/variants/MK25-RAMBo13a.h
+++ b/Firmware/variants/MK25-RAMBo13a.h
@@ -274,12 +274,6 @@
 
 #define MBL_Z_STEP 0.01
 
-// Mesh definitions
-#define MESH_MIN_X 24
-#define MESH_MAX_X 228
-#define MESH_MIN_Y 6
-#define MESH_MAX_Y 210
-
 // Mesh upsample definition
 #define MESH_NUM_X_POINTS 7
 #define MESH_NUM_Y_POINTS 7
diff --git a/Firmware/variants/MK25S-RAMBo10a.h b/Firmware/variants/MK25S-RAMBo10a.h
index 254be0d09a34..c82be7c5df00 100644
--- a/Firmware/variants/MK25S-RAMBo10a.h
+++ b/Firmware/variants/MK25S-RAMBo10a.h
@@ -273,12 +273,6 @@
 
 #define MBL_Z_STEP 0.01
 
-// Mesh definitions
-#define MESH_MIN_X 24
-#define MESH_MAX_X 228
-#define MESH_MIN_Y 6
-#define MESH_MAX_Y 210
-
 // Mesh upsample definition
 #define MESH_NUM_X_POINTS 7
 #define MESH_NUM_Y_POINTS 7
diff --git a/Firmware/variants/MK25S-RAMBo13a.h b/Firmware/variants/MK25S-RAMBo13a.h
index 2c375f920078..a547aa083a36 100644
--- a/Firmware/variants/MK25S-RAMBo13a.h
+++ b/Firmware/variants/MK25S-RAMBo13a.h
@@ -274,12 +274,6 @@
 
 #define MBL_Z_STEP 0.01
 
-// Mesh definitions
-#define MESH_MIN_X 24
-#define MESH_MAX_X 228
-#define MESH_MIN_Y 6
-#define MESH_MAX_Y 210
-
 // Mesh upsample definition
 #define MESH_NUM_X_POINTS 7
 #define MESH_NUM_Y_POINTS 7
diff --git a/Firmware/variants/MK3.h b/Firmware/variants/MK3.h
index 78ee31d00a55..8135d9856a16 100644
--- a/Firmware/variants/MK3.h
+++ b/Firmware/variants/MK3.h
@@ -423,12 +423,6 @@
 
 #define MBL_Z_STEP 0.01
 
-// Mesh definitions
-#define MESH_MIN_X 24
-#define MESH_MAX_X 228
-#define MESH_MIN_Y 6
-#define MESH_MAX_Y 210
-
 // Mesh upsample definition
 #define MESH_NUM_X_POINTS 7
 #define MESH_NUM_Y_POINTS 7
diff --git a/Firmware/variants/MK3S.h b/Firmware/variants/MK3S.h
index 111d151c335d..6b75132144b9 100644
--- a/Firmware/variants/MK3S.h
+++ b/Firmware/variants/MK3S.h
@@ -427,12 +427,6 @@
 
 #define MBL_Z_STEP 0.01
 
-// Mesh definitions
-#define MESH_MIN_X 24
-#define MESH_MAX_X 228
-#define MESH_MIN_Y 6
-#define MESH_MAX_Y 210
-
 // Mesh upsample definition
 #define MESH_NUM_X_POINTS 7
 #define MESH_NUM_Y_POINTS 7
diff --git a/Firmware/variants/obsolete/1_75mm_MK2-RAMBo10a-E3Dv6full.h b/Firmware/variants/obsolete/1_75mm_MK2-RAMBo10a-E3Dv6full.h
index 1700e0fc197e..eb3769854e10 100644
--- a/Firmware/variants/obsolete/1_75mm_MK2-RAMBo10a-E3Dv6full.h
+++ b/Firmware/variants/obsolete/1_75mm_MK2-RAMBo10a-E3Dv6full.h
@@ -202,12 +202,6 @@ BED SETTINGS
 
 #define MBL_Z_STEP 0.01
 
-// Mesh definitions
-#define MESH_MIN_X 35
-#define MESH_MAX_X 238
-#define MESH_MIN_Y 6
-#define MESH_MAX_Y 202
-
 // Mesh upsample definition
 #define MESH_NUM_X_POINTS 7
 #define MESH_NUM_Y_POINTS 7
diff --git a/Firmware/variants/obsolete/1_75mm_MK2-RAMBo13a-E3Dv6full.h b/Firmware/variants/obsolete/1_75mm_MK2-RAMBo13a-E3Dv6full.h
index 2349385e3f0a..dda5f0606556 100644
--- a/Firmware/variants/obsolete/1_75mm_MK2-RAMBo13a-E3Dv6full.h
+++ b/Firmware/variants/obsolete/1_75mm_MK2-RAMBo13a-E3Dv6full.h
@@ -201,12 +201,6 @@ BED SETTINGS
 
 #define MBL_Z_STEP 0.01
 
-// Mesh definitions
-#define MESH_MIN_X 35
-#define MESH_MAX_X 238
-#define MESH_MIN_Y 6
-#define MESH_MAX_Y 202
-
 // Mesh upsample definition
 #define MESH_NUM_X_POINTS 7
 #define MESH_NUM_Y_POINTS 7