diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
new file mode 100644
index 000000000..0f793558d
--- /dev/null
+++ b/.github/workflows/build.yml
@@ -0,0 +1,154 @@
+name: ci-build
+
+on:
+  pull_request:
+    branches:
+    - '*'
+  push:
+    branches: [ MK3, MK3_* ]
+
+env:
+  GH_ANNOTATIONS: 1
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    steps:
+
+    # setup base required dependencies
+    - name: Setup dependencies
+      run: |
+          sudo apt-get install cmake ninja-build python3-pyelftools python3-regex python3-polib
+
+    # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
+    - name: Checkout ${{ github.event.pull_request.head.ref }}
+      uses: actions/checkout@v3
+      if: ${{ github.event.pull_request }}
+      with:
+        ref: ${{ github.event.pull_request.head.sha }}
+        submodules: true
+
+    - name: Checkout ${{ github.event.ref }}
+      uses: actions/checkout@v3
+      if: ${{ !github.event.pull_request }}
+      with:
+        ref: ${{ github.event.ref }}
+        submodules: true
+
+    - name: Cache Dependencies
+      uses: actions/cache@v3.0.11
+      id: cache-pkgs
+      with:
+          path: ".dependencies"
+          key: "build-deps-1_0_0-linux"
+
+    - name: Setup build dependencies
+      run: |
+          ./utils/bootstrap.py
+
+    - name: Cache permissions
+      run: sudo chmod -R 744 .dependencies
+
+    - name: Build
+      run: |
+          mkdir build
+          cd build
+          cmake .. -DCMAKE_TOOLCHAIN_FILE="../cmake/AvrGcc.cmake" -DCMAKE_BUILD_TYPE=Release -G Ninja
+          ninja
+
+    - name: Upload artifacts
+      if: ${{ !github.event.pull_request }}
+      uses: actions/upload-artifact@v3.1.1
+      with:
+        name: Firmware
+        path: build/*.hex
+
+  check-lang:
+    runs-on: ubuntu-latest
+
+    steps:
+    # setup base required dependencies
+    - name: Setup dependencies
+      run: |
+          sudo apt-get install gcc-11 g++11 lcov cmake ninja-build python3-pyelftools python3-regex python3-polib
+
+    # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
+    - name: Checkout ${{ github.event.pull_request.head.ref }}
+      uses: actions/checkout@v3
+      if: ${{ github.event.pull_request }}
+      with:
+        ref: ${{ github.event.pull_request.head.sha }}
+        submodules: true
+
+    - name: Checkout ${{ github.event.ref }}
+      uses: actions/checkout@v3
+      if: ${{ !github.event.pull_request }}
+      with:
+        ref: ${{ github.event.ref }}
+        submodules: true
+
+    - name: Cache Dependencies
+      uses: actions/cache@v3.0.11
+      id: cache-pkgs
+      with:
+          path: ".dependencies"
+          key: "build-deps-1_0_0-linux"
+
+    - name: Setup build dependencies
+      run: |
+          ./utils/bootstrap.py
+
+    - name: Cache permissions
+      run: sudo chmod -R 744 .dependencies
+
+    - name: Run check
+      run: |
+          mkdir build
+          cd build
+          cmake .. -G Ninja -DCMAKE_TOOLCHAIN_FILE="../cmake/AvrGcc.cmake" -DCMAKE_BUILD_TYPE=Release -G Ninja
+          ninja check_lang
+
+  tests:
+    runs-on: ubuntu-latest
+
+    steps:
+    # setup base required dependencies
+    - name: Setup dependencies
+      run: |
+          sudo apt-get install gcc-11 g++11 lcov cmake ninja-build python3-pyelftools python3-regex python3-polib
+
+    # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
+    - name: Checkout ${{ github.event.pull_request.head.ref }}
+      uses: actions/checkout@v3
+      if: ${{ github.event.pull_request }}
+      with:
+        ref: ${{ github.event.pull_request.head.sha }}
+        submodules: true
+
+    - name: Checkout ${{ github.event.ref }}
+      uses: actions/checkout@v3
+      if: ${{ !github.event.pull_request }}
+      with:
+        ref: ${{ github.event.ref }}
+        submodules: true
+
+    - name: Cache Dependencies
+      uses: actions/cache@v3.0.11
+      id: cache-pkgs
+      with:
+          path: ".dependencies"
+          key: "build-deps-1_0_0-linux"
+
+    - name: Setup build dependencies
+      run: |
+          ./utils/bootstrap.py
+
+    - name: Cache permissions
+      run: sudo chmod -R 744 .dependencies
+
+    - name: Run check
+      run: |
+          mkdir build
+          cd build
+          cmake .. -G Ninja
+          ninja test_run_all
diff --git a/.github/workflows/pr-size.yml b/.github/workflows/pr-size.yml
index 2c68acd88..891336456 100644
--- a/.github/workflows/pr-size.yml
+++ b/.github/workflows/pr-size.yml
@@ -24,10 +24,20 @@ jobs:
     - name: Checkout base
       uses: actions/checkout@v3
 
+    - name: Cache Dependencies
+      uses: actions/cache@v3.0.11
+      id: cache-pkgs
+      with:
+          path: ".dependencies"
+          key: "build-deps-1_0_0-linux"
+
     - name: Setup build dependencies
       run: |
           ./utils/bootstrap.py
 
+    - name: Cache permissions
+      run: sudo chmod -R 744 .dependencies
+
     - name: Build base
       run: |
           rm -rf build-base
diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 4f22e13e2..b0c024c0e 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 ad4ffa31c..4730f7f00 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,
@@ -271,7 +267,7 @@ const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
 float destination[NUM_AXIS] = {  0.0, 0.0, 0.0, 0.0};
 
 // For tracing an arc
-static float offset[3] = {0.0, 0.0, 0.0};
+static float offset[2] = {0.0, 0.0};
 
 // Current feedrate
 float feedrate = 1500.0;
@@ -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,108 @@ 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 = eeprom_read_byte((uint8_t*)EEPROM_MBL_POINTS_NR);
+    if (uint8_t codeSeen = code_seen('N'), value = code_value_uint8(); codeSeen && (value == 7 || value == 3))
+      nMeasPoints = value;
+
+    uint8_t nProbeRetryCount = eeprom_read_byte((uint8_t*)EEPROM_MBL_PROBE_NR);
+    if (uint8_t codeSeen = code_seen('C'), value = code_value_uint8(); codeSeen && value >= 1 && value <= 10)
+      nProbeRetryCount = value;
+
+    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;
-
-#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
-
-    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;
-    }
-
-    uint8_t mesh_point = 0; //index number of calibration point
-    int Z_LIFT_FEEDRATE = homing_feedrate[Z_AXIS] / 40;
+    // 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)
-#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
-    int l_feedmultiply = setup_for_endstop_move(false); //save feedrate and feedmultiply, sets feedmultiply to 100
-    while (mesh_point != nMeasPoints * nMeasPoints) {
-        // 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();
+    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);
+            }
+
+            // 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;
+                }
+            }
+
+            // increment the total point counter if the points are not skipped
+            meshPointsToProbe++;
         }
-        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
+    }
+    mbl.upsample_3x3(); //upsample the default mesh
+
+    // 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
+    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 % 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);
+        current_position[X_AXIS] = x_pos;
+        current_position[Y_AXIS] = y_pos;
 
-        //printf_P(PSTR("[%f;%f]\n"), current_position[X_AXIS], current_position[Y_AXIS]);
-
-
-#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 +2931,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, nProbeRetryCount)) { //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, nProbeRetryCount)) { //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(_i("Mesh bed leveling failed. Please run Z calibration.")); ////MSG_MBL_FAILED_Z_CAL c=20 r=4
+        }
         Sound_MakeSound(e_SOUND_TYPE_StandardAlert);
         bool bState;
         do   {                             // repeat until Z-leveling o.k.
@@ -3054,14 +2992,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 +3007,64 @@ 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)
+
+    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)
+
+    uint8_t useMagnetCompensation = code_seen('M') ? code_value_uint8() : eeprom_read_byte((uint8_t*)EEPROM_MBL_MAGNET_ELIMINATION);
+    if (nMeasPoints == 7 && useMagnetCompensation) {
+        mbl_magnet_elimination();
     }
-    /*
-      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.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) {
@@ -3426,10 +3326,10 @@ static void mmu_M600_filament_change_screen(uint8_t eject_slot) {
         manage_heater();
         manage_inactivity(true);
 
-        btn = MMU2::mmu2.getPrinterButtonOperation();
+        btn = MMU2::mmu2.GetPrinterButtonOperation();
         if (btn != MMU2::Buttons::NoButton)
         {
-            MMU2::mmu2.clearPrinterButtonOperation();
+            MMU2::mmu2.ClearPrinterButtonOperation();
 
             if (btn == MMU2::Buttons::Eject) {
                 if (eject_slot != (uint8_t)MMU2::FILAMENT_UNKNOWN) {
@@ -4981,12 +4881,13 @@ 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 | C | O | M | 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.
+      - `N` - Number of mesh points on x axis. Default is value stored in EEPROM. Valid values are 3 and 7.
+      - `C` - Probe retry counts. Default is value stored in EEPROM. Valid values are 1 to 10.
+      - `O` - Return to origin. Default is 1. Valid values are 0 (false) and 1 (true).
+      - `M` - Use magnet compensation. Will only be used if number of mesh points is set to 7. Default is value stored in EEPROM. Valid values are 0 (false) and 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.");
@@ -7757,14 +7642,14 @@ Sigma_Exit:
   
   case 850: {
 	//! ### M850 - set sheet parameters
-	//! //!@n M850 - Set sheet data S[id] Z[offset] L[label] B[bed_temp] P[PINDA_TEMP]
-	bool bHasZ = false, bHasLabel = false, bHasBed = false, bHasPinda = false;
+	//! //!@n M850 - Set sheet data S[id] Z[offset] L[label] B[bed_temp] P[PINDA_TEMP] A[IS_ACTIVE]
 	uint8_t iSel = 0;
 	int16_t zraw = 0;
 	float z_val = 0;
 	char strLabel[8];
 	uint8_t iBedC = 0;
 	uint8_t iPindaC = 0;
+	bool bIsActive=false;
 	strLabel[7] = '\0'; // null terminate.
 	size_t max_sheets = sizeof(EEPROM_Sheets_base->s)/sizeof(EEPROM_Sheets_base->s[0]);
 	
@@ -7780,6 +7665,7 @@ Sigma_Exit:
 	} else {
 		break;
 	}
+	
 	if (code_seen('Z')){
 		z_val = code_value();
 		zraw = z_val*cs.axis_steps_per_mm[Z_AXIS];
@@ -7788,7 +7674,7 @@ Sigma_Exit:
 			SERIAL_PROTOCOLLNPGM(" Z VALUE OUT OF RANGE");
 			break;
 		}	
-		bHasZ = true;
+		eeprom_update_word(reinterpret_cast<uint16_t *>(&(EEPROM_Sheets_base->s[iSel].z_offset)),zraw);
 	}
 	else
 	{
@@ -7798,13 +7684,13 @@ Sigma_Exit:
 	
 	if (code_seen('L'))
 	{
-		bHasLabel = true;
 		char *src = strchr_pointer + 1;
 		while (*src == ' ') ++src;
 		if (*src != '\0')
 		{
 			strncpy(strLabel,src,7);	
 		}
+		eeprom_update_block(strLabel,EEPROM_Sheets_base->s[iSel].name,sizeof(Sheet::name));
 	}
 	else
 	{
@@ -7813,8 +7699,8 @@ Sigma_Exit:
 	
 	if (code_seen('B'))
 	{
-		bHasBed = true;
 		iBedC = code_value_uint8();
+		eeprom_update_byte(&EEPROM_Sheets_base->s[iSel].bed_temp, iBedC);
 	}
 	else
 	{
@@ -7823,12 +7709,22 @@ Sigma_Exit:
 	
 	if (code_seen('P'))
 	{
-		bHasPinda = true;
 		iPindaC = code_value_uint8();
+		eeprom_update_byte(&EEPROM_Sheets_base->s[iSel].pinda_temp, iPindaC);
 	}
 	else
-		iPindaC = eeprom_read_byte(&EEPROM_Sheets_base->s[iSel].pinda_temp);
 	{
+		iPindaC = eeprom_read_byte(&EEPROM_Sheets_base->s[iSel].pinda_temp);
+	}
+	
+	if (code_seen('A'))
+	{
+		bIsActive |= code_value_uint8() || (eeprom_read_byte(&(EEPROM_Sheets_base->active_sheet)) == iSel);
+		if(bIsActive) eeprom_update_byte(&EEPROM_Sheets_base->active_sheet, iSel);
+	}
+	else
+	{
+		bIsActive = (eeprom_read_byte(&(EEPROM_Sheets_base->active_sheet)) == iSel);
 	}
 	
 	SERIAL_PROTOCOLPGM("Sheet ");
@@ -7836,22 +7732,6 @@ Sigma_Exit:
 	if (!eeprom_is_sheet_initialized(iSel))
 		SERIAL_PROTOCOLLNPGM(" NOT INITIALIZED");
 	
-	if (bHasZ)
-	{
-		eeprom_update_word(reinterpret_cast<uint16_t *>(&(EEPROM_Sheets_base->s[iSel].z_offset)),zraw);
-	}
-	if (bHasLabel)
-	{
-		eeprom_update_block(strLabel,EEPROM_Sheets_base->s[iSel].name,sizeof(Sheet::name));
-	}
-	if (bHasBed)
-	{
-		eeprom_update_byte(&EEPROM_Sheets_base->s[iSel].bed_temp, iBedC);
-	}
-	if (bHasPinda)
-	{
-		eeprom_update_byte(&EEPROM_Sheets_base->s[iSel].pinda_temp, iPindaC);
-	}
 		
 	SERIAL_PROTOCOLPGM(" Z");
 	SERIAL_PROTOCOL_F(z_val,4);
@@ -7863,7 +7743,8 @@ Sigma_Exit:
 	SERIAL_PROTOCOL((int)iBedC);
 	SERIAL_PROTOCOLPGM(" P");
 	SERIAL_PROTOCOLLN((int)iPindaC);
-
+	SERIAL_PROTOCOLPGM(" A");
+	SERIAL_PROTOCOLLN((int)bIsActive);
 	break;
 }
 
@@ -7898,9 +7779,7 @@ Sigma_Exit:
 		cancel_heatup = false;
 
 		bool is_pinda_cooling = false;
-		if ((degTargetBed() == 0) && (degTargetHotend(0) == 0)) {
-		    is_pinda_cooling = true;
-		}
+		if (!(CHECK_ALL_HEATERS)) is_pinda_cooling = true;
 
 		while ( ((!is_pinda_cooling) && (!cancel_heatup) && (current_temperature_pinda < set_target_pinda)) || (is_pinda_cooling && (current_temperature_pinda > set_target_pinda)) ) {
 			if ((_millis() - codenum) > 1000) //Print Temp Reading every 1 second while waiting.
@@ -8250,43 +8129,53 @@ Sigma_Exit:
 	### M914 - Set TMC2130 normal mode <a href="https://reprap.org/wiki/G-code#M914:_Set_TMC2130_normal_mode">M914: Set TMC2130 normal mode</a>
   Updates EEPROM only if "P" is given, otherwise temporary (lasts until reset or motor idle timeout)
       #### Usage
-    
-        M914 [ P | R ]
-    
+
+        M914 [ P | R | Q ]
+
     #### Parameters
     - `P` - Make the mode change permanent (write to EEPROM)
     - `R` - Revert to EEPROM value
+    - `Q` - Print effective silent/normal status. (Does not report override)
+
     */
 
     /*!
 	### M915 - Set TMC2130 silent mode <a href="https://reprap.org/wiki/G-code#M915:_Set_TMC2130_silent_mode">M915: Set TMC2130 silent mode</a>
     Updates EEPROM only if "P" is given, otherwise temporary (lasts until reset or motor idle timeout)
       #### Usage
-    
-        M915 [ P | R ]
-    
+
+        M915 [ P | R | Q]
+
     #### Parameters
     - `P` - Make the mode change permanent (write to EEPROM)
     - `R` - Revert to EEPROM value
+    - `Q` - Print effective silent/normal status. (Does not report override)
     */
 #ifdef TMC2130
     case 914:
     case 915:
     {
-      uint8_t newMode = (mcode_in_progress==914) ? TMC2130_MODE_NORMAL : TMC2130_MODE_SILENT;
-      //printf_P(_n("tmc2130mode/smm/eep: %d %d %d %d"),tmc2130_mode,SilentModeMenu,eeprom_read_byte((uint8_t*)EEPROM_SILENT), bEnableForce_z);
-      if (code_seen('R'))
-      {
-          newMode = eeprom_read_byte((uint8_t*)EEPROM_SILENT);
-      }
-      else if (code_seen('P'))
-      {
-          uint8_t newMenuMode = (mcode_in_progress==914) ? SILENT_MODE_NORMAL : SILENT_MODE_STEALTH;
-          eeprom_update_byte((unsigned char *)EEPROM_SILENT, newMenuMode);
-          SilentModeMenu = newMenuMode;
-          //printf_P(_n("tmc2130mode/smm/eep: %d %d %d %d"),tmc2130_mode,SilentModeMenu,eeprom_read_byte((uint8_t*)EEPROM_SILENT), bEnableForce_z);
-      }
-      
+    uint8_t newMode = (mcode_in_progress==914) ? TMC2130_MODE_NORMAL : TMC2130_MODE_SILENT;
+    //printf_P(_n("tmc2130mode/smm/eep: %d %d %d %d"),tmc2130_mode,SilentModeMenu,eeprom_read_byte((uint8_t*)EEPROM_SILENT), bEnableForce_z);
+    if (code_seen('R'))
+    {
+        newMode = eeprom_read_byte((uint8_t*)EEPROM_SILENT);
+    }
+    else if (code_seen('P'))
+    {
+        uint8_t newMenuMode = (mcode_in_progress==914) ? SILENT_MODE_NORMAL : SILENT_MODE_STEALTH;
+        eeprom_update_byte((unsigned char *)EEPROM_SILENT, newMenuMode);
+        SilentModeMenu = newMenuMode;
+        //printf_P(_n("tmc2130mode/smm/eep: %d %d %d %d"),tmc2130_mode,SilentModeMenu,eeprom_read_byte((uint8_t*)EEPROM_SILENT), bEnableForce_z);
+    }
+    else if (code_seen('Q'))
+    {
+        printf_P(PSTR("%S: %S\n"), _O(MSG_MODE),
+            tmc2130_mode == TMC2130_MODE_NORMAL ?
+            _O(MSG_NORMAL) : _O(MSG_SILENT)
+        );
+
+    }
       if (tmc2130_mode != newMode
 #ifdef PSU_Delta 
           || !bEnableForce_z 
@@ -9392,11 +9281,11 @@ void controllerFan()
  */
 static void handleSafetyTimer()
 {
-    if (printer_active() || (!degTargetBed() && !degTargetHotend(0)) || (!safetytimer_inactive_time))
+    if (printer_active() || !(CHECK_ALL_HEATERS) || !safetytimer_inactive_time)
     {
         safetyTimer.stop();
     }
-    else if ((degTargetBed() || degTargetHotend(0)) && (!safetyTimer.running()))
+    else if ((CHECK_ALL_HEATERS) && !safetyTimer.running())
     {
         safetyTimer.start();
     }
diff --git a/Firmware/Timer.cpp b/Firmware/Timer.cpp
index d0a552b0a..b3ee423b2 100644
--- a/Firmware/Timer.cpp
+++ b/Firmware/Timer.cpp
@@ -6,17 +6,6 @@
 #include "Timer.h"
 #include "system_timer.h"
 
-/**
- * @brief construct Timer
- *
- * It is guaranteed, that construction is equivalent with zeroing all members.
- * This property can be exploited in menu_data.
- */
-template<typename T>
-Timer<T>::Timer() : m_isRunning(false), m_started()
-{
-}
-
 /**
  * @brief Start timer
  */
diff --git a/Firmware/Timer.h b/Firmware/Timer.h
index 9cb18a304..9880764d6 100644
--- a/Firmware/Timer.h
+++ b/Firmware/Timer.h
@@ -17,7 +17,10 @@ template <class T>
 class Timer
 {
 public:
-    Timer();
+    inline constexpr Timer()
+    : m_isRunning(false)
+    , m_started(0) {};
+
     void start();
     void stop(){m_isRunning = false;}
     bool running()const {return m_isRunning;}
diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp
index 853e88a03..ca444fcbe 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 fcfa9527b..651eaba71 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 aa969340c..9ffcbb59d 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;
-            }
-        }
-    }
-*/
 }
