diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 63be50d388..23be011058 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -384,8 +384,9 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
   #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 4
+  #define MESH_NUM_X_POINTS 3
   #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
 
 //===========================================================================
diff --git a/Marlin/ConfigurationStore.cpp b/Marlin/ConfigurationStore.cpp
index 0dee05ba78..6e0eeb04cf 100644
--- a/Marlin/ConfigurationStore.cpp
+++ b/Marlin/ConfigurationStore.cpp
@@ -20,6 +20,10 @@
  *  max_e_jerk
  *  add_homing (x3)
  *
+ * Mesh bed leveling:
+ *  active
+ *  z_values[][]
+ *
  * DELTA:
  *  endstop_adj (x3)
  *  delta_radius
@@ -69,6 +73,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--) {
@@ -128,6 +136,11 @@ void Config_StoreSettings()  {
   EEPROM_WRITE_VAR(i, max_e_jerk);
   EEPROM_WRITE_VAR(i, add_homing);
 
+  #if defined(MESH_BED_LEVELING)
+    EEPROM_WRITE_VAR(i, mbl.active);
+    EEPROM_WRITE_VAR(i, mbl.z_values);
+  #endif  // MESH_BED_LEVELING
+
   #ifdef DELTA
     EEPROM_WRITE_VAR(i, endstop_adj);               // 3 floats
     EEPROM_WRITE_VAR(i, delta_radius);              // 1 float
@@ -250,7 +263,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 +277,11 @@ void Config_RetrieveSettings() {
     EEPROM_READ_VAR(i, max_e_jerk);
     EEPROM_READ_VAR(i, add_homing);
 
+    #if defined(MESH_BED_LEVELING)
+      EEPROM_READ_VAR(i, mbl.active);
+      EEPROM_READ_VAR(i, mbl.z_values);
+    #endif  // MESH_BED_LEVELING
+
     #ifdef DELTA
       EEPROM_READ_VAR(i, endstop_adj);                // 3 floats
       EEPROM_READ_VAR(i, delta_radius);               // 1 float
@@ -392,6 +410,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 b07a2db1ec..5fc6d9c046 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1566,6 +1566,11 @@ inline void gcode_G28() {
     plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
   #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;
@@ -1780,6 +1785,23 @@ 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();
@@ -4998,6 +5020,13 @@ void calculate_delta(float cartesian[3])
 // 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);
@@ -5012,7 +5041,13 @@ void mesh_plan_buffer_line(float x, float y, float z, const float &e, float feed
     float ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
     float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
     x_splits ^= 1 << ix;
+    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);
     return;
   } else if (ix < pix && (x_splits)&(1<<pix)) {
@@ -5021,7 +5056,13 @@ void mesh_plan_buffer_line(float x, float y, float z, const float &e, float feed
     float ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
     float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
     x_splits ^= 1 << pix;
+    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);
     return;
   } else if (iy > piy && (y_splits)&(1<<iy)) {
@@ -5030,7 +5071,13 @@ void mesh_plan_buffer_line(float x, float y, float z, const float &e, float feed
     float nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
     float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
     y_splits ^= 1 << iy;
+    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);
     return;
   } else if (iy < piy && (y_splits)&(1<<piy)) {
@@ -5039,11 +5086,17 @@ void mesh_plan_buffer_line(float x, float y, float z, const float &e, float feed
     float nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
     float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
     y_splits ^= 1 << piy;
+    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);
     return;
   }
-  plan_buffer_line(x, y, z, e, feedrate, extruder);
+  plan_buffer_line(x, y, z, e, feed_rate, extruder);
   for(int8_t i=0; i < NUM_AXIS; i++) {
     current_position[i] = destination[i];
   }
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
index dc82d376ed..b383fe589a 100644
--- a/Marlin/mesh_bed_leveling.cpp
+++ b/Marlin/mesh_bed_leveling.cpp
@@ -4,4 +4,17 @@
 
 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
index bd48f564e1..d76321ab42 100644
--- a/Marlin/mesh_bed_leveling.h
+++ b/Marlin/mesh_bed_leveling.h
@@ -7,20 +7,12 @@
 
 class mesh_bed_leveling {
 public:
-    
+    uint8_t active;
     float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
     
-    mesh_bed_leveling() {
-        reset();
-    }
+    mesh_bed_leveling();
     
-    void 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;
-            }
-        }
-    }
+    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; }
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 1c7a3a9b38..3c3cb2832c 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -553,7 +553,9 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
   }
 
 #if defined(MESH_BED_LEVELING)
-  z += mbl.get_z(x, y);
+  if (mbl.active) {
+    z += mbl.get_z(x, y);
+  }
 #endif  // MESH_BED_LEVELING
 
 #ifdef ENABLE_AUTO_BED_LEVELING
@@ -1095,7 +1097,9 @@ void plan_set_position(const float &x, const float &y, const float &z, const flo
 #if defined(ENABLE_AUTO_BED_LEVELING)
   apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
 #elif defined(MESH_BED_LEVELING)
-  z += mbl.get_z(x, y);
+  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]);
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 8575abbd0a..daddfbfb92 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -68,6 +68,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. */
@@ -615,6 +622,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();
 }
@@ -1326,7 +1337,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;
       }
@@ -1745,4 +1761,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