diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index eaf000c3ae..c892cd9dea 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -374,6 +374,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/ConfigurationStore.cpp b/Marlin/ConfigurationStore.cpp
index 0dee05ba78..16d94760b6 100644
--- a/Marlin/ConfigurationStore.cpp
+++ b/Marlin/ConfigurationStore.cpp
@@ -20,6 +20,12 @@
  *  max_e_jerk
  *  add_homing (x3)
  *
+ * Mesh bed leveling:
+ *  active
+ *  mesh_num_x
+ *  mesh_num_y
+ *  z_values[][]
+ *
  * DELTA:
  *  endstop_adj (x3)
  *  delta_radius
@@ -69,6 +75,10 @@
 #include "ultralcd.h"
 #include "ConfigurationStore.h"
 
+#if defined(MESH_BED_LEVELING)
+   #include "mesh_bed_leveling.h"
+#endif  // MESH_BED_LEVELING
+
 void _EEPROM_writeData(int &pos, uint8_t* value, uint8_t size) {
   uint8_t c;
   while(size--) {
@@ -105,7 +115,7 @@ void _EEPROM_readData(int &pos, uint8_t* value, uint8_t size) {
 // wrong data being written to the variables.
 // ALSO:  always make sure the variables in the Store and retrieve sections are in the same order.
 
-#define EEPROM_VERSION "V16"
+#define EEPROM_VERSION "V17"
 
 #ifdef EEPROM_SETTINGS
 
@@ -128,6 +138,28 @@ void Config_StoreSettings()  {
   EEPROM_WRITE_VAR(i, max_e_jerk);
   EEPROM_WRITE_VAR(i, add_homing);
 
+  uint8_t mesh_num_x = 3;
+  uint8_t mesh_num_y = 3;
+  #if defined(MESH_BED_LEVELING)
+    // Compile time test that sizeof(mbl.z_values) is as expected
+    typedef char c_assert[(sizeof(mbl.z_values) == MESH_NUM_X_POINTS*MESH_NUM_Y_POINTS*sizeof(dummy)) ? 1 : -1];
+    mesh_num_x = MESH_NUM_X_POINTS;
+    mesh_num_y = MESH_NUM_Y_POINTS;
+    EEPROM_WRITE_VAR(i, mbl.active);
+    EEPROM_WRITE_VAR(i, mesh_num_x);
+    EEPROM_WRITE_VAR(i, mesh_num_y);
+    EEPROM_WRITE_VAR(i, mbl.z_values);
+  #else
+    uint8_t dummy_uint8 = 0;
+    EEPROM_WRITE_VAR(i, dummy_uint8);
+    EEPROM_WRITE_VAR(i, mesh_num_x);
+    EEPROM_WRITE_VAR(i, mesh_num_y);
+    dummy = 0.0f;
+    for (int q=0; q<mesh_num_x*mesh_num_y; q++) {
+      EEPROM_WRITE_VAR(i, dummy);
+    }
+  #endif  // MESH_BED_LEVELING
+
   #ifdef DELTA
     EEPROM_WRITE_VAR(i, endstop_adj);               // 3 floats
     EEPROM_WRITE_VAR(i, delta_radius);              // 1 float
@@ -250,7 +282,7 @@ void Config_RetrieveSettings() {
     EEPROM_READ_VAR(i, max_feedrate);
     EEPROM_READ_VAR(i, max_acceleration_units_per_sq_second);
 
-        // steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
+    // steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
     reset_acceleration_rates();
 
     EEPROM_READ_VAR(i, acceleration);
@@ -264,6 +296,31 @@ void Config_RetrieveSettings() {
     EEPROM_READ_VAR(i, max_e_jerk);
     EEPROM_READ_VAR(i, add_homing);
 
+    uint8_t mesh_num_x = 0;
+    uint8_t mesh_num_y = 0;
+    #if defined(MESH_BED_LEVELING)
+      EEPROM_READ_VAR(i, mbl.active);
+      EEPROM_READ_VAR(i, mesh_num_x);
+      EEPROM_READ_VAR(i, mesh_num_y);
+      if (mesh_num_x != MESH_NUM_X_POINTS ||
+          mesh_num_y != MESH_NUM_Y_POINTS) {
+        mbl.reset();
+        for (int q=0; q<mesh_num_x*mesh_num_y; q++) {
+          EEPROM_READ_VAR(i, dummy);
+        }
+      } else {
+        EEPROM_READ_VAR(i, mbl.z_values);
+      }
+    #else
+      uint8_t dummy_uint8 = 0;
+      EEPROM_READ_VAR(i, dummy_uint8);
+      EEPROM_READ_VAR(i, mesh_num_x);
+      EEPROM_READ_VAR(i, mesh_num_y);
+      for (int q=0; q<mesh_num_x*mesh_num_y; q++) {
+        EEPROM_READ_VAR(i, dummy);
+      }
+    #endif  // MESH_BED_LEVELING
+
     #ifdef DELTA
       EEPROM_READ_VAR(i, endstop_adj);                // 3 floats
       EEPROM_READ_VAR(i, delta_radius);               // 1 float
@@ -392,6 +449,10 @@ void Config_ResetDefault() {
   max_e_jerk = DEFAULT_EJERK;
   add_homing[X_AXIS] = add_homing[Y_AXIS] = add_homing[Z_AXIS] = 0;
 
+  #if defined(MESH_BED_LEVELING)
+    mbl.active = 0;
+  #endif  // MESH_BED_LEVELING
+
   #ifdef DELTA
     endstop_adj[X_AXIS] = endstop_adj[Y_AXIS] = endstop_adj[Z_AXIS] = 0;
     delta_radius =  DELTA_RADIUS;
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 4ef5d59f11..32eae47572 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -41,6 +41,10 @@
 
 #define SERVO_LEVELING defined(ENABLE_AUTO_BED_LEVELING) && PROBE_SERVO_DEACTIVATION_DELAY > 0
 
+#if defined(MESH_BED_LEVELING)
+  #include "mesh_bed_leveling.h"
+#endif  // MESH_BED_LEVELING
+
 #include "ultralcd.h"
 #include "planner.h"
 #include "stepper.h"
@@ -1737,6 +1741,11 @@ inline void gcode_G28() {
     #endif
   #endif
 
+  #if defined(MESH_BED_LEVELING)
+    uint8_t mbl_was_active = mbl.active;
+    mbl.active = 0;
+  #endif  // MESH_BED_LEVELING
+
   saved_feedrate = feedrate;
   saved_feedmultiply = feedmultiply;
   feedmultiply = 100;
@@ -1951,12 +1960,112 @@ inline void gcode_G28() {
     enable_endstops(false);
   #endif
 
+  #if defined(MESH_BED_LEVELING)
+    if (mbl_was_active) {
+      current_position[X_AXIS] = mbl.get_x(0);
+      current_position[Y_AXIS] = mbl.get_y(0);
+      destination[X_AXIS] = current_position[X_AXIS];
+      destination[Y_AXIS] = current_position[Y_AXIS];
+      destination[Z_AXIS] = current_position[Z_AXIS];
+      destination[E_AXIS] = current_position[E_AXIS];
+      feedrate = homing_feedrate[X_AXIS];
+      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+      st_synchronize();
+      current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+      mbl.active = 1;
+    }
+  #endif
+
   feedrate = saved_feedrate;
   feedmultiply = saved_feedmultiply;
   previous_millis_cmd = millis();
   endstops_hit_on_purpose();
 }
 
+#if defined(MESH_BED_LEVELING)
+
+  inline void gcode_G29() {
+    static int probe_point = -1;
+    int state = 0;
+    if (code_seen('S') || code_seen('s')) {
+      state = code_value_long();
+      if (state < 0 || state > 2) {
+        SERIAL_PROTOCOLPGM("S out of range (0-2).\n");
+        return;
+      }
+    }
+
+    if (state == 0) { // Dump mesh_bed_leveling
+      if (mbl.active) {
+        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_SEARCH_Z);
+        SERIAL_PROTOCOLPGM("\nMeasured points:\n");              
+        for (int y=0; y<MESH_NUM_Y_POINTS; y++) {
+          for (int x=0; x<MESH_NUM_X_POINTS; x++) {
+            SERIAL_PROTOCOLPGM("  ");              
+            SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
+          }
+          SERIAL_EOL;
+        }
+      } else {
+        SERIAL_PROTOCOLPGM("Mesh bed leveling not active.\n");
+      }
+
+    } else if (state == 1) { // Begin probing mesh points
+
+      mbl.reset();
+      probe_point = 0;
+      enquecommands_P(PSTR("G28"));
+      enquecommands_P(PSTR("G29 S2"));
+
+    } else if (state == 2) { // Goto next point
+
+      if (probe_point < 0) {
+        SERIAL_PROTOCOLPGM("Mesh probing not started.\n");
+        return;
+      }
+      int ix, iy;
+      if (probe_point == 0) {
+        current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+      } else {
+        ix = (probe_point-1) % MESH_NUM_X_POINTS;
+        iy = (probe_point-1) / MESH_NUM_X_POINTS;
+        if (iy&1) { // Zig zag
+          ix = (MESH_NUM_X_POINTS - 1) - ix;
+        }
+        mbl.set_z(ix, iy, current_position[Z_AXIS]);
+        current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
+        st_synchronize();
+      }
+      if (probe_point == MESH_NUM_X_POINTS*MESH_NUM_Y_POINTS) {
+        SERIAL_PROTOCOLPGM("Mesh done.\n");
+        probe_point = -1;
+        mbl.active = 1;
+        enquecommands_P(PSTR("G28"));
+        return;
+      }
+      ix = probe_point % MESH_NUM_X_POINTS;
+      iy = probe_point / MESH_NUM_X_POINTS;
+      if (iy&1) { // Zig zag
+        ix = (MESH_NUM_X_POINTS - 1) - ix;
+      }
+      current_position[X_AXIS] = mbl.get_x(ix);
+      current_position[Y_AXIS] = mbl.get_y(iy);
+      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
+      st_synchronize();
+      probe_point++;
+    }
+  }
+
+#endif
+
 #ifdef ENABLE_AUTO_BED_LEVELING
 
   // Define the possible boundaries for probing based on set limits
@@ -2118,9 +2227,10 @@ inline void gcode_G28() {
 
     st_synchronize();
 
-  #ifdef DELTA
-    reset_bed_level();
-  #else
+    #ifdef DELTA
+      reset_bed_level();
+    #else
+
     // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
     //vector_3 corrected_position = plan_get_position_mm();
     //corrected_position.debug("position before G29");
@@ -4661,6 +4771,12 @@ void process_commands() {
       gcode_G28();
       break;
 
+    #if defined(MESH_BED_LEVELING)
+      case 29: // G29 Handle mesh based leveling
+        gcode_G29();
+        break;
+    #endif
+
     #ifdef ENABLE_AUTO_BED_LEVELING
 
       case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
@@ -5280,6 +5396,81 @@ void prepare_move_raw()
 }
 #endif //DELTA
 
+#if defined(MESH_BED_LEVELING)
+#if !defined(MIN)
+#define MIN(_v1, _v2) (((_v1) < (_v2)) ? (_v1) : (_v2))
+#endif  // ! MIN
+// This function is used to split lines on mesh borders so each segment is only part of one mesh area
+void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_rate, const uint8_t &extruder, uint8_t x_splits=0xff, uint8_t y_splits=0xff)
+{
+  if (!mbl.active) {
+    plan_buffer_line(x, y, z, e, feed_rate, extruder);
+    for(int8_t i=0; i < NUM_AXIS; i++) {
+      current_position[i] = destination[i];
+    }
+    return;
+  }
+  int pix = mbl.select_x_index(current_position[X_AXIS]);
+  int piy = mbl.select_y_index(current_position[Y_AXIS]);
+  int ix = mbl.select_x_index(x);
+  int iy = mbl.select_y_index(y);
+  pix = MIN(pix, MESH_NUM_X_POINTS-2);
+  piy = MIN(piy, MESH_NUM_Y_POINTS-2);
+  ix = MIN(ix, MESH_NUM_X_POINTS-2);
+  iy = MIN(iy, MESH_NUM_Y_POINTS-2);
+  if (pix == ix && piy == iy) {
+    // Start and end on same mesh square
+    plan_buffer_line(x, y, z, e, feed_rate, extruder);
+    for(int8_t i=0; i < NUM_AXIS; i++) {
+      current_position[i] = destination[i];
+    }
+    return;
+  }
+  float nx, ny, ne, normalized_dist;
+  if (ix > pix && (x_splits) & BIT(ix)) {
+    nx = mbl.get_x(ix);
+    normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
+    ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
+    ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
+    x_splits ^= BIT(ix);
+  } else if (ix < pix && (x_splits) & BIT(pix)) {
+    nx = mbl.get_x(pix);
+    normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
+    ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
+    ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
+    x_splits ^= BIT(pix);
+  } else if (iy > piy && (y_splits) & BIT(iy)) {
+    ny = mbl.get_y(iy);
+    normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
+    nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
+    ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
+    y_splits ^= BIT(iy);
+  } else if (iy < piy && (y_splits) & BIT(piy)) {
+    ny = mbl.get_y(piy);
+    normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
+    nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
+    ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
+    y_splits ^= BIT(piy);
+  } else {
+    // Already split on a border
+    plan_buffer_line(x, y, z, e, feed_rate, extruder);
+    for(int8_t i=0; i < NUM_AXIS; i++) {
+      current_position[i] = destination[i];
+    }
+    return;
+  }
+  // Do the split and look for more borders
+  destination[X_AXIS] = nx;
+  destination[Y_AXIS] = ny;
+  destination[E_AXIS] = ne;
+  mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
+  destination[X_AXIS] = x;
+  destination[Y_AXIS] = y;
+  destination[E_AXIS] = e;
+  mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
+}
+#endif  // MESH_BED_LEVELING
+
 void prepare_move()
 {
   clamp_to_software_endstops(destination);
@@ -5395,10 +5586,14 @@ for (int s = 1; s <= steps; s++) {
 #if ! (defined DELTA || defined SCARA)
   // Do not use feedmultiply for E or Z only moves
   if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
-      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-  }
-  else {
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+  } else {
+#if defined(MESH_BED_LEVELING)
+    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+    return;
+#else
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+#endif  // MESH_BED_LEVELING
   }
 #endif // !(DELTA || SCARA)
 
diff --git a/Marlin/configurator/config/Configuration.h b/Marlin/configurator/config/Configuration.h
index 57ec74f9be..a115c7a534 100644
--- a/Marlin/configurator/config/Configuration.h
+++ b/Marlin/configurator/config/Configuration.h
@@ -412,6 +412,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/example_configurations/Felix/Configuration.h b/Marlin/example_configurations/Felix/Configuration.h
index 17da67953b..ccaf283cad 100644
--- a/Marlin/example_configurations/Felix/Configuration.h
+++ b/Marlin/example_configurations/Felix/Configuration.h
@@ -358,6 +358,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/example_configurations/Felix/Configuration_DUAL.h b/Marlin/example_configurations/Felix/Configuration_DUAL.h
index 9766961a50..b4c914a6b9 100644
--- a/Marlin/example_configurations/Felix/Configuration_DUAL.h
+++ b/Marlin/example_configurations/Felix/Configuration_DUAL.h
@@ -358,6 +358,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/example_configurations/Hephestos/Configuration.h b/Marlin/example_configurations/Hephestos/Configuration.h
index 3245546126..6aee728828 100644
--- a/Marlin/example_configurations/Hephestos/Configuration.h
+++ b/Marlin/example_configurations/Hephestos/Configuration.h
@@ -384,6 +384,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/example_configurations/K8200/Configuration.h b/Marlin/example_configurations/K8200/Configuration.h
index b1c63eb4fe..63bb61a7f4 100644
--- a/Marlin/example_configurations/K8200/Configuration.h
+++ b/Marlin/example_configurations/K8200/Configuration.h
@@ -389,6 +389,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h
index 6c12c2f192..8a67c7f1cd 100644
--- a/Marlin/example_configurations/SCARA/Configuration.h
+++ b/Marlin/example_configurations/SCARA/Configuration.h
@@ -413,6 +413,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/example_configurations/WITBOX/Configuration.h b/Marlin/example_configurations/WITBOX/Configuration.h
index 8d348ba9bc..20e0f8366f 100644
--- a/Marlin/example_configurations/WITBOX/Configuration.h
+++ b/Marlin/example_configurations/WITBOX/Configuration.h
@@ -383,6 +383,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/example_configurations/delta/generic/Configuration.h b/Marlin/example_configurations/delta/generic/Configuration.h
index d9d150f429..3b703f4ed0 100644
--- a/Marlin/example_configurations/delta/generic/Configuration.h
+++ b/Marlin/example_configurations/delta/generic/Configuration.h
@@ -413,6 +413,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration.h b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
index 1ff5c482eb..451aae2c82 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
@@ -414,6 +414,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/example_configurations/makibox/Configuration.h b/Marlin/example_configurations/makibox/Configuration.h
index cb61ca10f3..c47f4cd2d1 100644
--- a/Marlin/example_configurations/makibox/Configuration.h
+++ b/Marlin/example_configurations/makibox/Configuration.h
@@ -381,6 +381,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration.h b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
index bf4e27960f..09b5d2639a 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
@@ -383,6 +383,23 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)
 
 
+//===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+// #define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+// #define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited
+  #define MESH_NUM_Y_POINTS 3
+  #define MESH_HOME_SEARCH_Z 4  // Z after Home, bed somewhere below but above 0.0
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/language_en.h b/Marlin/language_en.h
index 636d622aba..0998d22ad7 100644
--- a/Marlin/language_en.h
+++ b/Marlin/language_en.h
@@ -95,6 +95,9 @@
 #ifndef MSG_MOVE_AXIS
 #define MSG_MOVE_AXIS                       "Move axis"
 #endif
+#ifndef MSG_LEVEL_BED
+#define MSG_LEVEL_BED                       "Level bed"
+#endif
 #ifndef MSG_MOVE_X
 #define MSG_MOVE_X                          "Move X"
 #endif
diff --git a/Marlin/mesh_bed_leveling.cpp b/Marlin/mesh_bed_leveling.cpp
new file mode 100644
index 0000000000..b383fe589a
--- /dev/null
+++ b/Marlin/mesh_bed_leveling.cpp
@@ -0,0 +1,20 @@
+#include "mesh_bed_leveling.h"
+
+#if defined(MESH_BED_LEVELING)
+
+mesh_bed_leveling mbl;
+
+mesh_bed_leveling::mesh_bed_leveling() {
+    reset();
+}
+    
+void mesh_bed_leveling::reset() {
+    for (int y=0; y<MESH_NUM_Y_POINTS; y++) {
+        for (int x=0; x<MESH_NUM_X_POINTS; x++) {
+            z_values[y][x] = 0;
+        }
+    }
+    active = 0;
+}
+
+#endif  // MESH_BED_LEVELING
diff --git a/Marlin/mesh_bed_leveling.h b/Marlin/mesh_bed_leveling.h
new file mode 100644
index 0000000000..d76321ab42
--- /dev/null
+++ b/Marlin/mesh_bed_leveling.h
@@ -0,0 +1,61 @@
+#include "Marlin.h"
+
+#if defined(MESH_BED_LEVELING)
+
+#define MESH_X_DIST ((MESH_MAX_X - MESH_MIN_X)/(MESH_NUM_X_POINTS - 1))
+#define MESH_Y_DIST ((MESH_MAX_Y - MESH_MIN_Y)/(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();
+    
+    void reset();
+    
+    float get_x(int i) { return MESH_MIN_X + MESH_X_DIST*i; }
+    float get_y(int i) { return MESH_MIN_Y + MESH_Y_DIST*i; }
+    void set_z(int ix, int 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 calc_z0(float a0, float a1, float z1, float a2, float z2) {
+        float delta_z = (z2 - z1)/(a2 - a1);
+        float delta_a = a0 - a1;
+        return z1 + delta_a * delta_z;
+    }
+    
+    float get_z(float x0, float y0) {
+        int x_index = select_x_index(x0);
+        int y_index = select_y_index(y0);
+        float z1 = calc_z0(x0,
+                           get_x(x_index), z_values[y_index][x_index],
+                           get_x(x_index+1), z_values[y_index][x_index+1]);
+        float z2 = calc_z0(x0,
+                           get_x(x_index), z_values[y_index+1][x_index],
+                           get_x(x_index+1), z_values[y_index+1][x_index+1]);
+        float z0 = calc_z0(y0,
+                           get_y(y_index), z1,
+                           get_y(y_index+1), z2);
+        return z0;
+    }
+};
+
+extern mesh_bed_leveling mbl;
+
+#endif  // MESH_BED_LEVELING
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 6b36cefab1..a1ef453c0f 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -58,6 +58,10 @@
 #include "ultralcd.h"
 #include "language.h"
 
+#if defined(MESH_BED_LEVELING)
+  #include "mesh_bed_leveling.h"
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= public variables ============================
 //===========================================================================
@@ -464,7 +468,7 @@ float junction_deviation = 0.1;
 // Add a new linear movement to the buffer. steps[X_AXIS], _y and _z is the absolute position in 
 // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-#ifdef ENABLE_AUTO_BED_LEVELING
+#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
   void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder)
 #else
   void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
@@ -481,6 +485,10 @@ float junction_deviation = 0.1;
     lcd_update();
   }
 
+  #ifdef MESH_BED_LEVELING
+    if (mbl.active) z += mbl.get_z(x, y);
+  #endif
+
   #ifdef ENABLE_AUTO_BED_LEVELING
     apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
   #endif
@@ -948,30 +956,31 @@ float junction_deviation = 0.1;
 
 } // plan_buffer_line()
 
-#ifdef ENABLE_AUTO_BED_LEVELING
+#if defined(ENABLE_AUTO_BED_LEVELING) && !defined(DELTA)
+  vector_3 plan_get_position() {
+    vector_3 position = vector_3(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS), st_get_position_mm(Z_AXIS));
 
-  #ifndef DELTA
-    vector_3 plan_get_position() {
-      vector_3 position = vector_3(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS), st_get_position_mm(Z_AXIS));
+    //position.debug("in plan_get position");
+    //plan_bed_level_matrix.debug("in plan_get bed_level");
+    matrix_3x3 inverse = matrix_3x3::transpose(plan_bed_level_matrix);
+    //inverse.debug("in plan_get inverse");
+    position.apply_rotation(inverse);
+    //position.debug("after rotation");
 
-      //position.debug("in plan_get position");
-      //plan_bed_level_matrix.debug("in plan_get bed_level");
-      matrix_3x3 inverse = matrix_3x3::transpose(plan_bed_level_matrix);
-      //inverse.debug("in plan_get inverse");
-      position.apply_rotation(inverse);
-      //position.debug("after rotation");
-
-      return position;
-    }
-  #endif //!DELTA
+    return position;
+  }
+#endif // ENABLE_AUTO_BED_LEVELING && !DELTA
 
+#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
   void plan_set_position(float x, float y, float z, const float &e)
 #else
   void plan_set_position(const float &x, const float &y, const float &z, const float &e)
-#endif // ENABLE_AUTO_BED_LEVELING
+#endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
   {
     #ifdef ENABLE_AUTO_BED_LEVELING
       apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
+    #elif defined(MESH_BED_LEVELING)
+      if (mbl.active) z += mbl.get_z(x, y);
     #endif
 
     float nx = position[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]);
diff --git a/Marlin/planner.h b/Marlin/planner.h
index 9e0631e831..ed219fa234 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -79,23 +79,23 @@ extern volatile unsigned char block_buffer_head;
 extern volatile unsigned char block_buffer_tail;
 FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
 
-#ifdef ENABLE_AUTO_BED_LEVELING
-  #include "vector_3.h"
-  // this holds the required transform to compensate for bed level
-  extern matrix_3x3 plan_bed_level_matrix;
+#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
+  #if defined(ENABLE_AUTO_BED_LEVELING)
+    #include "vector_3.h"
+    // this holds the required transform to compensate for bed level
+    extern matrix_3x3 plan_bed_level_matrix;
+    // Get the position applying the bed level matrix if enabled
+    vector_3 plan_get_position();
+  #endif  // ENABLE_AUTO_BED_LEVELING
   // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
-  // millimaters. Feed rate specifies the speed of the motion.
+  // millimeters. Feed rate specifies the speed of the motion.
   void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
   // Set position. Used for G92 instructions.
   void plan_set_position(float x, float y, float z, const float &e);
-  #ifndef DELTA
-    // Get the position applying the bed level matrix if enabled
-    vector_3 plan_get_position();
-  #endif
-#else //!ENABLE_AUTO_BED_LEVELING
+#else
   void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
   void plan_set_position(const float &x, const float &y, const float &z, const float &e);
-#endif //!ENABLE_AUTO_BED_LEVELING
+#endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
 
 void plan_set_e_position(const float &e);
 
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index 5064742066..26360a5144 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -145,7 +145,7 @@ static volatile bool temp_meas_ready = false;
   static float temp_iState_min_bed;
   static float temp_iState_max_bed;
 #else //PIDTEMPBED
-	static unsigned long  previous_millis_bed_heater;
+  static unsigned long  previous_millis_bed_heater;
 #endif //PIDTEMPBED
   static unsigned char soft_pwm[EXTRUDERS];
 
@@ -243,7 +243,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
     SERIAL_ECHOLN(MSG_PID_BAD_EXTRUDER_NUM);
     return;
   }
-	
+  
   SERIAL_ECHOLN(MSG_PID_AUTOTUNE_START);
 
   disable_heater(); // switch off all heaters.
@@ -755,8 +755,8 @@ void manage_heater() {
   #ifdef FILAMENT_SENSOR
     if (filament_sensor) {
       meas_shift_index = delay_index1 - meas_delay_cm;
-		  if (meas_shift_index < 0) meas_shift_index += MAX_MEASUREMENT_DELAY + 1;  //loop around buffer if needed
-		  
+      if (meas_shift_index < 0) meas_shift_index += MAX_MEASUREMENT_DELAY + 1;  //loop around buffer if needed
+      
       // Get the delayed info and add 100 to reconstitute to a percent of
       // the nominal filament diameter then square it to get an area
       meas_shift_index = constrain(meas_shift_index, 0, MAX_MEASUREMENT_DELAY);
@@ -1259,10 +1259,7 @@ enum TempState {
 ISR(TIMER0_COMPB_vect) {
   //these variables are only accesible from the ISR, but static, so they don't lose their value
   static unsigned char temp_count = 0;
-  static unsigned long raw_temp_0_value = 0;
-  static unsigned long raw_temp_1_value = 0;
-  static unsigned long raw_temp_2_value = 0;
-  static unsigned long raw_temp_3_value = 0;
+  static unsigned long raw_temp_value[EXTRUDERS] = { 0 };
   static unsigned long raw_temp_bed_value = 0;
   static TempState temp_state = StartupDelay;
   static unsigned char pwm_count = BIT(SOFT_PWM_SCALE);
@@ -1474,7 +1471,7 @@ ISR(TIMER0_COMPB_vect) {
       break;
     case MeasureTemp_0:
       #if HAS_TEMP_0
-        raw_temp_0_value += ADC;
+        raw_temp_value[0] += ADC;
       #endif
       temp_state = PrepareTemp_BED;
       break;
@@ -1500,7 +1497,7 @@ ISR(TIMER0_COMPB_vect) {
       break;
     case MeasureTemp_1:
       #if HAS_TEMP_1
-        raw_temp_1_value += ADC;
+        raw_temp_value[1] += ADC;
       #endif
       temp_state = PrepareTemp_2;
       break;
@@ -1513,7 +1510,7 @@ ISR(TIMER0_COMPB_vect) {
       break;
     case MeasureTemp_2:
       #if HAS_TEMP_2
-        raw_temp_2_value += ADC;
+        raw_temp_value[2] += ADC;
       #endif
       temp_state = PrepareTemp_3;
       break;
@@ -1526,7 +1523,7 @@ ISR(TIMER0_COMPB_vect) {
       break;
     case MeasureTemp_3:
       #if HAS_TEMP_3
-        raw_temp_3_value += ADC;
+        raw_temp_value[3] += ADC;
       #endif
       temp_state = Prepare_FILWIDTH;
       break;
@@ -1561,19 +1558,19 @@ ISR(TIMER0_COMPB_vect) {
   if (temp_count >= OVERSAMPLENR) { // 10 * 16 * 1/(16000000/64/256)  = 164ms.
     if (!temp_meas_ready) { //Only update the raw values if they have been read. Else we could be updating them during reading.
       #ifndef HEATER_0_USES_MAX6675
-        current_temperature_raw[0] = raw_temp_0_value;
+        current_temperature_raw[0] = raw_temp_value[0];
       #endif
       #if EXTRUDERS > 1
-        current_temperature_raw[1] = raw_temp_1_value;
+        current_temperature_raw[1] = raw_temp_value[1];
         #if EXTRUDERS > 2
-          current_temperature_raw[2] = raw_temp_2_value;
+          current_temperature_raw[2] = raw_temp_value[2];
           #if EXTRUDERS > 3
-            current_temperature_raw[3] = raw_temp_3_value;
+            current_temperature_raw[3] = raw_temp_value[3];
           #endif
         #endif
       #endif
       #ifdef TEMP_SENSOR_1_AS_REDUNDANT
-        redundant_temperature_raw = raw_temp_1_value;
+        redundant_temperature_raw = raw_temp_value[1];
       #endif
       current_temperature_bed_raw = raw_temp_bed_value;
     } //!temp_meas_ready
@@ -1585,31 +1582,67 @@ ISR(TIMER0_COMPB_vect) {
     
     temp_meas_ready = true;
     temp_count = 0;
-    raw_temp_0_value = 0;
-    raw_temp_1_value = 0;
-    raw_temp_2_value = 0;
-    raw_temp_3_value = 0;
+    for (int i = 0; i < EXTRUDERS; i++) raw_temp_value[i] = 0;
     raw_temp_bed_value = 0;
 
     #if HEATER_0_RAW_LO_TEMP > HEATER_0_RAW_HI_TEMP
-      #define MAXTEST <=
-      #define MINTEST >=
+      #define GE0 <=
+      #define LE0 >=
     #else
-      #define MAXTEST >=
-      #define MINTEST <=
+      #define GE0 >=
+      #define LE0 <=
+    #endif
+    if (current_temperature_raw[0] GE0 maxttemp_raw[0]) max_temp_error(0);
+    if (current_temperature_raw[0] LE0 minttemp_raw[0]) min_temp_error(0);
+
+    #if EXTRUDERS > 1
+      #if HEATER_1_RAW_LO_TEMP > HEATER_1_RAW_HI_TEMP
+        #define GE1 <=
+        #define LE1 >=
+      #else
+        #define GE1 >=
+        #define LE1 <=
+      #endif
+      if (current_temperature_raw[1] GE1 maxttemp_raw[1]) max_temp_error(1);
+      if (current_temperature_raw[1] LE1 minttemp_raw[1]) min_temp_error(1);
+      #if EXTRUDERS > 2
+        #if HEATER_2_RAW_LO_TEMP > HEATER_2_RAW_HI_TEMP
+          #define GE2 <=
+          #define LE2 >=
+        #else
+          #define GE2 >=
+          #define LE2 <=
+        #endif
+        if (current_temperature_raw[2] GE2 maxttemp_raw[2]) max_temp_error(2);
+        if (current_temperature_raw[2] LE2 minttemp_raw[2]) min_temp_error(2);
+        #if EXTRUDERS > 3
+          #if HEATER_3_RAW_LO_TEMP > HEATER_3_RAW_HI_TEMP
+            #define GE3 <=
+            #define LE3 >=
+          #else
+            #define GE3 >=
+            #define LE3 <=
+          #endif
+          if (current_temperature_raw[3] GE3 maxttemp_raw[3]) max_temp_error(3);
+          if (current_temperature_raw[3] LE3 minttemp_raw[3]) min_temp_error(3);
+        #endif // EXTRUDERS > 3
+      #endif // EXTRUDERS > 2
+    #endif // EXTRUDERS > 1
+
+    #if defined(BED_MAXTEMP) && (TEMP_SENSOR_BED != 0)
+      #if HEATER_BED_RAW_LO_TEMP > HEATER_BED_RAW_HI_TEMP
+        #define GEBED <=
+        #define LEBED >=
+      #else
+        #define GEBED >=
+        #define LEBED <=
+      #endif
+      if (current_temperature_bed_raw GEBED bed_maxttemp_raw) {
+        target_temperature_bed = 0;
+        bed_max_temp_error();
+      }
     #endif
 
-    for (int i=0; i<EXTRUDERS; i++) {
-      if (current_temperature_raw[i] MAXTEST maxttemp_raw[i]) max_temp_error(i);
-      else if (current_temperature_raw[i] MINTEST minttemp_raw[i]) min_temp_error(i);
-    }
-    /* No bed MINTEMP error? */
-    #if defined(BED_MAXTEMP) && (TEMP_SENSOR_BED != 0)
-      if (current_temperature_bed_raw MAXTEST bed_maxttemp_raw) {
-          target_temperature_bed = 0;
-          bed_max_temp_error();
-        }
-    #endif
   } // temp_count >= OVERSAMPLENR
 
   #ifdef BABYSTEPPING
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 610b21ccb6..a0f5e739af 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -70,6 +70,13 @@ static void lcd_sdcard_menu();
 static void lcd_delta_calibrate_menu();
 #endif // DELTA_CALIBRATION_MENU
 
+#if defined(MANUAL_BED_LEVELING)
+#include "mesh_bed_leveling.h"
+static void _lcd_level_bed();
+static void _lcd_level_bed_homing();
+static void lcd_level_bed();
+#endif  // MANUAL_BED_LEVELING
+
 static void lcd_quick_feedback();//Cause an LCD refresh, and give the user visual or audible feedback that something has happened
 
 /* Different types of actions that can be used in menu items. */
@@ -198,8 +205,8 @@ static void menu_action_setting_edit_callback_long5(const char* pstr, unsigned l
   #define MENU_MULTIPLIER_ITEM_EDIT_CALLBACK(type, label, args...) MENU_ITEM(setting_edit_callback_ ## type, label, PSTR(label), ## args)
 #endif //!ENCODER_RATE_MULTIPLIER
 #define END_MENU() \
-    if (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM >= _menuItemNr) encoderPosition = _menuItemNr * ENCODER_STEPS_PER_MENU_ITEM - 1; \
-    if ((uint8_t)(encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) >= currentMenuViewOffset + LCD_HEIGHT) { currentMenuViewOffset = (encoderPosition / ENCODER_STEPS_PER_MENU_ITEM) - LCD_HEIGHT + 1; lcdDrawUpdate = 1; _lineNr = currentMenuViewOffset - 1; _drawLineNr = -1; } \
+    if (encoderLine >= _menuItemNr) encoderPosition = _menuItemNr * ENCODER_STEPS_PER_MENU_ITEM - 1; encoderLine = encoderPosition / ENCODER_STEPS_PER_MENU_ITEM;\
+    if (encoderLine >= currentMenuViewOffset + LCD_HEIGHT) { currentMenuViewOffset = encoderLine - LCD_HEIGHT + 1; lcdDrawUpdate = 1; _lineNr = currentMenuViewOffset - 1; _drawLineNr = -1; } \
     } } while(0)
 
 /** Used variables to keep track of the menu */
@@ -630,6 +637,10 @@ static void lcd_prepare_menu() {
     }
   #endif
   MENU_ITEM(submenu, MSG_MOVE_AXIS, lcd_move_menu);
+
+  #if defined(MANUAL_BED_LEVELING)
+    MENU_ITEM(submenu, MSG_LEVEL_BED, lcd_level_bed);
+  #endif
 	
   END_MENU();
 }
@@ -1341,7 +1352,12 @@ void lcd_update() {
     #endif
 
     #ifdef ULTIPANEL
-      if (currentMenu != lcd_status_screen && millis() > timeoutToStatus) {
+      if (currentMenu != lcd_status_screen &&
+        #if defined(MANUAL_BED_LEVELING)
+          currentMenu != _lcd_level_bed && 
+          currentMenu != _lcd_level_bed_homing && 
+        #endif  // MANUAL_BED_LEVELING
+          millis() > timeoutToStatus) {
         lcd_return_to_status();
         lcdDrawUpdate = 2;
       }
@@ -1760,4 +1776,75 @@ char *ftostr52(const float &x)
   return conv;
 }
 
+#if defined(MANUAL_BED_LEVELING)
+static int _lcd_level_bed_position;
+static void _lcd_level_bed()
+{
+  if (encoderPosition != 0) {
+    refresh_cmd_timeout();
+    current_position[Z_AXIS] += float((int)encoderPosition) * 0.05;
+    if (min_software_endstops && current_position[Z_AXIS] < Z_MIN_POS) current_position[Z_AXIS] = Z_MIN_POS;
+    if (max_software_endstops && current_position[Z_AXIS] > Z_MAX_POS) current_position[Z_AXIS] = Z_MAX_POS;
+    encoderPosition = 0;
+    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[Z_AXIS]/60, active_extruder);
+    lcdDrawUpdate = 1;
+  }
+  if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR("Z"), ftostr32(current_position[Z_AXIS]));
+  static bool debounce_click = false;
+  if (LCD_CLICKED) {
+    if (!debounce_click) {
+      debounce_click = true;
+      int ix = _lcd_level_bed_position % MESH_NUM_X_POINTS;
+      int iy = _lcd_level_bed_position / MESH_NUM_X_POINTS;
+      mbl.set_z(ix, iy, current_position[Z_AXIS]);
+      _lcd_level_bed_position++;
+      if (_lcd_level_bed_position == MESH_NUM_X_POINTS*MESH_NUM_Y_POINTS) {
+        current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[X_AXIS]/60, active_extruder);
+        mbl.active = 1;
+        enquecommands_P(PSTR("G28"));
+        lcd_return_to_status();
+      } else {
+        current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[X_AXIS]/60, active_extruder);
+        ix = _lcd_level_bed_position % MESH_NUM_X_POINTS;
+        iy = _lcd_level_bed_position / MESH_NUM_X_POINTS;
+        if (iy&1) { // Zig zag
+          ix = (MESH_NUM_X_POINTS - 1) - ix;
+        }
+        current_position[X_AXIS] = mbl.get_x(ix);
+        current_position[Y_AXIS] = mbl.get_y(iy);
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[X_AXIS]/60, active_extruder);
+        lcdDrawUpdate = 1;
+      }
+    }
+  } else {
+    debounce_click = false;
+  }
+}
+static void _lcd_level_bed_homing()
+{
+  if (axis_known_position[X_AXIS] &&
+      axis_known_position[Y_AXIS] &&
+      axis_known_position[Z_AXIS]) {
+    current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+    current_position[X_AXIS] = MESH_MIN_X;
+    current_position[Y_AXIS] = MESH_MIN_Y;
+    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[X_AXIS]/60, active_extruder);
+    _lcd_level_bed_position = 0;
+    lcd_goto_menu(_lcd_level_bed);
+  }
+}
+static void lcd_level_bed()
+{
+  axis_known_position[X_AXIS] = false;
+  axis_known_position[Y_AXIS] = false;
+  axis_known_position[Z_AXIS] = false;
+  mbl.reset();
+  enquecommands_P(PSTR("G28"));
+  lcd_goto_menu(_lcd_level_bed_homing);
+}
+#endif  // MANUAL_BED_LEVELING
+
 #endif //ULTRA_LCD