diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 2ba65bc58..b0b19e244 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -370,6 +370,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 0dee05ba7..16d94760b 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 58679e0f8..c64f7eb32 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -38,6 +38,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"
@@ -1727,6 +1731,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;
@@ -1941,12 +1950,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
 
   /**
@@ -4613,6 +4722,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.
@@ -5232,6 +5347,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);
@@ -5347,10 +5537,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/language_en.h b/Marlin/language_en.h
index 636d622ab..0998d22ad 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 000000000..b383fe589
--- /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 000000000..d76321ab4
--- /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 316c0de2f..27ca0e850 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 ============================
 //===========================================================================
@@ -530,7 +534,7 @@ float junction_deviation = 0.1;
 // Add a new linear movement to the buffer. steps_x, _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)
@@ -548,6 +552,12 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
     lcd_update();
   }
 
+#if defined(MESH_BED_LEVELING)
+  if (mbl.active) {
+    z += mbl.get_z(x, y);
+  }
+#endif  // MESH_BED_LEVELING
+
 #ifdef ENABLE_AUTO_BED_LEVELING
   apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
 #endif // ENABLE_AUTO_BED_LEVELING
@@ -1078,14 +1088,19 @@ vector_3 plan_get_position() {
 }
 #endif // ENABLE_AUTO_BED_LEVELING
 
-#ifdef ENABLE_AUTO_BED_LEVELING
+#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
 void plan_set_position(float x, float y, float z, const float &e)
-{
-  apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
 #else
 void plan_set_position(const float &x, const float &y, const float &z, const float &e)
+#endif  // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
 {
-#endif // ENABLE_AUTO_BED_LEVELING
+#if defined(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  // ENABLE_AUTO_BED_LEVELING
 
   position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
   position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
diff --git a/Marlin/planner.h b/Marlin/planner.h
index 6b68d14cb..49ccbe9dd 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -82,23 +82,24 @@ void plan_init();
 // 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.
 
-#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);
-
+#if defined(ENABLE_AUTO_BED_LEVELING)
   #ifndef DELTA
   // Get the position applying the bed level matrix if enabled
   vector_3 plan_get_position();
   #endif
+#endif  // 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);
-#endif // ENABLE_AUTO_BED_LEVELING
+#endif  // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
 
 // Set position. Used for G92 instructions.
-#ifdef ENABLE_AUTO_BED_LEVELING
+#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
 
 void plan_set_e_position(const float &e);
 
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index 24a2dc31c..0bc5a6a5b 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -109,7 +109,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];
 
@@ -207,7 +207,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.
@@ -670,8 +670,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);
@@ -1174,10 +1174,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);
@@ -1389,7 +1386,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;
@@ -1415,7 +1412,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;
@@ -1428,7 +1425,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;
@@ -1441,7 +1438,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;
@@ -1476,19 +1473,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
@@ -1500,31 +1497,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
 
-    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();
-        }
+      #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
+
   } // temp_count >= OVERSAMPLENR
 
   #ifdef BABYSTEPPING
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 5ae32b0b9..f59ccce78 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -69,6 +69,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. */
@@ -629,6 +636,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();
 }
@@ -1336,7 +1347,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;
       }
@@ -1755,4 +1771,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