-#endif
+
+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  // MESH_BED_LEVELING
diff --git a/Firmware/mesh_bed_leveling.h b/Firmware/mesh_bed_leveling.h
index 7df04844a..7b3313a12 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/mmu2.cpp b/Firmware/mmu2.cpp
index cffcf4c45..333a9d0e1 100644
--- a/Firmware/mmu2.cpp
+++ b/Firmware/mmu2.cpp
@@ -81,8 +81,7 @@ void MMU2::StopKeepPowered() {
 void MMU2::Tune() {
     switch (lastErrorCode) {
     case ErrorCode::HOMING_SELECTOR_FAILED:
-    case ErrorCode::HOMING_IDLER_FAILED:
-    {
+    case ErrorCode::HOMING_IDLER_FAILED: {
         // Prompt a menu for different values
         tuneIdlerStallguardThreshold();
         break;
@@ -115,7 +114,7 @@ void MMU2::ResetX0() {
     logic.ResetMMU(); // Send soft reset
 }
 
-void MMU2::ResetX42(){
+void MMU2::ResetX42() {
     logic.ResetMMU(42);
 }
 
@@ -196,7 +195,7 @@ void __attribute__((noinline)) MMU2::mmu_loop_inner(bool reportErrors) {
     if (isErrorScreenRunning()) {
         // Call this every iteration to keep the knob rotation responsive
         // This includes when mmu_loop is called within manage_response
-        ReportErrorHook((CommandInProgress)logic.CommandInProgress(), (uint16_t)lastErrorCode, uint8_t(lastErrorSource));
+        ReportErrorHook((CommandInProgress)logic.CommandInProgress(), lastErrorCode, uint8_t(lastErrorSource));
     }
 }
 
@@ -204,7 +203,7 @@ void MMU2::CheckFINDARunout() {
     // Check for FINDA filament runout
     if (!FindaDetectsFilament() && check_fsensor()) {
         SERIAL_ECHOLNPGM("FINDA filament runout!");
-        stop_and_save_print_to_ram(0, 0);
+        marlin_stop_and_save_print_to_ram();
         restore_print_from_ram_and_continue(0);
         if (SpoolJoin::spooljoin.isSpoolJoinEnabled() && get_current_tool() != (uint8_t)FILAMENT_UNKNOWN){ // Can't auto if F=?
             enquecommand_front_P(PSTR("M600 AUTO")); // save print and run M600 command
@@ -218,10 +217,10 @@ struct ReportingRAII {
     CommandInProgress cip;
     explicit inline __attribute__((always_inline)) ReportingRAII(CommandInProgress cip)
         : cip(cip) {
-        BeginReport(cip, (uint16_t)ProgressCode::EngagingIdler);
+        BeginReport(cip, ProgressCode::EngagingIdler);
     }
     inline __attribute__((always_inline)) ~ReportingRAII() {
-        EndReport(cip, (uint16_t)ProgressCode::OK);
+        EndReport(cip, ProgressCode::OK);
     }
 };
 
@@ -237,11 +236,11 @@ bool MMU2::WaitForMMUReady() {
     }
 }
 
-bool MMU2::RetryIfPossible(uint16_t ec) {
+bool MMU2::RetryIfPossible(ErrorCode ec) {
     if (logic.RetryAttempts()) {
         SetButtonResponse(ButtonOperations::Retry);
         // check, that Retry is actually allowed on that operation
-        if (ButtonAvailable(ec) != NoButton) {
+        if (ButtonAvailable(ec) != Buttons::NoButton) {
             logic.SetInAutoRetry(true);
             SERIAL_ECHOLNPGM("RetryButtonPressed");
             // We don't decrement until the button is acknowledged by the MMU.
@@ -256,23 +255,13 @@ bool MMU2::RetryIfPossible(uint16_t ec) {
 bool MMU2::VerifyFilamentEnteredPTFE() {
     planner_synchronize();
 
-    if (WhereIsFilament() == FilamentState::NOT_PRESENT)
+    if (WhereIsFilament() != FilamentState::AT_FSENSOR)
         return false;
 
-    uint8_t fsensorState = 0;
-    uint8_t fsensorStateLCD = 0;
-    uint8_t lcd_cursor_col = 0;
     // MMU has finished its load, push the filament further by some defined constant length
     // If the filament sensor reads 0 at any moment, then report FAILURE
-
-    const float delta_mm = MMU2_CHECK_FILAMENT_PRESENCE_EXTRUSION_LENGTH - logic.ExtraLoadDistance();
-
-    // The total length is twice delta_mm. Divide that length by number of pixels
-    // available to get length per pixel.
-    // Note: Below is the reciprocal of (2 * delta_mm) / LCD_WIDTH [mm/pixel]
-    const float pixel_per_mm =  0.5f * float(LCD_WIDTH) / (delta_mm);
-
-    TryLoadUnloadProgressbarInit();
+    const float tryload_length = MMU2_CHECK_FILAMENT_PRESENCE_EXTRUSION_LENGTH - logic.ExtraLoadDistance();
+    TryLoadUnloadReporter tlur(tryload_length);
 
     /* The position is a triangle wave
     // current position is not zero, it is an offset
@@ -284,7 +273,7 @@ bool MMU2::VerifyFilamentEnteredPTFE() {
     // in the slope's sign or check the last machine position.
     //              y(x)
     //              ▲
-    //              │     ^◄────────── delta_mm + current_position
+    //              │     ^◄────────── tryload_length + current_position
     //   machine    │    / \
     //   position   │   /   \◄────────── stepper_position_mm + current_position
     //    (mm)      │  /     \
@@ -295,42 +284,24 @@ bool MMU2::VerifyFilamentEnteredPTFE() {
     //                 pixel #
     */
 
+    bool filament_inserted = true; // expect success
     // Pixel index will go from 0 to 10, then back from 10 to 0
     // The change in this number is used to indicate a new pixel
     // should be drawn on the display
-    uint8_t dpixel1 = 0;
-    uint8_t dpixel0 = 0;
     for (uint8_t move = 0; move < 2; move++) {
-        MoveE(move == 0 ? delta_mm : -delta_mm, MMU2_VERIFY_LOAD_TO_NOZZLE_FEED_RATE);
+        extruder_move(move == 0 ? tryload_length : -tryload_length, MMU2_VERIFY_LOAD_TO_NOZZLE_FEED_RATE);
         while (planner_any_moves()) {
-            // Wait for move to finish and monitor the fsensor the entire time
-            // A single 0 reading will set the bit.
-            fsensorStateLCD |= (WhereIsFilament() == FilamentState::NOT_PRESENT);
-            fsensorState |= fsensorStateLCD; // No need to do the above comparison twice, just bitwise OR
-
-            // Always round up, you can only have 'whole' pixels. (floor is also an option)
-            dpixel1 = ceil((stepper_get_machine_position_E_mm() - planner_get_current_position_E()) * pixel_per_mm);
-            if (dpixel1 - dpixel0) {
-                dpixel0 = dpixel1;
-                if (lcd_cursor_col > (LCD_WIDTH - 1)) lcd_cursor_col = LCD_WIDTH - 1;
-                TryLoadUnloadProgressbar(lcd_cursor_col++, fsensorStateLCD);
-                fsensorStateLCD = 0;      // Clear temporary bit
-            }
+            filament_inserted = filament_inserted && (WhereIsFilament() == FilamentState::AT_FSENSOR);
+            tlur.Progress(filament_inserted);
             safe_delay_keep_alive(0);
         }
     }
-
     Disable_E0();
-    TryLoadUnloadProgressbarEcho();
-    TryLoadUnloadProgressbarDeinit();
-
-    if (fsensorState) {
+    if (!filament_inserted) {
         IncrementLoadFails();
-        return false;
-    } else {
-        // else, happy printing! :)
-        return true;
     }
+    tlur.DumpToSerial();
+    return filament_inserted;
 }
 
 bool MMU2::ToolChangeCommonOnce(uint8_t slot) {
@@ -355,8 +326,6 @@ bool MMU2::ToolChangeCommonOnce(uint8_t slot) {
             // but honestly - if the MMU restarts during every toolchange,
             // something else is seriously broken and stopping a print is probably our best option.
         }
-        // reset current position to whatever the planner thinks it is
-        planner_set_current_position_E(planner_get_current_position_E());
         if (VerifyFilamentEnteredPTFE()) {
             return true; // success
         } else {         // Prepare a retry attempt
@@ -455,7 +424,7 @@ uint8_t MMU2::get_tool_change_tool() const {
 bool MMU2::set_filament_type(uint8_t /*slot*/, uint8_t /*type*/) {
     if (!WaitForMMUReady())
         return false;
-    
+
     // @@TODO - this is not supported in the new MMU yet
     //    slot = slot; // @@TODO
     //    type = type; // @@TODO
@@ -496,9 +465,11 @@ bool MMU2::unload() {
 
     WaitForHotendTargetTempBeep();
 
-    ReportingRAII rep(CommandInProgress::UnloadFilament);
-    UnloadInner();
-
+    {
+        ReportingRAII rep(CommandInProgress::UnloadFilament);
+        UnloadInner();
+    }
+    ScreenUpdateEnable();
     return true;
 }
 
@@ -526,10 +497,10 @@ bool MMU2::cut_filament(uint8_t slot, bool enableFullScreenMsg /*= true*/) {
 
         ReportingRAII rep(CommandInProgress::CutFilament);
         CutFilamentInner(slot);
+        extruder = MMU2_NO_TOOL;
+        tool_change_extruder = MMU2_NO_TOOL;
+        MakeSound(SoundType::Confirm);
     }
-    extruder = MMU2_NO_TOOL;
-    tool_change_extruder = MMU2_NO_TOOL;
-    MakeSound(SoundType::Confirm);
     ScreenUpdateEnable();
     return true;
 }
@@ -548,20 +519,18 @@ bool MMU2::load_filament(uint8_t slot) {
         return false;
 
     FullScreenMsgLoad(slot);
-
-    ReportingRAII rep(CommandInProgress::LoadFilament);
-    for (;;) {
-        Disable_E0();
-        logic.LoadFilament(slot);
-        if (manage_response(false, false))
-            break;
-        IncrementMMUFails();
+    {
+        ReportingRAII rep(CommandInProgress::LoadFilament);
+        for (;;) {
+            Disable_E0();
+            logic.LoadFilament(slot);
+            if (manage_response(false, false))
+                break;
+            IncrementMMUFails();
+        }
+        MakeSound(SoundType::Confirm);
     }
-
-    MakeSound(SoundType::Confirm);
-
     ScreenUpdateEnable();
-
     return true;
 }
 
@@ -611,10 +580,11 @@ bool MMU2::eject_filament(uint8_t slot, bool enableFullScreenMsg /* = true */) {
                 break;
             IncrementMMUFails();
         }
+        extruder = MMU2_NO_TOOL;
+        tool_change_extruder = MMU2_NO_TOOL;
+        MakeSound(Confirm);
     }
-    extruder = MMU2_NO_TOOL;
-    tool_change_extruder = MMU2_NO_TOOL;
-    MakeSound(Confirm);
+    ScreenUpdateEnable();
     return true;
 }
 
@@ -647,14 +617,14 @@ void MMU2::SaveAndPark(bool move_axes) {
 
         // In case a power panic happens while waiting for the user
         // take a partial back up of print state into RAM (current position, etc.)
-        refresh_print_state_in_ram();
+        marlin_refresh_print_state_in_ram();
 
         if (move_axes) {
             mmu_print_saved |= SavedState::ParkExtruder;
             resume_position = planner_current_position(); // save current pos
 
             // lift Z
-            MoveRaiseZ(MMU_ERR_Z_PAUSE_LIFT);
+            move_raise_z(MMU_ERR_Z_PAUSE_LIFT);
 
             // move XY aside
             if (all_axes_homed()) {
@@ -706,14 +676,14 @@ void MMU2::ResumeUnpark() {
         // From this point forward, power panic should not use
         // the partial backup in RAM since the extruder is no
         // longer in parking position
-        clear_print_state_in_ram();
+        marlin_clear_print_state_in_ram();
 
         mmu_print_saved &= ~(SavedState::ParkExtruder);
     }
 }
 
 void MMU2::CheckUserInput() {
-    auto btn = ButtonPressed((uint16_t)lastErrorCode);
+    auto btn = ButtonPressed(lastErrorCode);
 
     // Was a button pressed on the MMU itself instead of the LCD?
     if (btn == Buttons::NoButton && lastButton != Buttons::NoButton) {
@@ -733,16 +703,16 @@ void MMU2::CheckUserInput() {
     }
 
     switch (btn) {
-    case Left:
-    case Middle:
-    case Right:
+    case Buttons::Left:
+    case Buttons::Middle:
+    case Buttons::Right:
         SERIAL_ECHOPGM("CheckUserInput-btnLMR ");
-        SERIAL_ECHOLN(btn);
+        SERIAL_ECHOLN(buttons_to_uint8t(btn));
         ResumeHotendTemp(); // Recover the hotend temp before we attempt to do anything else...
 
         if (mmu2.MMULastErrorSource() == MMU2::ErrorSourceMMU) {
             // Do not send a button to the MMU unless the MMU is in error state
-            Button(btn);
+            Button(buttons_to_uint8t(btn));
         }
 
         // A quick hack: for specific error codes move the E-motor every time.
@@ -757,22 +727,22 @@ void MMU2::CheckUserInput() {
             break;
         }
         break;
-    case TuneMMU:
+    case Buttons::TuneMMU:
         Tune();
         break;
-    case Load:
-    case Eject:
+    case Buttons::Load:
+    case Buttons::Eject:
         // High level operation
-        setPrinterButtonOperation(btn);
+        SetPrinterButtonOperation(btn);
         break;
-    case ResetMMU:
+    case Buttons::ResetMMU:
         Reset(ResetPin); // we cannot do power cycle on the MK3
         // ... but mmu2_power.cpp knows this and triggers a soft-reset instead.
         break;
-    case DisableMMU:
+    case Buttons::DisableMMU:
         Stop(); // Poweroff handles updating the EEPROM shutoff.
         break;
-    case StopPrint:
+    case Buttons::StopPrint:
         // @@TODO not sure if we shall handle this high level operation at this spot
         break;
     default:
@@ -929,8 +899,8 @@ void MMU2::execute_extruder_sequence(const E_Step *sequence, uint8_t steps) {
     planner_synchronize();
 
     const E_Step *step = sequence;
-    for (uint8_t i = steps; i ; --i) {
-        MoveE(pgm_read_float(&(step->extrude)), pgm_read_float(&(step->feedRate)));
+    for (uint8_t i = steps; i > 0; --i) {
+        extruder_move(pgm_read_float(&(step->extrude)), pgm_read_float(&(step->feedRate)));
         step++;
     }
     planner_synchronize(); // it looks like it's better to sync the moves at the end - smoother move (if the sequence is not too long).
@@ -976,12 +946,13 @@ void MMU2::ReportError(ErrorCode ec, ErrorSource res) {
     if (ec != lastErrorCode) { // deduplicate: only report changes in error codes into the log
         lastErrorCode = ec;
         lastErrorSource = res;
-        LogErrorEvent_P(_O(PrusaErrorTitle(PrusaErrorCodeIndex((uint16_t)ec))));
+        LogErrorEvent_P(_O(PrusaErrorTitle(PrusaErrorCodeIndex(ec))));
 
         if (ec != ErrorCode::OK && ec != ErrorCode::FILAMENT_EJECTED && ec != ErrorCode::FILAMENT_CHANGE) {
             IncrementMMUFails();
 
             // check if it is a "power" failure - we consider TMC-related errors as power failures
+            // clang-format off
             static constexpr uint16_t tmcMask =
                 ( (uint16_t)ErrorCode::TMC_IOIN_MISMATCH
                 | (uint16_t)ErrorCode::TMC_RESET
@@ -990,6 +961,7 @@ void MMU2::ReportError(ErrorCode ec, ErrorSource res) {
                 | (uint16_t)ErrorCode::TMC_OVER_TEMPERATURE_WARN
                 | (uint16_t)ErrorCode::TMC_OVER_TEMPERATURE_ERROR
                 | (uint16_t)ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION ) & 0x7fffU; // skip the top bit
+            // clang-format on
             static_assert(tmcMask == 0x7e00); // just make sure we fail compilation if any of the TMC error codes change
 
             if ((uint16_t)ec & tmcMask) { // @@TODO can be optimized to uint8_t operation
@@ -999,11 +971,11 @@ void MMU2::ReportError(ErrorCode ec, ErrorSource res) {
         }
     }
 
-    if (!mmu2.RetryIfPossible((uint16_t)ec)) {
+    if (!mmu2.RetryIfPossible(ec)) {
         // If retry attempts are all used up
         // or if 'Retry' operation is not available
-        // raise the MMU error sceen and wait for user input
-        ReportErrorHook((CommandInProgress)logic.CommandInProgress(), (uint16_t)ec, uint8_t(lastErrorSource));
+        // raise the MMU error screen and wait for user input
+        ReportErrorHook((CommandInProgress)logic.CommandInProgress(), ec, uint8_t(lastErrorSource));
     }
 
     static_assert(mmu2Magic[0] == 'M'
@@ -1016,8 +988,8 @@ void MMU2::ReportError(ErrorCode ec, ErrorSource res) {
 }
 
 void MMU2::ReportProgress(ProgressCode pc) {
-    ReportProgressHook((CommandInProgress)logic.CommandInProgress(), (uint16_t)pc);
-    LogEchoEvent_P(_O(ProgressCodeToText((uint16_t)pc)));
+    ReportProgressHook((CommandInProgress)logic.CommandInProgress(), pc);
+    LogEchoEvent_P(_O(ProgressCodeToText(pc)));
 }
 
 void MMU2::OnMMUProgressMsg(ProgressCode pc) {
@@ -1058,7 +1030,7 @@ void MMU2::OnMMUProgressMsgChanged(ProgressCode pc) {
 }
 
 void __attribute__((noinline)) MMU2::HelpUnloadToFinda() {
-    MoveE(-MMU2_RETRY_UNLOAD_TO_FINDA_LENGTH, MMU2_RETRY_UNLOAD_TO_FINDA_FEED_RATE);
+    extruder_move(-MMU2_RETRY_UNLOAD_TO_FINDA_LENGTH, MMU2_RETRY_UNLOAD_TO_FINDA_FEED_RATE);
 }
 
 void MMU2::OnMMUProgressMsgSame(ProgressCode pc) {
@@ -1089,7 +1061,7 @@ void MMU2::OnMMUProgressMsgSame(ProgressCode pc) {
                 // After the MMU knows the FSENSOR is triggered it will:
                 // 1. Push the filament by additional 30mm (see fsensorToNozzle)
                 // 2. Disengage the idler and push another 2mm.
-                MoveE(logic.ExtraLoadDistance() + 2, logic.PulleySlowFeedRate());
+                extruder_move(logic.ExtraLoadDistance() + 2, logic.PulleySlowFeedRate());
                 break;
             case FilamentState::NOT_PRESENT:
                 // fsensor not triggered, continue moving extruder
@@ -1099,7 +1071,7 @@ void MMU2::OnMMUProgressMsgSame(ProgressCode pc) {
                     // than 450mm because the firmware will ignore too long extrusions
                     // for safety reasons. See PREVENT_LENGTHY_EXTRUDE.
                     // Use 350mm to be safely away from the prevention threshold
-                    MoveE(350.0f, logic.PulleySlowFeedRate());
+                    extruder_move(350.0f, logic.PulleySlowFeedRate());
                 }
                 break;
             default:
diff --git a/Firmware/mmu2.h b/Firmware/mmu2.h
index 0c4df18e1..750154ce0 100644
--- a/Firmware/mmu2.h
+++ b/Firmware/mmu2.h
@@ -5,14 +5,11 @@
 #include "mmu2_marlin.h"
 
 #ifdef __AVR__
-#include "mmu2_protocol_logic.h"
+    #include "mmu2_protocol_logic.h"
 typedef float feedRate_t;
 
 #else
-
     #include "protocol_logic.h"
-    #include "../../Marlin/src/core/macros.h"
-    #include "../../Marlin/src/core/types.h"
     #include <atomic>
 #endif
 
@@ -48,9 +45,9 @@ public:
 
     /// Different levels of resetting the MMU
     enum ResetForm : uint8_t {
-        Software = 0,   ///< sends a X0 command into the MMU, the MMU will watchdog-reset itself
-        ResetPin = 1,   ///< trigger the reset pin of the MMU
-        CutThePower = 2, ///< power off and power on (that includes +5V and +24V power lines)
+        Software = 0,     ///< sends a X0 command into the MMU, the MMU will watchdog-reset itself
+        ResetPin = 1,     ///< trigger the reset pin of the MMU
+        CutThePower = 2,  ///< power off and power on (that includes +5V and +24V power lines)
         EraseEEPROM = 42, ///< erase MMU EEPROM and then perform a software reset
     };
 
@@ -182,7 +179,7 @@ public:
     /// Automagically "press" a Retry button if we have any retry attempts left
     /// @param ec ErrorCode enum value
     /// @returns true if auto-retry is ongoing, false when retry is unavailable or retry attempts are all used up
-    bool RetryIfPossible(uint16_t ec);
+    bool RetryIfPossible(ErrorCode ec);
 
     /// @return count for toolchange in current print
     inline uint16_t ToolChangeCounter() const { return toolchange_counter; };
@@ -201,9 +198,9 @@ public:
     };
     inline void InvokeErrorScreen(ErrorCode ec) {
         // The printer may not raise an error when the MMU is busy
-        if ( !logic.CommandInProgress() // MMU must not be busy
+        if (!logic.CommandInProgress()                // MMU must not be busy
             && MMUCurrentErrorCode() == ErrorCode::OK // The protocol must not be in error state
-            && lastErrorCode != ec) // The error code is not a duplicate
+            && lastErrorCode != ec)                   // The error code is not a duplicate
         {
             ReportError(ec, ErrorSource::ErrorSourcePrinter);
         }
@@ -217,21 +214,23 @@ public:
 
     /// @brief Queue a button operation which the printer can act upon
     /// @param btn Button operation
-    inline void setPrinterButtonOperation(Buttons btn) {
+    inline void SetPrinterButtonOperation(Buttons btn) {
         printerButtonOperation = btn;
     }
 
     /// @brief Get the printer button operation
     /// @return currently set printer button operation, it can be NoButton if nothing is queued
-    inline Buttons getPrinterButtonOperation() {
+    inline Buttons GetPrinterButtonOperation() {
         return printerButtonOperation;
     }
 
-    inline void clearPrinterButtonOperation() {
+    inline void ClearPrinterButtonOperation() {
         printerButtonOperation = Buttons::NoButton;
     }
 
+#ifndef UNITTEST
 private:
+#endif
     /// Perform software self-reset of the MMU (sends an X0 command)
     void ResetX0();
 
@@ -279,6 +278,11 @@ private:
 
     /// Responds to a change of MMU's progress
     /// - plans additional steps, e.g. starts the E-motor after fsensor trigger
+    /// The function is quite complex, because it needs to handle asynchronnous
+    /// progress and error reports coming from the MMU without an explicit command
+    /// - typically after MMU's start or after some HW issue on the MMU.
+    /// It must ensure, that calls to @ref ReportProgress and/or @ref ReportError are
+    /// only executed after @ref BeginReport has been called first.
     void OnMMUProgressMsg(ProgressCode pc);
     /// Progress code changed - act accordingly
     void OnMMUProgressMsgChanged(ProgressCode pc);
diff --git a/Firmware/mmu2/buttons.h b/Firmware/mmu2/buttons.h
index 4941513a0..2d7b3128f 100644
--- a/Firmware/mmu2/buttons.h
+++ b/Firmware/mmu2/buttons.h
@@ -2,8 +2,8 @@
 #include <stdint.h>
 
 // Helper macros to parse the operations from Btns()
-#define BUTTON_OP_RIGHT(X) ( ( X & 0xF0 ) >> 4 )
-#define BUTTON_OP_MIDDLE(X) ( X & 0x0F )
+#define BUTTON_OP_RIGHT(X)  ((X & 0xF0) >> 4)
+#define BUTTON_OP_MIDDLE(X) (X & 0x0F)
 
 namespace MMU2 {
 
@@ -23,11 +23,11 @@ enum class ButtonOperations : uint8_t {
 };
 
 /// Button codes + extended actions performed on the printer's side
-enum Buttons : uint8_t {
+enum class Buttons : uint_least8_t {
     Right = 0,
     Middle,
     Left,
-    
+
     // performed on the printer's side
     ResetMMU,
     Load,
@@ -35,9 +35,12 @@ enum Buttons : uint8_t {
     StopPrint,
     DisableMMU,
     TuneMMU, // Printer changes MMU register value
-    
+
     NoButton = 0xff // shall be kept last
 };
 
+constexpr uint_least8_t buttons_to_uint8t(Buttons b) {
+    return static_cast<uint8_t>(b);
+}
 
 } // namespace MMU2
diff --git a/Firmware/mmu2/registers.h b/Firmware/mmu2/registers.h
index 489facc39..29d4da9c5 100644
--- a/Firmware/mmu2/registers.h
+++ b/Firmware/mmu2/registers.h
@@ -1,8 +1,10 @@
 #pragma once
 
+
+namespace MMU2 {
+
 // Register map for MMU
-enum class Register : uint8_t
-{
+enum class Register : uint8_t {
     Project_Major = 0x00,
     Project_Minor = 0x01,
     Project_Revision = 0x02,
@@ -38,3 +40,5 @@ enum class Register : uint8_t
     Set_Get_Idler_iRun = 0x20,
     Reserved = 0x21,
 };
+
+} // namespace MMU2
diff --git a/Firmware/mmu2_crc.cpp b/Firmware/mmu2_crc.cpp
index 20c68f7c5..994518911 100644
--- a/Firmware/mmu2_crc.cpp
+++ b/Firmware/mmu2_crc.cpp
@@ -2,12 +2,12 @@
 #include "mmu2_crc.h"
 
 #ifdef __AVR__
-#include <util/crc16.h>
+    #include <util/crc16.h>
 #endif
 
 namespace modules {
+// clang-format off
 namespace crc {
-
 #ifdef __AVR__
 uint8_t CRC8::CCITT_update(uint8_t crc, uint8_t b) {
     return _crc8_ccitt_update(crc, b);
@@ -17,6 +17,6 @@ uint8_t CRC8::CCITT_update(uint8_t crc, uint8_t b) {
     return CCITT_updateCX(crc, b);
 }
 #endif
-
 } // namespace crc
+// clang-format on
 } // namespace modules
diff --git a/Firmware/mmu2_crc.h b/Firmware/mmu2_crc.h
index cc8f06dab..9fefbd319 100644
--- a/Firmware/mmu2_crc.h
+++ b/Firmware/mmu2_crc.h
@@ -4,6 +4,9 @@
 
 namespace modules {
 
+// clang-format off
+// prevent silly indenting of the whole file
+
 /// Contains all the necessary functions for computation of CRC
 namespace crc {
 
@@ -40,4 +43,6 @@ public:
 
 } // namespace crc
 
+// clang-format on
+
 } // namespace modules
diff --git a/Firmware/mmu2_error_converter.cpp b/Firmware/mmu2_error_converter.cpp
index 0acc3d778..1c760b94b 100644
--- a/Firmware/mmu2_error_converter.cpp
+++ b/Firmware/mmu2_error_converter.cpp
@@ -35,116 +35,125 @@ static_assert( FindErrorIndex(ERR_MECHANICAL_FINDA_FILAMENT_STUCK) == 1);
 static_assert( FindErrorIndex(ERR_MECHANICAL_FSENSOR_DIDNT_TRIGGER) == 2);
 static_assert( FindErrorIndex(ERR_MECHANICAL_FSENSOR_FILAMENT_STUCK) == 3);
 
-uint8_t PrusaErrorCodeIndex(uint16_t ec) {
+constexpr ErrorCode operator&(ErrorCode a, ErrorCode b){
+    return (ErrorCode)((uint16_t)a & (uint16_t)b);
+}
+
+constexpr bool ContainsBit(ErrorCode ec, ErrorCode mask){
+    return (uint16_t)ec & (uint16_t)mask;
+}
+
+uint8_t PrusaErrorCodeIndex(ErrorCode ec) {
     switch (ec) {
-    case (uint16_t)ErrorCode::FINDA_DIDNT_SWITCH_ON:
+    case ErrorCode::FINDA_DIDNT_SWITCH_ON:
         return FindErrorIndex(ERR_MECHANICAL_FINDA_DIDNT_TRIGGER);
-    case (uint16_t)ErrorCode::FINDA_DIDNT_SWITCH_OFF:
+    case ErrorCode::FINDA_DIDNT_SWITCH_OFF:
         return FindErrorIndex(ERR_MECHANICAL_FINDA_FILAMENT_STUCK);
-    case (uint16_t)ErrorCode::FSENSOR_DIDNT_SWITCH_ON:
+    case ErrorCode::FSENSOR_DIDNT_SWITCH_ON:
         return FindErrorIndex(ERR_MECHANICAL_FSENSOR_DIDNT_TRIGGER);
-    case (uint16_t)ErrorCode::FSENSOR_DIDNT_SWITCH_OFF:
+    case ErrorCode::FSENSOR_DIDNT_SWITCH_OFF:
         return FindErrorIndex(ERR_MECHANICAL_FSENSOR_FILAMENT_STUCK);
-    case (uint16_t)ErrorCode::FSENSOR_TOO_EARLY:
+    case ErrorCode::FSENSOR_TOO_EARLY:
         return FindErrorIndex(ERR_MECHANICAL_FSENSOR_TOO_EARLY);
-    case (uint16_t)ErrorCode::FINDA_FLICKERS:
+    case ErrorCode::FINDA_FLICKERS:
         return FindErrorIndex(ERR_MECHANICAL_INSPECT_FINDA);
-    case (uint16_t)ErrorCode::LOAD_TO_EXTRUDER_FAILED:
+    case ErrorCode::LOAD_TO_EXTRUDER_FAILED:
         return FindErrorIndex(ERR_MECHANICAL_LOAD_TO_EXTRUDER_FAILED);
-    case (uint16_t)ErrorCode::FILAMENT_EJECTED:
+    case ErrorCode::FILAMENT_EJECTED:
         return FindErrorIndex(ERR_SYSTEM_FILAMENT_EJECTED);
-    case (uint16_t)ErrorCode::FILAMENT_CHANGE:
+    case ErrorCode::FILAMENT_CHANGE:
         return FindErrorIndex(ERR_SYSTEM_FILAMENT_CHANGE);
 
-    case (uint16_t)ErrorCode::STALLED_PULLEY:
-    case (uint16_t)ErrorCode::MOVE_PULLEY_FAILED:
+    case ErrorCode::STALLED_PULLEY:
+    case ErrorCode::MOVE_PULLEY_FAILED:
         return FindErrorIndex(ERR_MECHANICAL_PULLEY_CANNOT_MOVE);
         
-    case (uint16_t)ErrorCode::HOMING_SELECTOR_FAILED:
+    case ErrorCode::HOMING_SELECTOR_FAILED:
         return FindErrorIndex(ERR_MECHANICAL_SELECTOR_CANNOT_HOME);
-    case (uint16_t)ErrorCode::MOVE_SELECTOR_FAILED:
+    case ErrorCode::MOVE_SELECTOR_FAILED:
         return FindErrorIndex(ERR_MECHANICAL_SELECTOR_CANNOT_MOVE);
         
-    case (uint16_t)ErrorCode::HOMING_IDLER_FAILED:
+    case ErrorCode::HOMING_IDLER_FAILED:
         return FindErrorIndex(ERR_MECHANICAL_IDLER_CANNOT_HOME);
-    case (uint16_t)ErrorCode::MOVE_IDLER_FAILED:
+    case ErrorCode::MOVE_IDLER_FAILED:
         return FindErrorIndex(ERR_MECHANICAL_IDLER_CANNOT_MOVE);
         
-    case (uint16_t)ErrorCode::MMU_NOT_RESPONDING:
+    case ErrorCode::MMU_NOT_RESPONDING:
         return FindErrorIndex(ERR_CONNECT_MMU_NOT_RESPONDING);
-    case (uint16_t)ErrorCode::PROTOCOL_ERROR:
+    case ErrorCode::PROTOCOL_ERROR:
         return FindErrorIndex(ERR_CONNECT_COMMUNICATION_ERROR);
-    case (uint16_t)ErrorCode::FILAMENT_ALREADY_LOADED:
+    case ErrorCode::FILAMENT_ALREADY_LOADED:
         return FindErrorIndex(ERR_SYSTEM_FILAMENT_ALREADY_LOADED);
-    case (uint16_t)ErrorCode::INVALID_TOOL:
+    case ErrorCode::INVALID_TOOL:
         return FindErrorIndex(ERR_SYSTEM_INVALID_TOOL);
-    case (uint16_t)ErrorCode::QUEUE_FULL:
+    case ErrorCode::QUEUE_FULL:
         return FindErrorIndex(ERR_SYSTEM_QUEUE_FULL);
-    case (uint16_t)ErrorCode::VERSION_MISMATCH:
+    case ErrorCode::VERSION_MISMATCH:
         return FindErrorIndex(ERR_SYSTEM_FW_UPDATE_NEEDED);
-    case (uint16_t)ErrorCode::INTERNAL:
+    case ErrorCode::INTERNAL:
         return FindErrorIndex(ERR_SYSTEM_FW_RUNTIME_ERROR);
-    case (uint16_t)ErrorCode::FINDA_VS_EEPROM_DISREPANCY:
+    case ErrorCode::FINDA_VS_EEPROM_DISREPANCY:
         return FindErrorIndex(ERR_SYSTEM_UNLOAD_MANUALLY);
-    case (uint16_t)ErrorCode::MCU_UNDERVOLTAGE_VCC:
+    case ErrorCode::MCU_UNDERVOLTAGE_VCC:
         return FindErrorIndex(ERR_ELECTRICAL_MMU_MCU_ERROR);
+    default: break;
     }
     
     // Electrical issues which can be detected somehow.
     // Need to be placed before TMC-related errors in order to process couples of error bits between single ones
     // and to keep the code size down.
-    if (ec & (uint16_t)ErrorCode::TMC_PULLEY_BIT) {
-        if ((ec & (uint16_t)ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION) == (uint16_t)ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION)
+    if (ContainsBit(ec, ErrorCode::TMC_PULLEY_BIT)) {
+        if ((ec & ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION) == ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION)
             return FindErrorIndex(ERR_ELECTRICAL_MMU_PULLEY_SELFTEST_FAILED);
-    } else if (ec & (uint16_t)ErrorCode::TMC_SELECTOR_BIT) {
-        if ((ec & (uint16_t)ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION) == (uint16_t)ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION)
+    } else if (ContainsBit(ec, ErrorCode::TMC_SELECTOR_BIT)) {
+        if ((ec & ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION) == ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION)
             return FindErrorIndex(ERR_ELECTRICAL_MMU_SELECTOR_SELFTEST_FAILED);
-    } else if (ec & (uint16_t)ErrorCode::TMC_IDLER_BIT) {
-        if ((ec & (uint16_t)ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION) == (uint16_t)ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION)
+    } else if (ContainsBit(ec, ErrorCode::TMC_IDLER_BIT)) {
+        if ((ec & ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION) == ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION)
             return FindErrorIndex(ERR_ELECTRICAL_MMU_IDLER_SELFTEST_FAILED);
     }
 
     // TMC-related errors - multiple of these can occur at once
     // - in such a case we report the first which gets found/converted into Prusa-Error-Codes (usually the fact, that one TMC has an issue is serious enough)
     // By carefully ordering the checks here we can prioritize the errors being reported to the user.
-    if (ec & (uint16_t)ErrorCode::TMC_PULLEY_BIT) {
-        if (ec & (uint16_t)ErrorCode::TMC_IOIN_MISMATCH)
+    if (ContainsBit(ec, ErrorCode::TMC_PULLEY_BIT)) {
+        if (ContainsBit(ec, ErrorCode::TMC_IOIN_MISMATCH))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_PULLEY_DRIVER_ERROR);
-        if (ec & (uint16_t)ErrorCode::TMC_RESET)
+        if (ContainsBit(ec, ErrorCode::TMC_RESET))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_PULLEY_DRIVER_RESET);
-        if (ec & (uint16_t)ErrorCode::TMC_UNDERVOLTAGE_ON_CHARGE_PUMP)
+        if (ContainsBit(ec, ErrorCode::TMC_UNDERVOLTAGE_ON_CHARGE_PUMP))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_PULLEY_UNDERVOLTAGE_ERROR);
-        if (ec & (uint16_t)ErrorCode::TMC_SHORT_TO_GROUND)
+        if (ContainsBit(ec, ErrorCode::TMC_SHORT_TO_GROUND))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_PULLEY_DRIVER_SHORTED);
-        if (ec & (uint16_t)ErrorCode::TMC_OVER_TEMPERATURE_WARN)
+        if (ContainsBit(ec, ErrorCode::TMC_OVER_TEMPERATURE_WARN))
             return FindErrorIndex(ERR_TEMPERATURE_WARNING_TMC_PULLEY_TOO_HOT);
-        if (ec & (uint16_t)ErrorCode::TMC_OVER_TEMPERATURE_ERROR)
+        if (ContainsBit(ec, ErrorCode::TMC_OVER_TEMPERATURE_ERROR))
             return FindErrorIndex(ERR_TEMPERATURE_TMC_PULLEY_OVERHEAT_ERROR);
-    } else if (ec & (uint16_t)ErrorCode::TMC_SELECTOR_BIT) {
-        if (ec & (uint16_t)ErrorCode::TMC_IOIN_MISMATCH)
+    } else if (ContainsBit(ec, ErrorCode::TMC_SELECTOR_BIT)) {
+        if (ContainsBit(ec, ErrorCode::TMC_IOIN_MISMATCH))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_SELECTOR_DRIVER_ERROR);
-        if (ec & (uint16_t)ErrorCode::TMC_RESET)
+        if (ContainsBit(ec, ErrorCode::TMC_RESET))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_SELECTOR_DRIVER_RESET);
-        if (ec & (uint16_t)ErrorCode::TMC_UNDERVOLTAGE_ON_CHARGE_PUMP)
+        if (ContainsBit(ec, ErrorCode::TMC_UNDERVOLTAGE_ON_CHARGE_PUMP))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_SELECTOR_UNDERVOLTAGE_ERROR);
-        if (ec & (uint16_t)ErrorCode::TMC_SHORT_TO_GROUND)
+        if (ContainsBit(ec, ErrorCode::TMC_SHORT_TO_GROUND))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_SELECTOR_DRIVER_SHORTED);
-        if (ec & (uint16_t)ErrorCode::TMC_OVER_TEMPERATURE_WARN)
+        if (ContainsBit(ec, ErrorCode::TMC_OVER_TEMPERATURE_WARN))
             return FindErrorIndex(ERR_TEMPERATURE_WARNING_TMC_SELECTOR_TOO_HOT);
-        if (ec & (uint16_t)ErrorCode::TMC_OVER_TEMPERATURE_ERROR)
+        if (ContainsBit(ec, ErrorCode::TMC_OVER_TEMPERATURE_ERROR))
             return FindErrorIndex(ERR_TEMPERATURE_TMC_SELECTOR_OVERHEAT_ERROR);
-    } else if (ec & (uint16_t)ErrorCode::TMC_IDLER_BIT) {
-        if (ec & (uint16_t)ErrorCode::TMC_IOIN_MISMATCH)
+    } else if (ContainsBit(ec, ErrorCode::TMC_IDLER_BIT)) {
+        if (ContainsBit(ec, ErrorCode::TMC_IOIN_MISMATCH))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_IDLER_DRIVER_ERROR);
-        if (ec & (uint16_t)ErrorCode::TMC_RESET)
+        if (ContainsBit(ec, ErrorCode::TMC_RESET))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_IDLER_DRIVER_RESET);
-        if (ec & (uint16_t)ErrorCode::TMC_UNDERVOLTAGE_ON_CHARGE_PUMP)
+        if (ContainsBit(ec, ErrorCode::TMC_UNDERVOLTAGE_ON_CHARGE_PUMP))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_IDLER_UNDERVOLTAGE_ERROR);
-        if (ec & (uint16_t)ErrorCode::TMC_SHORT_TO_GROUND)
+        if (ContainsBit(ec, ErrorCode::TMC_SHORT_TO_GROUND))
             return FindErrorIndex(ERR_ELECTRICAL_TMC_IDLER_DRIVER_SHORTED);
-        if (ec & (uint16_t)ErrorCode::TMC_OVER_TEMPERATURE_WARN)
+        if (ContainsBit(ec, ErrorCode::TMC_OVER_TEMPERATURE_WARN))
             return FindErrorIndex(ERR_TEMPERATURE_WARNING_TMC_IDLER_TOO_HOT);
-        if (ec & (uint16_t)ErrorCode::TMC_OVER_TEMPERATURE_ERROR)
+        if (ContainsBit(ec, ErrorCode::TMC_OVER_TEMPERATURE_ERROR))
             return FindErrorIndex(ERR_TEMPERATURE_TMC_IDLER_OVERHEAT_ERROR);
     }
 
@@ -184,16 +193,16 @@ struct ResetOnExit {
     }
 };
 
-Buttons ButtonPressed(uint16_t ec) {
+Buttons ButtonPressed(ErrorCode ec) {
     if (buttonSelectedOperation == ButtonOperations::NoOperation) {
-        return NoButton; // no button
+        return Buttons::NoButton; // no button
     }
     
     ResetOnExit ros; // clear buttonSelectedOperation on exit from this call
     return ButtonAvailable(ec);
 }
 
-Buttons ButtonAvailable(uint16_t ec) {
+Buttons ButtonAvailable(ErrorCode ec) {
     uint8_t ei = PrusaErrorCodeIndex(ec);
     
     // The list of responses which occur in mmu error dialogs
@@ -214,7 +223,7 @@ Buttons ButtonAvailable(uint16_t ec) {
         switch (buttonSelectedOperation) {
         // may be allow move selector right and left in the future
         case ButtonOperations::Retry: // "Repeat action"
-            return Middle;
+            return Buttons::Middle;
         default:
             break;
         }
@@ -224,9 +233,9 @@ Buttons ButtonAvailable(uint16_t ec) {
         switch (buttonSelectedOperation) {
         // may be allow move selector right and left in the future
         case ButtonOperations::Tune: // Tune Stallguard threshold
-            return TuneMMU;
+            return Buttons::TuneMMU;
         case ButtonOperations::Retry: // "Repeat action"
-            return Middle;
+            return Buttons::Middle;
         default:
             break;
         }
@@ -235,7 +244,7 @@ Buttons ButtonAvailable(uint16_t ec) {
     case ERR_SYSTEM_FILAMENT_EJECTED:
         switch (buttonSelectedOperation) {
         case ButtonOperations::Continue: // User solved the serious mechanical problem by hand - there is no other way around
-            return Middle;
+            return Buttons::Middle;
         default:
             break;
         }
@@ -243,9 +252,9 @@ Buttons ButtonAvailable(uint16_t ec) {
     case ERR_SYSTEM_FILAMENT_CHANGE:
         switch (buttonSelectedOperation) {
         case ButtonOperations::Load:
-            return Load;
+            return Buttons::Load;
         case ButtonOperations::Eject:
-            return Eject;
+            return Buttons::Eject;
         default:
             break;
         }
@@ -255,9 +264,9 @@ Buttons ButtonAvailable(uint16_t ec) {
     case ERR_TEMPERATURE_WARNING_TMC_IDLER_TOO_HOT:
         switch (buttonSelectedOperation) {
         case ButtonOperations::Continue: // "Continue"
-            return Left;
+            return Buttons::Left;
         case ButtonOperations::ResetMMU: // "Reset MMU"
-            return ResetMMU;
+            return Buttons::ResetMMU;
         default:
             break;
         }
@@ -292,7 +301,7 @@ Buttons ButtonAvailable(uint16_t ec) {
     case ERR_ELECTRICAL_MMU_MCU_ERROR:
         switch (buttonSelectedOperation) {
         case ButtonOperations::ResetMMU: // "Reset MMU"
-            return ResetMMU;
+            return Buttons::ResetMMU;
         default:
             break;
         }
@@ -302,9 +311,9 @@ Buttons ButtonAvailable(uint16_t ec) {
     case ERR_SYSTEM_FW_UPDATE_NEEDED:
         switch (buttonSelectedOperation) {
         case ButtonOperations::DisableMMU: // "Disable"
-            return DisableMMU;
+            return Buttons::DisableMMU;
         case ButtonOperations::ResetMMU: // "ResetMMU"
-            return ResetMMU;
+            return Buttons::ResetMMU;
         default:
             break;
         }
@@ -312,9 +321,9 @@ Buttons ButtonAvailable(uint16_t ec) {
     case ERR_SYSTEM_FILAMENT_ALREADY_LOADED:
         switch (buttonSelectedOperation) {
         case ButtonOperations::Unload: // "Unload"
-            return Left;
+            return Buttons::Left;
         case ButtonOperations::Continue: // "Proceed/Continue"
-            return Right;
+            return Buttons::Right;
         default:
             break;
         }
@@ -323,9 +332,9 @@ Buttons ButtonAvailable(uint16_t ec) {
     case ERR_SYSTEM_INVALID_TOOL:
         switch (buttonSelectedOperation) {
         case ButtonOperations::StopPrint: // "Stop print"
-            return StopPrint;
+            return Buttons::StopPrint;
         case ButtonOperations::ResetMMU: // "Reset MMU"
-            return ResetMMU;
+            return Buttons::ResetMMU;
         default:
             break;
         }
@@ -335,7 +344,7 @@ Buttons ButtonAvailable(uint16_t ec) {
         break;
     }
     
-    return NoButton;
+    return Buttons::NoButton;
 }
 
 void SetButtonResponse(ButtonOperations rsp){
diff --git a/Firmware/mmu2_error_converter.h b/Firmware/mmu2_error_converter.h
index 5f24bcb5c..fea2ba2a1 100644
--- a/Firmware/mmu2_error_converter.h
+++ b/Firmware/mmu2_error_converter.h
@@ -1,13 +1,20 @@
 #pragma once
 #include <stdint.h>
 #include <stddef.h>
-#include "mmu2/buttons.h"
+#ifdef __AVR__
+    #include "mmu2/buttons.h"
+    #include "mmu2/error_codes.h"
+#else
+    #include "buttons.h"
+    #include "../../../../../../Prusa-Error-Codes/04_MMU/button_operations.h"
+    #include "../../../../../../Prusa-Firmware-MMU/src/logic/error_codes.h"
+#endif
 
 namespace MMU2 {
 
 /// Translates MMU2::ErrorCode into an index of Prusa-Error-Codes
 /// Basically this is the way to obtain an index into all other functions in this API
-uint8_t PrusaErrorCodeIndex(uint16_t ec);
+uint8_t PrusaErrorCodeIndex(ErrorCode ec);
 
 /// @returns pointer to a PROGMEM string representing the Title of the Prusa-Error-Codes error
 /// @param i index of the error - obtained by calling ErrorCodeIndex
@@ -38,11 +45,11 @@ void SetButtonResponse(ButtonOperations rsp);
 
 /// @returns button index/code based on currently processed error/screen
 /// Clears the "pressed" button upon exit
-Buttons ButtonPressed(uint16_t ec);
+Buttons ButtonPressed(ErrorCode ec);
 
 /// @returns button index/code based on currently processed error/screen
 /// Used as a subfunction of ButtonPressed.
 /// Does not clear the "pressed" button upon exit
-Buttons ButtonAvailable(uint16_t ec);
+Buttons ButtonAvailable(ErrorCode ec);
 
 } // namespace MMU2
diff --git a/Firmware/mmu2_log.h b/Firmware/mmu2_log.h
index bc4313294..1d18a2b6d 100644
--- a/Firmware/mmu2_log.h
+++ b/Firmware/mmu2_log.h
@@ -1,7 +1,7 @@
 #pragma once
-
-#ifndef UNITTEST
-#include "Marlin.h"
+#ifdef __AVR__
+    #include <avr/pgmspace.h>
+#endif
 
 // Beware - before changing this prefix, think twice
 // you'd need to change appmain.cpp app_marlin_serial_output_write_hook
@@ -14,16 +14,22 @@ namespace MMU2 {
 /// @param msg pointer to a string in PROGMEM
 /// On the AVR platform this variant reads the input string from PROGMEM.
 /// On the ARM platform it calls LogErrorEvent directly (silently expecting the compiler to optimize it away)
-void LogErrorEvent_P(const char *msg);
+void LogErrorEvent_P(const char *msg_P);
 
 /// Report the msg into the general logging subsystem (through Marlin's SERIAL_ECHO stuff)
 /// @param msg pointer to a string in PROGMEM
 /// On the AVR platform this variant reads the input string from PROGMEM.
 /// On the ARM platform it calls LogErrorEvent directly (silently expecting the compiler to optimize it away)
-void LogEchoEvent_P(const char *msg);
+void LogEchoEvent_P(const char *msg_P);
 
-} // namespace
+} // namespace MMU2
 
+#ifndef UNITTEST
+    #ifdef __AVR__
+        #include "Marlin.h"
+    #else
+        #include "../../core/serial.h"
+    #endif
     #define SERIAL_MMU2() \
         { serialprintPGM(mmu2Magic); }
 
@@ -49,11 +55,14 @@ void LogEchoEvent_P(const char *msg);
         } while (0)
     #define MMU2_ERROR_MSG(S) MMU2_ECHO_MSG(S) //!@todo Decide MMU errors  on serial line
 
-#else // #ifndef UNITTEST
+#else                                          // #ifndef UNITTEST
+    #include "stubs/stub_interfaces.h"
+    #define MMU2_ECHO_MSGLN(S)    marlinLogSim.AppendLine(S)
+    #define MMU2_ERROR_MSGLN(S)   marlinLogSim.AppendLine(S)
+    #define MMU2_ECHO_MSGRPGM(S)  /*marlinLogSim.AppendLine(S)*/
+    #define MMU2_ERROR_MSGRPGM(S) /*marlinLogSim.AppendLine(S)*/
+    #define SERIAL_ECHOLNPGM(S)   /*marlinLogSim.AppendLine(S)*/
+    #define SERIAL_ECHOPGM(S)     /* */
+    #define SERIAL_ECHOLN(S)      /*marlinLogSim.AppendLine(S)*/
 
-    #define MMU2_ECHO_MSGLN(S)    /* */
-    #define MMU2_ERROR_MSGLN(S)   /* */
-    #define MMU2_ECHO_MSGRPGM(S)  /* */
-    #define MMU2_ERROR_MSGRPGM(S) /* */
-
-#endif // #ifndef UNITTEST
+#endif                            // #ifndef UNITTEST
diff --git a/Firmware/mmu2_marlin.h b/Firmware/mmu2_marlin.h
index 0877a60f8..40aeee0f2 100644
--- a/Firmware/mmu2_marlin.h
+++ b/Firmware/mmu2_marlin.h
@@ -14,27 +14,25 @@ struct pos3d {
     pos3d() = default;
     inline constexpr pos3d(float x, float y, float z)
         : xyz { x, y, z } {}
-    pos3d operator=(const float *newP){
-        for(uint8_t i = 0; i < 3; ++i){
+    pos3d operator=(const float *newP) {
+        for (uint8_t i = 0; i < 3; ++i) {
             xyz[i] = newP[i];
         }
         return *this;
     }
 };
 
-void MoveE(float delta, float feedRate);
+void extruder_move(float distance, float feed_rate);
+void extruder_schedule_turning(float feed_rate);
 
-float MoveRaiseZ(float delta);
+float move_raise_z(float delta);
 
 void planner_abort_queued_moves();
 void planner_synchronize();
 bool planner_any_moves();
-float planner_get_machine_position_E_mm();
 float stepper_get_machine_position_E_mm();
 float planner_get_current_position_E();
 void planner_set_current_position_E(float e);
-void planner_line_to_current_position(float feedRate_mm_s);
-void planner_line_to_current_position_sync(float feedRate_mm_s);
 pos3d planner_current_position();
 
 void motion_do_blocking_move_to_xy(float rx, float ry, float feedRate_mm_s);
@@ -46,6 +44,9 @@ bool marlin_printingIsActive();
 void marlin_manage_heater();
 void marlin_manage_inactivity(bool b);
 void marlin_idle(bool b);
+void marlin_refresh_print_state_in_ram();
+void marlin_clear_print_state_in_ram();
+void marlin_stop_and_save_print_to_ram();
 
 int16_t thermal_degTargetHotend();
 int16_t thermal_degHotend();
@@ -61,6 +62,4 @@ bool all_axes_homed();
 
 void gcode_reset_stepper_timeout();
 
-bool cutter_enabled();
-
 } // namespace MMU2
diff --git a/Firmware/mmu2_marlin1.cpp b/Firmware/mmu2_marlin1.cpp
index 58ce99171..3a7dfa837 100644
--- a/Firmware/mmu2_marlin1.cpp
+++ b/Firmware/mmu2_marlin1.cpp
@@ -9,12 +9,21 @@
 
 namespace MMU2 {
 
-void MoveE(float delta, float feedRate) {
+static void planner_line_to_current_position(float feedRate_mm_s){
+     plan_buffer_line_curposXYZE(feedRate_mm_s);
+}
+
+static void planner_line_to_current_position_sync(float feedRate_mm_s){
+    planner_line_to_current_position(feedRate_mm_s);
+    planner_synchronize();
+}
+
+void extruder_move(float delta, float feedRate) {
     current_position[E_AXIS] += delta;
     planner_line_to_current_position(feedRate);
 }
 
-float MoveRaiseZ(float delta) {
+float move_raise_z(float delta) {
     return raise_z(delta);
 }
 
@@ -53,15 +62,6 @@ void planner_set_current_position_E(float e){
         current_position[E_AXIS] = e;
 }
 
-void planner_line_to_current_position(float feedRate_mm_s){
-     plan_buffer_line_curposXYZE(feedRate_mm_s);
-}
-
-void planner_line_to_current_position_sync(float feedRate_mm_s){
-    planner_line_to_current_position(feedRate_mm_s);
-    planner_synchronize();
-}
-
 pos3d planner_current_position(){
     return pos3d(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
 }
@@ -101,6 +101,18 @@ void marlin_idle(bool b){
     manage_inactivity(b);
 }
 
+void marlin_refresh_print_state_in_ram(){
+    refresh_print_state_in_ram();
+}
+
+void marlin_clear_print_state_in_ram(){
+    clear_print_state_in_ram();
+}
+
+void marlin_stop_and_save_print_to_ram(){
+    stop_and_save_print_to_ram(0,0);
+}
+
 int16_t thermal_degTargetHotend() {
     return degTargetHotend(0);
 }
@@ -132,8 +144,4 @@ bool all_axes_homed(){
     return axis_known_position[X_AXIS] && axis_known_position[Y_AXIS];
 }
 
-bool cutter_enabled(){
-    return eeprom_read_byte((uint8_t*)EEPROM_MMU_CUTTER_ENABLED) == EEPROM_MMU_CUTTER_ENABLED_enabled;
-}
-
 } // namespace MMU2
diff --git a/Firmware/mmu2_marlin_macros.h b/Firmware/mmu2_marlin_macros.h
index af2f79504..38f515e66 100644
--- a/Firmware/mmu2_marlin_macros.h
+++ b/Firmware/mmu2_marlin_macros.h
@@ -8,6 +8,10 @@
     // brings _O and _T macros into MMU
     #include "language.h"
     #define MARLIN_KEEPALIVE_STATE_IN_PROCESS KEEPALIVE_STATE(IN_PROCESS)
+#elif defined(UNITTEST)
+    #define _O(x)                             x
+    #define _T(x)                             x
+    #define MARLIN_KEEPALIVE_STATE_IN_PROCESS /*KEEPALIVE_STATE(IN_PROCESS) TODO*/
 #else
     #include "../../gcode/gcode.h"
     #define _O(x)                             x
diff --git a/Firmware/mmu2_progress_converter.cpp b/Firmware/mmu2_progress_converter.cpp
index b44951385..29fd0d250 100644
--- a/Firmware/mmu2_progress_converter.cpp
+++ b/Firmware/mmu2_progress_converter.cpp
@@ -62,10 +62,10 @@ static const char * const progressTexts[] PROGMEM = {
     _R(MSG_PROGRESS_FEED_FSENSOR)
 };
 
-const char * ProgressCodeToText(uint16_t pc){
+const char * ProgressCodeToText(ProgressCode pc){
     // @@TODO ?? a better fallback option?
-    return ( pc <= (sizeof(progressTexts) / sizeof(progressTexts[0])) )
-       ? static_cast<const char *>(pgm_read_ptr(&progressTexts[pc]))
+    return ( (uint16_t)pc <= (sizeof(progressTexts) / sizeof(progressTexts[0])) )
+       ? static_cast<const char *>(pgm_read_ptr(&progressTexts[(uint16_t)pc]))
        : static_cast<const char *>(pgm_read_ptr(&progressTexts[0]));
 }
 
diff --git a/Firmware/mmu2_progress_converter.h b/Firmware/mmu2_progress_converter.h
index 19cb1e3fc..560cc5f7d 100644
--- a/Firmware/mmu2_progress_converter.h
+++ b/Firmware/mmu2_progress_converter.h
@@ -1,9 +1,14 @@
 #pragma once
 #include <stdint.h>
 #include <stddef.h>
+#ifdef __AVR__
+    #include "mmu2/progress_codes.h"
+#else
+    #include "../../../../../../Prusa-Firmware-MMU/src/logic/progress_codes.h"
+#endif
 
 namespace MMU2 {
 
-const char * ProgressCodeToText(uint16_t pc);
+const char *ProgressCodeToText(ProgressCode pc);
 
 }
diff --git a/Firmware/mmu2_protocol_logic.cpp b/Firmware/mmu2_protocol_logic.cpp
index 82b28b2f1..77a0da950 100644
--- a/Firmware/mmu2_protocol_logic.cpp
+++ b/Firmware/mmu2_protocol_logic.cpp
@@ -9,6 +9,11 @@
     // irrelevant on Buddy FW, just keep "_millis" as "millis"
     #include <wiring_time.h>
     #define _millis millis
+    #ifdef UNITTEST
+        #define strncmp_P strncmp
+    #else
+        #include <Marlin/src/core/serial.h>
+    #endif
 #endif
 
 #include <string.h>
@@ -16,7 +21,7 @@
 
 namespace MMU2 {
 
-/// Beware:
+/// Beware - on AVR/MK3S:
 /// Changing the supportedMmuVersion numbers requires patching MSG_DESC_FW_UPDATE_NEEDED and all its related translations by hand.
 ///
 /// The message reads:
@@ -27,18 +32,18 @@ namespace MMU2 {
 static constexpr uint8_t supportedMmuFWVersion[3] PROGMEM = { mmuVersionMajor, mmuVersionMinor, mmuVersionPatch };
 
 const Register ProtocolLogic::regs8Addrs[ProtocolLogic::regs8Count] PROGMEM = {
-    Register::FINDA_State,    // FINDA state
+    Register::FINDA_State,           // FINDA state
     Register::Set_Get_Selector_Slot, // Selector slot
-    Register::Set_Get_Idler_Slot, // Idler slot
+    Register::Set_Get_Idler_Slot,    // Idler slot
 };
 
 const Register ProtocolLogic::regs16Addrs[ProtocolLogic::regs16Count] PROGMEM = {
-    Register::MMU_Errors,    // MMU errors - aka statistics
+    Register::MMU_Errors,          // MMU errors - aka statistics
     Register::Get_Pulley_Position, // Pulley position [mm]
 };
 
 const Register ProtocolLogic::initRegs8Addrs[ProtocolLogic::initRegs8Count] PROGMEM = {
-    Register::Extra_Load_Distance, // extra load distance [mm]
+    Register::Extra_Load_Distance,  // extra load distance [mm]
     Register::Pulley_Slow_Feedrate, // pulley slow feedrate [mm/s]
 };
 
@@ -181,7 +186,7 @@ StepStatus ProtocolLogic::ExpectingMessage() {
                 break;
             }
         }
-            [[fallthrough]]; // otherwise
+            [[fallthrough]];      // otherwise
         default:
             RecordUARTActivity(); // something has happened on the UART, update the timeout record
             return ProtocolError;
@@ -197,7 +202,11 @@ StepStatus ProtocolLogic::ExpectingMessage() {
 }
 
 void ProtocolLogic::SendMsg(RequestMsg rq) {
+#ifdef __AVR__
+    // Buddy FW cannot use stack-allocated txbuff - DMA doesn't work with CCMRAM
+    // No restrictions on MK3/S/+ though
     uint8_t txbuff[Protocol::MaxRequestSize()];
+#endif
     uint8_t len = Protocol::EncodeRequest(rq, txbuff);
     uart->write(txbuff, len);
     LogRequestMsg(txbuff, len);
@@ -205,7 +214,11 @@ void ProtocolLogic::SendMsg(RequestMsg rq) {
 }
 
 void ProtocolLogic::SendWriteMsg(RequestMsg rq) {
+#ifdef __AVR__
+    // Buddy FW cannot use stack-allocated txbuff - DMA doesn't work with CCMRAM
+    // No restrictions on MK3/S/+ though
     uint8_t txbuff[Protocol::MaxRequestSize()];
+#endif
     uint8_t len = Protocol::EncodeWriteRequest(rq.value, rq.value2, txbuff);
     uart->write(txbuff, len);
     LogRequestMsg(txbuff, len);
@@ -275,9 +288,9 @@ StepStatus ProtocolLogic::ScopeStep() {
         case Scope::StartSeq:
             return StartSeqStep(); // ~270B
         case Scope::Idle:
-            return IdleStep(); // ~300B
+            return IdleStep();     // ~300B
         case Scope::Command:
-            return CommandStep(); // ~430B
+            return CommandStep();  // ~430B
         case Scope::Stopped:
             return StoppedStep();
         default:
@@ -322,7 +335,7 @@ StepStatus ProtocolLogic::StartSeqStep() {
 StepStatus ProtocolLogic::DelayedRestartWait() {
     if (Elapsed(heartBeatPeriod)) { // this basically means, that we are waiting until there is some traffic on
         while (uart->read() != -1)
-            ; // clear the input buffer
+            ;                       // clear the input buffer
         // switch to StartSeq
         Start();
     }
@@ -349,6 +362,7 @@ StepStatus ProtocolLogic::ProcessCommandQueryResponse() {
         return Processing;
     case ResponseMsgParamCodes::Error:
         // in case of an error the progress code remains as it has been before
+        progressCode = ProgressCode::ERRWaitingForUser;
         errorCode = static_cast<ErrorCode>(rsp.paramValue);
         // keep on reporting the state of fsensor regularly even in command error state
         // - the MMU checks FINDA and fsensor even while recovering from errors
@@ -469,9 +483,11 @@ StepStatus ProtocolLogic::IdleStep() {
             case ResponseMsgParamCodes::Processing:
                 // @@TODO we may actually use this branch to report progress of manual operation on the MMU
                 // The MMU sends e.g. X0 P27 after its restart when the user presses an MMU button to move the Selector
+                progressCode = static_cast<ProgressCode>(rsp.paramValue);
                 errorCode = ErrorCode::OK;
                 break;
             default:
+                progressCode = ProgressCode::ERRWaitingForUser;
                 errorCode = static_cast<ErrorCode>(rsp.paramValue);
                 StartReading8bitRegisters(); // continue Idle state without restarting the communication
                 return CommandError;
@@ -532,7 +548,7 @@ ProtocolLogic::ProtocolLogic(MMU2Serial *uart, uint8_t extraLoadDistance, uint8_
     , uart(uart)
     , errorCode(ErrorCode::OK)
     , progressCode(ProgressCode::OK)
-    , buttonCode(NoButton)
+    , buttonCode(Buttons::NoButton)
     , lastFSensor((uint8_t)WhereIsFilament())
     , regIndex(0)
     , retryAttempts(MAX_RETRIES)
@@ -758,6 +774,7 @@ void ProtocolLogic::LogResponse() {
 StepStatus ProtocolLogic::SuppressShortDropOuts(const char *msg_P, StepStatus ss) {
     if (dataTO.Record(ss)) {
         LogError(msg_P);
+        dataTO.Reset(); // prepare for another run of consecutive retries before firing an error
         return dataTO.InitialCause();
     } else {
         return Processing; // suppress short drop outs of communication
diff --git a/Firmware/mmu2_protocol_logic.h b/Firmware/mmu2_protocol_logic.h
index 251674ce8..bf884106b 100644
--- a/Firmware/mmu2_protocol_logic.h
+++ b/Firmware/mmu2_protocol_logic.h
@@ -3,37 +3,39 @@
 #include <avr/pgmspace.h>
 
 #ifdef __AVR__
-#include "mmu2/error_codes.h"
-#include "mmu2/progress_codes.h"
-#include "mmu2/buttons.h"
-#include "mmu2/registers.h"
-#include "mmu2_protocol.h"
+    #include "mmu2/error_codes.h"
+    #include "mmu2/progress_codes.h"
+    #include "mmu2/buttons.h"
+    #include "mmu2/registers.h"
+    #include "mmu2_protocol.h"
 
 // #include <array> std array is not available on AVR ... we need to "fake" it
 namespace std {
-template<typename T, uint8_t N>
+template <typename T, uint8_t N>
 class array {
     T data[N];
+
 public:
     array() = default;
-    inline constexpr T* begin()const { return data; }
-    inline constexpr T* end()const { return data + N; }
+    inline constexpr T *begin() const { return data; }
+    inline constexpr T *end() const { return data + N; }
     static constexpr uint8_t size() { return N; }
-    inline T &operator[](uint8_t i){
+    inline T &operator[](uint8_t i) {
         return data[i];
     }
 };
-}
+} // namespace std
 #else
 
-#include <array>
-#include "../../../../../../Prusa-Firmware-MMU/src/logic/error_codes.h"
-#include "../../../../../../Prusa-Firmware-MMU/src/logic/progress_codes.h"
+    #include <array>
+    #include "../../../../../../Prusa-Firmware-MMU/src/logic/error_codes.h"
+    #include "../../../../../../Prusa-Firmware-MMU/src/logic/progress_codes.h"
 
-// prevent ARM HAL macros from breaking our code
-#undef CRC
-#include "../../../../../../Prusa-Firmware-MMU/src/modules/protocol.h"
-#include "buttons.h"
+    // prevent ARM HAL macros from breaking our code
+    #undef CRC
+    #include "../../../../../../Prusa-Firmware-MMU/src/modules/protocol.h"
+    #include "buttons.h"
+    #include "registers.h"
 #endif
 
 #include "mmu2_serial.h"
@@ -50,9 +52,9 @@ class ProtocolLogic;
 /// ProtocolLogic stepping statuses
 enum StepStatus : uint_fast8_t {
     Processing = 0,
-    MessageReady, ///< a message has been successfully decoded from the received bytes
-    Finished, ///< Scope finished successfully
-    Interrupted, ///< received "Finished" message related to a different command than originally issued (most likely the MMU restarted while doing something)
+    MessageReady,         ///< a message has been successfully decoded from the received bytes
+    Finished,             ///< Scope finished successfully
+    Interrupted,          ///< received "Finished" message related to a different command than originally issued (most likely the MMU restarted while doing something)
     CommunicationTimeout, ///< the MMU failed to respond to a request within a specified time frame
     ProtocolError,        ///< bytes read from the MMU didn't form a valid response
     CommandRejected,      ///< the MMU rejected the command due to some other command in progress, may be the user is operating the MMU locally (button commands)
@@ -60,19 +62,17 @@ enum StepStatus : uint_fast8_t {
     VersionMismatch,      ///< the MMU reports its firmware version incompatible with our implementation
     PrinterError,         ///< printer's explicit error - MMU is fine, but the printer was unable to complete the requested operation
     CommunicationRecovered,
-    ButtonPushed, ///< The MMU reported the user pushed one of its three buttons.
+    ButtonPushed,         ///< The MMU reported the user pushed one of its three buttons.
 };
 
-static constexpr uint32_t linkLayerTimeout = 2000;                 ///< default link layer communication timeout
-static constexpr uint32_t dataLayerTimeout = linkLayerTimeout * 3; ///< data layer communication timeout
-static constexpr uint32_t heartBeatPeriod = linkLayerTimeout / 2;  ///< period of heart beat messages (Q0)
+inline constexpr uint32_t linkLayerTimeout = 2000;                 ///< default link layer communication timeout
+inline constexpr uint32_t dataLayerTimeout = linkLayerTimeout * 3; ///< data layer communication timeout
+inline constexpr uint32_t heartBeatPeriod = linkLayerTimeout / 2;  ///< period of heart beat messages (Q0)
 
 static_assert(heartBeatPeriod < linkLayerTimeout && linkLayerTimeout < dataLayerTimeout, "Incorrect ordering of timeouts");
 
 ///< Filter of short consecutive drop outs which are recovered instantly
 class DropOutFilter {
-    StepStatus cause;
-    uint8_t occurrences;
 public:
     static constexpr uint8_t maxOccurrences = 10; // ideally set this to >8 seconds -> 12x heartBeatPeriod
     static_assert(maxOccurrences > 1, "we should really silently ignore at least 1 comm drop out if recovered immediately afterwards");
@@ -86,6 +86,10 @@ public:
 
     /// Rearms the object for further processing - basically call this once the MMU responds with something meaningful (e.g. S0 A2)
     inline void Reset() { occurrences = maxOccurrences; }
+
+private:
+    StepStatus cause;
+    uint8_t occurrences = maxOccurrences;
 };
 
 /// Logic layer of the MMU vs. printer communication protocol
@@ -115,11 +119,11 @@ public:
     /// Sets the extra load distance to be reported to the MMU.
     /// Beware - this call doesn't send anything to the MMU.
     /// The MMU gets the newly set value either by a communication restart or via an explicit WriteRegister call
-    inline void PlanExtraLoadDistance(uint8_t eld_mm){
+    inline void PlanExtraLoadDistance(uint8_t eld_mm) {
         initRegs8[0] = eld_mm;
     }
     /// @returns the currently preset extra load distance
-    inline uint8_t ExtraLoadDistance()const {
+    inline uint8_t ExtraLoadDistance() const {
         return initRegs8[0];
     }
 
@@ -187,13 +191,13 @@ public:
         inAutoRetry = iar;
     }
 
-    inline void SetPrinterError(ErrorCode ec){
+    inline void SetPrinterError(ErrorCode ec) {
         explicitPrinterError = ec;
     }
-    inline void ClearPrinterError(){
+    inline void ClearPrinterError() {
         explicitPrinterError = ErrorCode::OK;
     }
-    inline bool IsPrinterError()const {
+    inline bool IsPrinterError() const {
         return explicitPrinterError != ErrorCode::OK;
     }
     inline ErrorCode PrinterError() const {
@@ -228,15 +232,6 @@ private:
         Running       ///< normal operation - Idle + Command processing
     };
 
-    // individual sub-state machines - may be they can be combined into a union since only one is active at once
-    // or we can blend them into ProtocolLogic at the cost of a less nice code (but hopefully shorter)
-//    Stopped stopped;
-//    StartSeq startSeq;
-//    DelayedRestart delayedRestart;
-//    Idle idle;
-//    Command command;
-//    ProtocolLogicPartBase *currentState; ///< command currently being processed
-    
     enum class Scope : uint_fast8_t {
         Stopped,
         StartSeq,
@@ -350,25 +345,30 @@ private:
     /// Activate the planned state once the immediate response to a sent request arrived
     bool ActivatePlannedRequest();
 
-    uint32_t lastUARTActivityMs; ///< timestamp - last ms when something occurred on the UART
-    DropOutFilter dataTO;        ///< Filter of short consecutive drop outs which are recovered instantly
+    uint32_t lastUARTActivityMs;               ///< timestamp - last ms when something occurred on the UART
+    DropOutFilter dataTO;                      ///< Filter of short consecutive drop outs which are recovered instantly
 
-    ResponseMsg rsp; ///< decoded response message from the MMU protocol
+    ResponseMsg rsp;                           ///< decoded response message from the MMU protocol
 
-    State state; ///< internal state of ProtocolLogic
+    State state;                               ///< internal state of ProtocolLogic
 
-    Protocol protocol; ///< protocol codec
+    Protocol protocol;                         ///< protocol codec
 
     std::array<uint8_t, 16> lastReceivedBytes; ///< remembers the last few bytes of incoming communication for diagnostic purposes
     uint8_t lrb;
 
-    MMU2Serial *uart; ///< UART interface
+    MMU2Serial *uart;          ///< UART interface
 
     ErrorCode errorCode;       ///< last received error code from the MMU
     ProgressCode progressCode; ///< last received progress code from the MMU
     Buttons buttonCode;        ///< Last received button from the MMU.
 
-    uint8_t lastFSensor; ///< last state of filament sensor
+    uint8_t lastFSensor;       ///< last state of filament sensor
+
+#ifndef __AVR__
+    uint8_t txbuff[Protocol::MaxRequestSize()]; ///< In Buddy FW - a static transmit buffer needs to exist as DMA cannot be used from CCMRAM.
+                                                ///< On MK3/S/+ the transmit buffer is allocated on the stack without restrictions
+#endif
 
     // 8bit registers
     static constexpr uint8_t regs8Count = 3;
diff --git a/Firmware/mmu2_reporting.cpp b/Firmware/mmu2_reporting.cpp
index d86f5abb1..0efe3341f 100644
--- a/Firmware/mmu2_reporting.cpp
+++ b/Firmware/mmu2_reporting.cpp
@@ -3,6 +3,7 @@
 #include "mmu2_log.h"
 #include "mmu2_reporting.h"
 #include "mmu2_error_converter.h"
+#include "mmu2_progress_converter.h"
 #include "mmu2/error_codes.h"
 #include "mmu2/buttons.h"
 #include "menu.h"
@@ -14,14 +15,12 @@
 
 namespace MMU2 {
 
-const char * ProgressCodeToText(uint16_t pc); // we may join progress convertor and reporter together
-
-void BeginReport(CommandInProgress /*cip*/, uint16_t ec) {
+void BeginReport(CommandInProgress /*cip*/, ProgressCode ec) {
     custom_message_type = CustomMsg::MMUProgress;
     lcd_setstatuspgm( _T(ProgressCodeToText(ec)) );
 }
 
-void EndReport(CommandInProgress /*cip*/, uint16_t /*ec*/) {
+void EndReport(CommandInProgress /*cip*/, ProgressCode /*ec*/) {
     // clear the status msg line - let the printed filename get visible again
     if (!printJobOngoing()) {
         lcd_setstatuspgm(MSG_WELCOME);
@@ -231,7 +230,7 @@ bool TuneMenuEntered() {
     return putErrorScreenToSleep;
 }
 
-void ReportErrorHook(CommandInProgress /*cip*/, uint16_t ec, uint8_t /*es*/) {
+void ReportErrorHook(CommandInProgress /*cip*/, ErrorCode ec, uint8_t /*es*/) {
     if (putErrorScreenToSleep) return;
     
     if (mmu2.MMUCurrentErrorCode() == ErrorCode::OK && mmu2.MMULastErrorSource() == MMU2::ErrorSourceMMU) {
@@ -241,7 +240,7 @@ void ReportErrorHook(CommandInProgress /*cip*/, uint16_t ec, uint8_t /*es*/) {
         ReportErrorHookState = ReportErrorHookStates::DISMISS_ERROR_SCREEN;
     }
 
-    const uint8_t ei = PrusaErrorCodeIndex(ec);
+    const uint8_t ei = PrusaErrorCodeIndex((ErrorCode)ec);
 
     switch ((uint8_t)ReportErrorHookState) {
     case (uint8_t)ReportErrorHookStates::RENDER_ERROR_SCREEN:
@@ -289,39 +288,57 @@ void ReportErrorHook(CommandInProgress /*cip*/, uint16_t ec, uint8_t /*es*/) {
     }
 }
 
-void ReportProgressHook(CommandInProgress cip, uint16_t ec) {
+void ReportProgressHook(CommandInProgress cip, ProgressCode ec) {
     if (cip != CommandInProgress::NoCommand) {
         custom_message_type = CustomMsg::MMUProgress;
         lcd_setstatuspgm( _T(ProgressCodeToText(ec)) );
     }
 }
 
-void TryLoadUnloadProgressbarInit() {
+TryLoadUnloadReporter::TryLoadUnloadReporter(float delta_mm)
+: dpixel0(0)
+, dpixel1(0)
+, lcd_cursor_col(0)
+, pixel_per_mm(0.5F * float(LCD_WIDTH) / (delta_mm))
+{
     lcd_clearstatus();
 }
 
-void TryLoadUnloadProgressbarDeinit() {
+TryLoadUnloadReporter::~TryLoadUnloadReporter() {
     // Delay the next status message just so
     // the user can see the results clearly
     lcd_reset_status_message_timeout();
 }
 
-void TryLoadUnloadProgressbarEcho() {
-    char buf[LCD_WIDTH];
+void TryLoadUnloadReporter::Render(uint8_t col, bool sensorState) {
+    // Set the cursor position each time in case some other
+    // part of the firmware changes the cursor position
+    lcd_insert_char_into_status(col, sensorState ? LCD_STR_SOLID_BLOCK[0] : '-');
+    if (!lcd_update_enabled) lcdui_print_status_line();
+}
+
+void TryLoadUnloadReporter::Progress(bool sensorState){
+    // Always round up, you can only have 'whole' pixels. (floor is also an option)
+    dpixel1 = ceil((stepper_get_machine_position_E_mm() - planner_get_current_position_E()) * pixel_per_mm);
+    if (dpixel1 - dpixel0) {
+        dpixel0 = dpixel1;
+        if (lcd_cursor_col > (LCD_WIDTH - 1)) lcd_cursor_col = LCD_WIDTH - 1;
+        Render(lcd_cursor_col++, sensorState);
+    }
+}
+
+void TryLoadUnloadReporter::DumpToSerial(){
+    char buf[LCD_WIDTH + 1];
     lcd_getstatus(buf);
     for (uint8_t i = 0; i < sizeof(buf); i++) {
         // 0xFF is -1 when converting from unsigned to signed char
         // If the number is negative, that means filament is present
         buf[i] = (buf[i] < 0) ? '1' : '0';
     }
+    buf[LCD_WIDTH] = 0;
     MMU2_ECHO_MSGLN(buf);
 }
 
-void TryLoadUnloadProgressbar(uint8_t col, bool sensorState) {
-    lcd_insert_char_into_status(col, sensorState ? '-' : LCD_STR_SOLID_BLOCK[0]);
-    if (!lcd_update_enabled) lcdui_print_status_line();
-}
-
 void IncrementLoadFails(){
     eeprom_increment_byte((uint8_t *)EEPROM_MMU_LOAD_FAIL);
     eeprom_increment_word((uint16_t *)EEPROM_MMU_LOAD_FAIL_TOT);
@@ -332,6 +349,10 @@ void IncrementMMUFails(){
     eeprom_increment_word((uint16_t *)EEPROM_MMU_FAIL_TOT);
 }
 
+bool cutter_enabled(){
+    return eeprom_read_byte((uint8_t*)EEPROM_MMU_CUTTER_ENABLED) == EEPROM_MMU_CUTTER_ENABLED_enabled;
+}
+
 void MakeSound(SoundType s){
     Sound_MakeSound( (eSOUND_TYPE)s);
 }
diff --git a/Firmware/mmu2_reporting.h b/Firmware/mmu2_reporting.h
index 89a2765bd..2a01afa0c 100644
--- a/Firmware/mmu2_reporting.h
+++ b/Firmware/mmu2_reporting.h
@@ -2,12 +2,19 @@
 
 #pragma once
 #include <stdint.h>
+#ifdef __AVR__
+    #include "mmu2/error_codes.h"
+    #include "mmu2/progress_codes.h"
+#else
+    #include "../../../../../../Prusa-Firmware-MMU/src/logic/error_codes.h"
+    #include "../../../../../../Prusa-Firmware-MMU/src/logic/progress_codes.h"
+#endif
 
 namespace MMU2 {
 
 enum CommandInProgress : uint8_t {
     NoCommand = 0,
-    CutFilament = 'C',
+    CutFilament = 'K',
     EjectFilament = 'E',
     Homing = 'H',
     LoadFilament = 'L',
@@ -17,10 +24,10 @@ enum CommandInProgress : uint8_t {
 };
 
 /// Called at the begin of every MMU operation
-void BeginReport(CommandInProgress cip, uint16_t ec);
+void BeginReport(CommandInProgress cip, ProgressCode ec);
 
 /// Called at the end of every MMU operation
-void EndReport(CommandInProgress cip, uint16_t ec);
+void EndReport(CommandInProgress cip, ProgressCode ec);
 
 /// Return true if the printer's LCD is drawing the error screen
 bool isErrorScreenRunning();
@@ -35,24 +42,31 @@ bool TuneMenuEntered();
 /// and allow the MMU and printer to communicate with each other.
 /// @param[in] ec error code
 /// @param[in] es error source
-void ReportErrorHook(CommandInProgress cip, uint16_t ec, uint8_t es);
+void ReportErrorHook(CommandInProgress cip, ErrorCode ec, uint8_t es);
 
 /// Called when the MMU sends operation progress update
-void ReportProgressHook(CommandInProgress cip, uint16_t ec);
+void ReportProgressHook(CommandInProgress cip, ProgressCode ec);
 
-/// @brief Clear the status line and setup the LCD cursor
-void TryLoadUnloadProgressbarInit();
+struct TryLoadUnloadReporter {
+    TryLoadUnloadReporter(float delta_mm);
+    ~TryLoadUnloadReporter();
+    void Progress(bool sensorState);
+    void DumpToSerial();
 
-/// @brief Clear the status line and setup the LCD cursor
-void TryLoadUnloadProgressbarDeinit();
+private:
+    /// @brief Add one block to the progress bar
+    /// @param col pixel position on the LCD status line, should range from 0 to (LCD_WIDTH - 1)
+    /// @param sensorState if true, filament is not present, else filament is present. This controls which character to render
+    void Render(uint8_t col, bool sensorState);
 
-/// @brief Report the results to serial
-void TryLoadUnloadProgressbarEcho();
-
-/// @brief Add one block to the progress bar
-/// @param col pixel position on the LCD status line, should range from 0 to (LCD_WIDTH - 1)
-/// @param sensorState if true, filament is not present, else filament is present. This controls which character to render
-void TryLoadUnloadProgressbar(uint8_t col, bool sensorState);
+    uint8_t dpixel0;
+    uint8_t dpixel1;
+    uint8_t lcd_cursor_col;
+    // The total length is twice delta_mm. Divide that length by number of pixels
+    // available to get length per pixel.
+    // Note: Below is the reciprocal of (2 * delta_mm) / LCD_WIDTH [mm/pixel]
+    float pixel_per_mm;
+};
 
 /// Remders the sensor status line. Also used by the "resume temperature" screen.
 void ReportErrorHookDynamicRender();
@@ -74,6 +88,9 @@ void IncrementLoadFails();
 /// Increments EEPROM cell - number of MMU errors
 void IncrementMMUFails();
 
+/// @returns true when Cutter is enabled in the menus
+bool cutter_enabled();
+
 // Beware: enum values intentionally chosen to match the 8bit FW to save code size
 enum SoundType {
     Prompt = 2,
@@ -93,4 +110,4 @@ void ScreenClear();
 
 void tuneIdlerStallguardThreshold();
 
-} // namespace
+} // namespace MMU2
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 002372743..2312fbfb8 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -189,20 +189,17 @@ void checkHitEndstops()
 {
  if(endstop_hit) {
 #ifdef VERBOSE_CHECK_HIT_ENDSTOPS
-   SERIAL_ECHO_START;
-   SERIAL_ECHORPGM(MSG_ENDSTOPS_HIT);
-   if(endstop_hit & _BV(X_AXIS)) {
-     SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/cs.axis_steps_per_mm[X_AXIS]);
-//     LCD_MESSAGERPGM(CAT2((MSG_ENDSTOPS_HIT), PSTR("X")));
-   }
-   if(endstop_hit & _BV(Y_AXIS)) {
-     SERIAL_ECHOPAIR(" Y:",(float)endstops_trigsteps[Y_AXIS]/cs.axis_steps_per_mm[Y_AXIS]);
-//     LCD_MESSAGERPGM(CAT2((MSG_ENDSTOPS_HIT), PSTR("Y")));
-   }
-   if(endstop_hit & _BV(Z_AXIS)) {
-     SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/cs.axis_steps_per_mm[Z_AXIS]);
-//     LCD_MESSAGERPGM(CAT2((MSG_ENDSTOPS_HIT),PSTR("Z")));
-   }
+    SERIAL_ECHO_START;
+    SERIAL_ECHORPGM(PSTR("Endstops Hit"));
+    for (uint8_t axis = 0; axis < E_AXIS; axis++) // XYZ
+    {
+        if(endstop_hit & _BV(axis)) {
+            SERIAL_ECHO(' ');
+            SERIAL_ECHO(char('X' + axis));
+            SERIAL_ECHO(':');
+            SERIAL_ECHO(float(endstops_trigsteps[axis]) / cs.axis_steps_per_mm[axis]);
+        }
+    }
    SERIAL_ECHOLN("");
 #endif //VERBOSE_CHECK_HIT_ENDSTOPS
    endstop_hit = 0;
diff --git a/Firmware/tmc2130.h b/Firmware/tmc2130.h
index e09e027c3..7ebc875e6 100644
--- a/Firmware/tmc2130.h
+++ b/Firmware/tmc2130.h
@@ -137,7 +137,7 @@ struct TMCInitParams {
     inline explicit TMCInitParams(bool bSuppressFlag, bool enableECool):bSuppressFlag(bSuppressFlag), enableECool(enableECool) { }
     inline explicit TMCInitParams(bool enableECool)
         : bSuppressFlag(
-#ifdef PSU_delta
+#ifdef PSU_Delta
         1
 #else
         0
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 4e306aac7..be066530f 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -241,6 +241,8 @@ static void lcd_sheet_menu();
 static void menu_action_sdfile(const char* filename);
 static void menu_action_sddirectory(const char* filename);
 
+static void lcd_rehome_xy();
+
 #define ENCODER_FEEDRATE_DEADZONE 10
 
 #define STATE_NA 255
@@ -277,40 +279,34 @@ static void lcd_implementation_drawmenu_sddirectory(uint8_t row, const char* lon
 
 static void menu_item_sddir(const char* str_fn, char* str_fnl)
 {
-	if (menu_item == menu_line)
+	if (lcd_draw_update)
 	{
-		if (lcd_draw_update)
-		{
-			lcd_implementation_drawmenu_sddirectory(menu_row, (str_fnl[0] == '\0') ? str_fn : str_fnl);
-		}
-		if (menu_clicked && (lcd_encoder == menu_item))
-		{
-			lcd_update_enabled = false;
-			menu_action_sddirectory(str_fn);
-			lcd_update_enabled = true;
-			menu_item_ret();
-			return;
-		}
+		lcd_implementation_drawmenu_sddirectory(menu_row, (str_fnl[0] == '\0') ? str_fn : str_fnl);
+	}
+	if (menu_clicked && (lcd_encoder == menu_item))
+	{
+		lcd_update_enabled = false;
+		menu_action_sddirectory(str_fn);
+		lcd_update_enabled = true;
+		menu_item_ret();
+		return;
 	}
 	menu_item++;
 }
 
 static void menu_item_sdfile(const char* str_fn, char* str_fnl)
 {
-	if (menu_item == menu_line)
+	if (lcd_draw_update)
 	{
-		if (lcd_draw_update)
-		{
-			lcd_implementation_drawmenu_sdfile(menu_row, (str_fnl[0] == '\0') ? str_fn : str_fnl);
-		}
-		if (menu_clicked && (lcd_encoder == menu_item))
-		{
-			lcd_update_enabled = false;
-			menu_action_sdfile(str_fn);
-			lcd_update_enabled = true;
-			menu_item_ret();
-			return;
-		}
+		lcd_implementation_drawmenu_sdfile(menu_row, (str_fnl[0] == '\0') ? str_fn : str_fnl);
+	}
+	if (menu_clicked && (lcd_encoder == menu_item))
+	{
+		lcd_update_enabled = false;
+		menu_action_sdfile(str_fn);
+		lcd_update_enabled = true;
+		menu_item_ret();
+		return;
 	}
 	menu_item++;
 }
@@ -5298,7 +5294,7 @@ static void lcd_main_menu()
     if(!isPrintPaused && (custom_message_type != CustomMsg::Resuming)) MENU_ITEM_SUBMENU_P(_T(MSG_CALIBRATION), lcd_calibration_menu);
     }
 
-    if (!usb_timer.running() && (lcd_commands_type == LcdCommands::Idle)) {
+    if (!usb_timer.running()) {
         MENU_ITEM_SUBMENU_P(_i("Statistics"), lcd_menu_statistics);////MSG_STATISTICS c=18
     }
 
@@ -5335,6 +5331,15 @@ void stepper_timer_overflow() {
 }
 #endif /* DEBUG_STEPPER_TIMER_MISSED */
 
+static void lcd_rehome_xy() {
+	// Do home directly, G28 X Y resets MBL, which could be bad.
+	homeaxis(X_AXIS);
+	homeaxis(Y_AXIS);
+	lcd_setstatuspgm(_T(MSG_AUTO_HOME));
+	lcd_return_to_status();
+	lcd_draw_update = 3;
+}
+
 
 static void lcd_colorprint_change() {
 
@@ -5441,7 +5446,9 @@ static void lcd_tune_menu()
     if (!farm_mode)
         MENU_ITEM_FUNCTION_P(_T(MSG_FILAMENTCHANGE), lcd_colorprint_change);
 #endif
-
+    if (isPrintPaused) {// Don't allow rehome if actively printing. Maaaaybe it could work to insert on the fly, seems too risky.
+        MENU_ITEM_FUNCTION_P(_T(MSG_AUTO_HOME), lcd_rehome_xy);
+    }
 #ifdef FILAMENT_SENSOR
     MENU_ITEM_SUBMENU_P(_T(MSG_FSENSOR), lcd_fsensor_settings_menu);
 #endif //FILAMENT_SENSOR
@@ -5477,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;
@@ -5492,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();
@@ -6188,7 +6196,7 @@ static bool lcd_selfcheck_axis_sg(uint8_t axis) {
 	float max_error_mm = 5;
 	switch (axis) {
 	case 0: axis_length = X_MAX_POS; break;
-	case 1: axis_length = Y_MAX_POS + 8; break;
+	case 1: axis_length = Y_MAX_POS - Y_MIN_POS + 4; break;
 	default: axis_length = 210; break;
 	}
 
diff --git a/Firmware/variants/MK25-RAMBo10a.h b/Firmware/variants/MK25-RAMBo10a.h
index 099ad329f..81289cbf9 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 7cce101be..ecce19774 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 254be0d09..c82be7c5d 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 2c375f920..a547aa083 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-E3DREVO.h b/Firmware/variants/MK3-E3DREVO.h
index f9e55e4c5..d1d1e978f 100644
--- a/Firmware/variants/MK3-E3DREVO.h
+++ b/Firmware/variants/MK3-E3DREVO.h
@@ -420,12 +420,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-E3DREVO_HF_60W.h b/Firmware/variants/MK3-E3DREVO_HF_60W.h
index e60d5b3f4..07e64648b 100644
--- a/Firmware/variants/MK3-E3DREVO_HF_60W.h
+++ b/Firmware/variants/MK3-E3DREVO_HF_60W.h
@@ -421,12 +421,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 78ee31d00..8135d9856 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-E3DREVO.h b/Firmware/variants/MK3S-E3DREVO.h
index 030ce2188..e5ba54596 100644
--- a/Firmware/variants/MK3S-E3DREVO.h
+++ b/Firmware/variants/MK3S-E3DREVO.h
@@ -424,12 +424,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-E3DREVO_HF_60W.h b/Firmware/variants/MK3S-E3DREVO_HF_60W.h
index bc5445af1..aa5238b5c 100644
--- a/Firmware/variants/MK3S-E3DREVO_HF_60W.h
+++ b/Firmware/variants/MK3S-E3DREVO_HF_60W.h
@@ -425,12 +425,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 111d151c3..6b7513214 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 1700e0fc1..eb3769854 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 2349385e3..dda5f0606 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
diff --git a/lang/lang-check.py b/lang/lang-check.py
index 618b6b03d..1c651b692 100755
--- a/lang/lang-check.py
+++ b/lang/lang-check.py
@@ -40,10 +40,30 @@ import os
 
 from lib import charset as cs
 from lib.io import load_map
+import enum
 
 COLORIZE = (stdout.isatty() and os.getenv("TERM", "dumb") != "dumb") or os.getenv('NO_COLOR') == "0"
 LCD_WIDTH = 20
 
+GH_ANNOTATIONS = os.getenv('GH_ANNOTATIONS') == "1"
+CURRENT_PO = "Unknown file"
+GH_ERR_COUNT = 0
+
+class AN_TYPE(enum.Enum):
+
+    def __new__(cls, *args, **kwds):
+        value = len(cls.__members__) + 1
+        obj = object.__new__(cls)
+        obj._value_ = value
+        return obj
+    def __init__(self, a, b):
+        self.prefix = a
+        self.print_fmt = b
+
+    ERROR = "error", "[E]"
+    WARNING = "warning", "[W]"
+    NOTICE = "notice", "[S]"
+
 def color_maybe(color_attr, text):
     if COLORIZE:
         return '\033[0;' + str(color_attr) + 'm' + text + '\033[0m'
@@ -119,6 +139,33 @@ def ign_char_first(c):
 def ign_char_last(c):
     return c.isalnum() or c in {'.', "'"}
 
+# Print_anyway is used to reduce code copypasta.
+# specifically, if we have all the info here to construct the "normal" message as well, it's done here
+
+def gh_annotate(an_type, start_line, message, end_line = None, print_anyway = False):
+    if not GH_ANNOTATIONS:
+        if print_anyway:
+            if end_line is not None:
+                line_text = "lines {}-{}".format(start_line, end_line)
+            else:
+                line_text = "line {}".format(start_line)
+            message_simple = "{} on {}".format(message, line_text)
+            if an_type == AN_TYPE.ERROR:
+                print(red("{}: {}".format(an_type.print_fmt, message_simple)))
+            else:
+                print(yellow("{}: {}".format(an_type.print_fmt, message_simple)))
+        return
+    if end_line is not None:
+        line_info = "line={},endLine={}".format(start_line,end_line)
+    else:
+        line_info = "line={}".format(start_line)
+
+    print("::{} file={},{}::{}".format(an_type.prefix, CURRENT_PO, line_info, message))
+    if an_type == AN_TYPE.ERROR:
+        global GH_ERR_COUNT
+        GH_ERR_COUNT += 1
+
+
 def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty, warn_same, information, shorter):
     """Check strings to display definition."""
 
@@ -137,10 +184,10 @@ def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty,
     # Check comment syntax (non-empty and include a MSG id)
     if known_msgid or warn_empty:
         if len(meta) == 0:
-            print(red("[E]: Translation doesn't contain any comment metadata on line %d" % line))
+            gh_annotate(AN_TYPE.ERROR, line, "Translation missing comment metadata", None, True)
             return False
         if not meta.startswith('MSG'):
-            print(red("[E]: Critical syntax error: comment doesn't start with MSG on line %d" % line))
+            gh_annotate(AN_TYPE.ERROR, line, "Critical Syntax Error: comment doesn't start with MSG", None, True)
             print(red(" comment: " + meta))
             return False
 
@@ -158,29 +205,29 @@ def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty,
             else:
                 raise ValueError
         except ValueError:
-            print(red("[E]: Invalid display definition on line %d" % line))
+            gh_annotate(AN_TYPE.ERROR, line, "Invalid display definition", None, True)
             print(red(" definition: " + meta))
             return False
 
     if not cols:
         if not no_warning and known_msgid and not rows:
             errors += 1
-            print(yellow("[W]: No usable display definition on line %d" % line))
+            gh_annotate(AN_TYPE.WARNING, line, "No usable display definition", None, True)
         # probably fullscreen, guess from the message length to continue checking
         cols = LCD_WIDTH
     if cols > LCD_WIDTH:
         errors += 1
-        print(yellow("[W]: Invalid column count on line %d" % line))
+        gh_annotate(AN_TYPE.WARNING, line, "Invalid column count", None, True)
     if not rows:
         rows = 1
     elif rows > 1 and cols != LCD_WIDTH:
         errors += 1
-        print(yellow("[W]: Multiple rows with odd number of columns on line %d" % line))
+        gh_annotate(AN_TYPE.WARNING, line, "Multiple rows with odd number of columns", None, True)
 
     # Check if translation contains unsupported characters
     invalid_char = cs.translation_check(cs.unicode_to_source(translation))
     if invalid_char is not None:
-        print(red('[E]: Critical syntax: Unhandled char %s found on line %d' % (repr(invalid_char), line)))
+        gh_annotate(AN_TYPE.ERROR, line, "Critical syntax: Unhandled char %s found".format(repr(invalid_char)), None, True )
         print(red(' translation: ' + translation))
         return False
 
@@ -195,13 +242,13 @@ def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty,
     # Incorrect number of rows/cols on the definition
     if rows == 1 and (len(source) > cols or rows_count_source > rows):
         errors += 1
-        print(yellow('[W]: Source text longer than %d cols as defined on line %d:' % (cols, line)))
+        gh_annotate(AN_TYPE.WARNING, line, "Source text longer than %d cols as defined".format(cols), None, True)
         print_ruler(4, cols);
         print_truncated(source, cols)
         print()
     elif rows_count_source > rows:
         errors += 1
-        print(yellow('[W]: Wrapped source text longer than %d rows as defined on line %d:' % (rows, line)))
+        gh_annotate(AN_TYPE.WARNING, line, "Source text longer than %d rows as defined".format(rows), None, True)
         print_ruler(6, cols);
         print_wrapped(wrapped_source, rows, cols)
         print()
@@ -214,9 +261,9 @@ def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty,
     if len(translation) == 0 and (warn_empty or (not no_warning and known_msgid)):
         errors += 1
         if rows == 1:
-            print(yellow("[W]: Empty translation for \"%s\" on line %d" % (source, line)))
+            gh_annotate(AN_TYPE.WARNING, line, "Empty translation for \"{}\"".format(source), line + rows, True )
         else:
-            print(yellow("[W]: Empty translation on line %d" % line))
+            gh_annotate(AN_TYPE.WARNING, line, "Empty translation", line + rows, True )
             print_ruler(6, cols);
             print_wrapped(wrapped_source, rows, cols)
             print()
@@ -224,6 +271,7 @@ def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty,
     # Check for translation length too long
     if (rows_count_translation > rows) or (rows == 1 and len(translation) > cols):
         errors += 1
+        gh_annotate(AN_TYPE.ERROR, line, "Text is longer than definition", line + rows)
         print(red('[E]: Text is longer than definition on line %d: cols=%d rows=%d (rows diff=%d)'
                 % (line, cols, rows, rows_count_translation-rows)))
         print_source_translation(source, translation,
@@ -232,6 +280,7 @@ def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty,
 
     # Check for translation length shorter
     if shorter and (rows_count_translation < rows-1):
+        gh_annotate(AN_TYPE.NOTICE, line, "Text is shorter than definition", line + rows)
         print(yellow('[S]: Text is shorter than definition on line %d: cols=%d rows=%d (rows diff=%d)'
                 % (line, cols, rows, rows_count_translation-rows)))
         print_source_translation(source, translation,
@@ -241,7 +290,7 @@ def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty,
     # Different count of % sequences
     if source.count('%') != translation.count('%') and len(translation) > 0:
         errors += 1
-        print(red('[E]: Unequal count of %% escapes on line %d:' % (line)))
+        gh_annotate(AN_TYPE.ERROR, line, "Unequal count of %% escapes", None, True)
         print_source_translation(source, translation,
                                 wrapped_source, wrapped_translation,
                                 rows, cols)
@@ -254,14 +303,14 @@ def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty,
         end_diff = not (ign_char_last(source_end) and ign_char_last(translation_end)) and source_end != translation_end
         if start_diff or end_diff:
             if start_diff:
-                print(yellow('[S]: Differing first punctuation character (%s => %s) on line %d:' % (source[0], translation[0], line)))
+                gh_annotate(AN_TYPE.NOTICE, line, "Differing first punctuation character: ({} => {})".format(source[0],translation[0]), None, True)
             if end_diff:
-                print(yellow('[S]: Differing last punctuation character (%s => %s) on line %d:' % (source[-1], translation[-1], line)))
+                gh_annotate(AN_TYPE.NOTICE, line, "Differing last punctuation character: ({} => {})".format(source[-1],translation[-1]), None, True)
             print_source_translation(source, translation,
                                     wrapped_source, wrapped_translation,
                                     rows, cols)
     if not no_suggest and source == translation and (warn_same or len(source.split(' ', 1)) > 1):
-        print(yellow('[S]: Translation same as original on line %d:' %line))
+        gh_annotate(AN_TYPE.NOTICE, line, "Translation same as original text", None, True)
         print_source_translation(source, translation,
                                 wrapped_source, wrapped_translation,
                                 rows, cols)
@@ -269,7 +318,7 @@ def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty,
     # Short translation
     if not no_suggest and len(source) > 0 and len(translation) > 0:
         if len(translation.rstrip()) < len(source.rstrip()) / 2:
-            print(yellow('[S]: Short translation on line %d:' % (line)))
+            gh_annotate(AN_TYPE.NOTICE, line, "Short translation", None, True)
             print_source_translation(source, translation,
                                     wrapped_source, wrapped_translation,
                                     rows, cols)
@@ -280,7 +329,7 @@ def check_translation(entry, msgids, is_pot, no_warning, no_suggest, warn_empty,
      translation.rstrip() != translation and \
      (rows > 1 or len(translation) != len(source)):
         errors += 1
-        print(yellow('[W]: Incorrect trailing whitespace for translation on line %d:' % (line)))
+        gh_annotate(AN_TYPE.WARNING, line, "Incorrect trailing whitespace for translation", None, True)
         source = highlight_trailing_white(source)
         translation = highlight_trailing_white(translation)
         wrapped_translation = highlight_trailing_white(wrapped_translation)
@@ -350,9 +399,14 @@ def main():
     # check each translation in turn
     status = True
     for translation in polib.pofile(args.po):
+        global CURRENT_PO
+        CURRENT_PO=args.po
         status &= check_translation(translation, msgids, args.pot, args.no_warning, args.no_suggest,
                                     args.warn_empty, args.warn_same, args.information, args.shorter)
-    return 0 if status else 1
+    if GH_ANNOTATIONS:
+        return GH_ERR_COUNT > 0 # Do not cause a failure if only warnings or notices.
+    else:
+        return 0 if status else 1
 
 if __name__ == "__main__":
     exit(main())
diff --git a/lang/po/Firmware.pot b/lang/po/Firmware.pot
index 27b43fbaa..5470a549f 100644
--- a/lang/po/Firmware.pot
+++ b/lang/po/Firmware.pot
@@ -1209,6 +1209,11 @@ msgstr ""
 msgid "Mesh Bed Leveling"
 msgstr ""
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr ""
+
 #. MSG_MODE c=6
 #: ../../Firmware/messages.cpp:107 ../../Firmware/ultralcd.cpp:4122
 #: ../../Firmware/ultralcd.cpp:4126 ../../Firmware/ultralcd.cpp:4134
diff --git a/lang/po/Firmware_cs.po b/lang/po/Firmware_cs.po
index 49098bcd8..cbabfc95d 100644
--- a/lang/po/Firmware_cs.po
+++ b/lang/po/Firmware_cs.po
@@ -2550,6 +2550,11 @@ msgstr "Výměna filamentu M600. Vložte nový filament nebo vysuňte starý."
 msgid "Sensitivity"
 msgstr "Citlivost"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Mesh Bed Leveling selhal. Spusťte kalibraci osy Z."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr "Vyjmete stary filament a stisknete tlacitko pro zavedeni noveho."
 
diff --git a/lang/po/Firmware_de.po b/lang/po/Firmware_de.po
index 094cfb8f4..7967cc104 100644
--- a/lang/po/Firmware_de.po
+++ b/lang/po/Firmware_de.po
@@ -2578,6 +2578,11 @@ msgstr ""
 msgid "Sensitivity"
 msgstr "Sensitivität"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "MeshBett Ausgleich fehlgeschlagen. Z Kalibrierung ausführen."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr "Entferne das alte Fil. und drücke den Knopf, um das neue zu laden."
 
diff --git a/lang/po/Firmware_es.po b/lang/po/Firmware_es.po
index dd1c19daa..a98b34175 100644
--- a/lang/po/Firmware_es.po
+++ b/lang/po/Firmware_es.po
@@ -2574,6 +2574,11 @@ msgstr ""
 msgid "Sensitivity"
 msgstr "Sensibilidad"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Nivelacion fallada. Ejecute la calibración Z."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr ""
 #~ "Retira el fil. viejo y presione el dial para comenzar a cargar el nuevo."
diff --git a/lang/po/Firmware_fr.po b/lang/po/Firmware_fr.po
index c4520e162..c142a85ed 100644
--- a/lang/po/Firmware_fr.po
+++ b/lang/po/Firmware_fr.po
@@ -2589,6 +2589,11 @@ msgstr ""
 msgid "Sensitivity"
 msgstr "Sensibilité"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr ""
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr ""
 #~ "Retirez l'ancien fil. puis appuyez sur le bouton pour charger le nouveau."
diff --git a/lang/po/Firmware_hr.po b/lang/po/Firmware_hr.po
index 31557cc05..a3a37dc21 100644
--- a/lang/po/Firmware_hr.po
+++ b/lang/po/Firmware_hr.po
@@ -2567,6 +2567,11 @@ msgstr "Promjena filamenta M600. Stavite novu nit ili izbacite staru."
 msgid "Sensitivity"
 msgstr "Osjetljivost"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Niveliranje podloge nije uspijelo. Pokrenite Z kalibraciju."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr "Uklonite stari fil. i pritisnite gumb za pocetak stavljanja novog."
 
diff --git a/lang/po/Firmware_hu.po b/lang/po/Firmware_hu.po
index e68c4e49a..8f6cb8614 100644
--- a/lang/po/Firmware_hu.po
+++ b/lang/po/Firmware_hu.po
@@ -2572,6 +2572,11 @@ msgstr ""
 msgid "Sensitivity"
 msgstr "Érzékenység"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Sikertelen asztal szintezés. Kérjük, futtasd a Z kalibrálást."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr "Vedd ki a regi fil., majd nyomd meg a gombot az uj fil. betoltesehez."
 
diff --git a/lang/po/Firmware_it.po b/lang/po/Firmware_it.po
index 999d5b98b..19c0a5780 100644
--- a/lang/po/Firmware_it.po
+++ b/lang/po/Firmware_it.po
@@ -2573,6 +2573,11 @@ msgstr ""
 msgid "Sensitivity"
 msgstr "Sensibilità"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Livellamento piano fallito. Si prega di eseguire la calibrazione Z."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr "Rimuovi il fil. precedente e premi la manopola per caricare il nuovo."
 
diff --git a/lang/po/Firmware_nl.po b/lang/po/Firmware_nl.po
index f4f8547f3..fb2454076 100644
--- a/lang/po/Firmware_nl.po
+++ b/lang/po/Firmware_nl.po
@@ -2575,6 +2575,11 @@ msgstr "M600-filamentwissel. Laad een nieuw filament of werp het oude uit."
 msgid "Sensitivity"
 msgstr "Sensitiviteit"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Bed leveling mislukt. Voer de Z-kalibratie uit."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr ""
 #~ "Verwijder de oude filament en druk op de knop om nieuwe filament te laden."
diff --git a/lang/po/Firmware_no.po b/lang/po/Firmware_no.po
index fab3dcc41..731f05efb 100644
--- a/lang/po/Firmware_no.po
+++ b/lang/po/Firmware_no.po
@@ -2549,6 +2549,11 @@ msgstr "M600 filamentskifte. Sett inn en ny filament eller løs ut den gamle."
 msgid "Sensitivity"
 msgstr "Sensitivitet"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Sengeplanering feilet. Kjør Z-kalibrering."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr "Ta bort det gamle filamentet og trykk valghjulet for å laste et nytt."
 
diff --git a/lang/po/Firmware_pl.po b/lang/po/Firmware_pl.po
index b0abab91e..d7d2ad8d2 100644
--- a/lang/po/Firmware_pl.po
+++ b/lang/po/Firmware_pl.po
@@ -2568,6 +2568,11 @@ msgstr "Załaduj nowy filament lub wyładuj poprzedni."
 msgid "Sensitivity"
 msgstr "Wrażliwość"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Poziomowanie stołu nieudane. Proszę uruchomić kalibrację Z."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr "Wyciagnij poprzedni filament i nacisnij pokretlo aby zaladowac nowy."
 
diff --git a/lang/po/Firmware_ro.po b/lang/po/Firmware_ro.po
index f21be410b..0aeae1ecb 100644
--- a/lang/po/Firmware_ro.po
+++ b/lang/po/Firmware_ro.po
@@ -2573,6 +2573,11 @@ msgstr ""
 msgid "Sensitivity"
 msgstr "Sensibilitate"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Nivelarea patului a eșuat. Rulează Calibrare Z."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr "Scoateti fil. vechi si apasati butonul pentru a incarca nou."
 
diff --git a/lang/po/Firmware_sk.po b/lang/po/Firmware_sk.po
index 04219d0a9..98ec21c57 100644
--- a/lang/po/Firmware_sk.po
+++ b/lang/po/Firmware_sk.po
@@ -2555,6 +2555,11 @@ msgstr "Výmena filamentu M600. Vložte nový filament alebo vysuňte starý."
 msgid "Sensitivity"
 msgstr "Citlivosť"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Vyrovnanie platne zlyhalo. Spustite kalibráciu Z."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr "Vyberte stary filament a stlacte tlacidlo pre zavedenie noveho."
 
diff --git a/lang/po/Firmware_sv.po b/lang/po/Firmware_sv.po
index df05c1e81..b5e333df7 100644
--- a/lang/po/Firmware_sv.po
+++ b/lang/po/Firmware_sv.po
@@ -2562,6 +2562,11 @@ msgstr "M600 filamentbyte. Ladda en ny filament eller mata ut den gamla."
 msgid "Sensitivity"
 msgstr "Känslighet"
 
+#. MSG_MBL_FAILED_Z_CAL c=20 r=4
+#: ../../Firmware/Marlin_main.cpp:2976
+msgid "Mesh bed leveling failed. Please run Z calibration."
+msgstr "Bäddnivelleringen felade. Kör Z-kalibrering."
+
 #~ msgid "Remove old filament and press the knob to start loading new filament."
 #~ msgstr "Ta bort det gamla fil. och tryck på knappen för att börja ladda nytt."
 
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 7bab4fea2..b20179864 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -16,3 +16,27 @@ add_executable(tests ${TEST_SOURCES})
 target_include_directories(tests PRIVATE tests)
 target_link_libraries(tests Catch2::Catch2WithMain)
 catch_discover_tests(tests)
+
+set(ctest_test_args --output-on-failure)
+
+include(ProcessorCount)
+ProcessorCount(N)
+if(N EQUAL 0)
+  message(
+    WARNING "CTest: There was an issue reading the core count, tests won't be run in parallel"
+    )
+else()
+  message(STATUS "CTest: Detected ${N} CPU threads")
+  set(ctest_test_args -j${N} ${ctest_test_args})
+endif()
+
+# This step needs to always return OK but log whether it was successful or not. The thought here
+# is that if the tests all pass, .ctest-finished is created and we can check for its existance
+# after generating the report to determine if the overall build result is a pass or fail.
+add_custom_target(
+  test_run_all
+  COMMAND ${CMAKE_CTEST_COMMAND} ${ctest_test_args}
+  COMMAND ${CMAKE_COMMAND} -E touch .ctest-finished || exit 0
+  BYPRODUCTS ${PROJECT_BINARY_DIR}/.ctest-finished
+  WORKING_DIRECTORY "${PROJECT_BINARY_DIR}"
+  )