From 0e51e53813c3f2ccd64e09c2cbc16c2740b3974c Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Sun, 15 Mar 2015 10:43:26 +0100
Subject: [PATCH 1/9] WIP. Adding bed leveling code.

---
 Marlin/Configuration.h       | 16 ++++++++
 Marlin/Marlin_main.cpp       | 73 ++++++++++++++++++++++++++++++++++--
 Marlin/mesh_bed_leveling.cpp |  7 ++++
 Marlin/mesh_bed_leveling.h   | 69 ++++++++++++++++++++++++++++++++++
 Marlin/planner.cpp           | 21 ++++++++---
 Marlin/planner.h             | 11 +++---
 6 files changed, 184 insertions(+), 13 deletions(-)
 create mode 100644 Marlin/mesh_bed_leveling.cpp
 create mode 100644 Marlin/mesh_bed_leveling.h

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index a2c43bd3d11..63be50d388c 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -372,6 +372,22 @@ 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 4
+  #define MESH_NUM_Y_POINTS 3
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index e7298c43cb2..b07a2db1ec9 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"
@@ -4987,6 +4991,65 @@ void calculate_delta(float cartesian[3])
 }
 #endif
 
+#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)
+{
+  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 (ix > pix && (x_splits)&(1<<ix)) {
+    float nx = mbl.get_x(ix);
+    float normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
+    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;
+    mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
+    mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
+    return;
+  } else if (ix < pix && (x_splits)&(1<<pix)) {
+    float nx = mbl.get_x(pix);
+    float normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
+    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;
+    mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
+    mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
+    return;
+  } else if (iy > piy && (y_splits)&(1<<iy)) {
+    float ny = mbl.get_y(iy);
+    float normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
+    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;
+    mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
+    mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
+    return;
+  } else if (iy < piy && (y_splits)&(1<<piy)) {
+    float ny = mbl.get_y(piy);
+    float normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
+    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;
+    mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
+    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);
+  for(int8_t i=0; i < NUM_AXIS; i++) {
+    current_position[i] = destination[i];
+  }
+}
+#endif  // MESH_BED_LEVELING
+
 void prepare_move()
 {
   clamp_to_software_endstops(destination);
@@ -5102,10 +5165,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/mesh_bed_leveling.cpp b/Marlin/mesh_bed_leveling.cpp
new file mode 100644
index 00000000000..dc82d376edc
--- /dev/null
+++ b/Marlin/mesh_bed_leveling.cpp
@@ -0,0 +1,7 @@
+#include "mesh_bed_leveling.h"
+
+#if defined(MESH_BED_LEVELING)
+
+mesh_bed_leveling mbl;
+
+#endif  // MESH_BED_LEVELING
diff --git a/Marlin/mesh_bed_leveling.h b/Marlin/mesh_bed_leveling.h
new file mode 100644
index 00000000000..bd48f564e19
--- /dev/null
+++ b/Marlin/mesh_bed_leveling.h
@@ -0,0 +1,69 @@
+#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:
+    
+    float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
+    
+    mesh_bed_leveling() {
+        reset();
+    }
+    
+    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;
+            }
+        }
+    }
+    
+    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 9bcad5661db..1c7a3a9b38d 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,10 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
     lcd_update();
   }
 
+#if defined(MESH_BED_LEVELING)
+  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 +1086,17 @@ 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)
+  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 a64a0f0d3c9..22443c05fb2 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -82,21 +82,22 @@ 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);
-
 // Get the position applying the bed level matrix if enabled
+#if defined(ENABLE_AUTO_BED_LEVELING)
 vector_3 plan_get_position();
+#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);
 

From 8005d22c8107bfa60c265969a636219fd0ed86fc Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Sun, 15 Mar 2015 23:18:11 +0100
Subject: [PATCH 2/9] Added menu option for bed leveling.

---
 Marlin/Configuration.h        |  3 +-
 Marlin/ConfigurationStore.cpp | 24 +++++++++-
 Marlin/Marlin_main.cpp        | 55 +++++++++++++++++++++-
 Marlin/language_en.h          |  3 ++
 Marlin/mesh_bed_leveling.cpp  | 13 +++++
 Marlin/mesh_bed_leveling.h    | 14 ++----
 Marlin/planner.cpp            |  8 +++-
 Marlin/ultralcd.cpp           | 89 ++++++++++++++++++++++++++++++++++-
 8 files changed, 192 insertions(+), 17 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 63be50d388c..23be0110580 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 0dee05ba789..6e0eeb04cff 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 b07a2db1ec9..5fc6d9c0465 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 636d622abad..0998d22ad77 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 dc82d376edc..b383fe589a8 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 bd48f564e19..d76321ab42d 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 1c7a3a9b38d..3c3cb2832c8 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 8575abbd0a8..daddfbfb925 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

From f34b9c83d1596057f724d0eeb5c21233a546cd13 Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Mon, 16 Mar 2015 01:48:46 +0100
Subject: [PATCH 3/9] Added comment about MESH_NUM axis points.

---
 Marlin/Configuration.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 23be0110580..6fc0362b91c 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -384,7 +384,7 @@ 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 3
+  #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

From e983a5ab92b7db6aa6577d85425ca399771ff424 Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Mon, 16 Mar 2015 02:50:11 +0100
Subject: [PATCH 4/9] Disable option. Enable for use/test.

---
 Marlin/Configuration.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 6fc0362b91c..da9d2b60af9 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -376,8 +376,8 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 //============================ Manual Bed Leveling ==========================
 //===========================================================================
 
-#define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
-#define MESH_BED_LEVELING    // Enable mesh 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

From 0d43898a22b0d8f77ef0c84e0809e6a254cde1e5 Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Tue, 17 Mar 2015 22:55:29 +0100
Subject: [PATCH 5/9] Remove of mesh_plan_buffer_line parameter reference (e)

---
 Marlin/Marlin_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 82d28b1b564..81cfe9951e8 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -5307,7 +5307,7 @@ void prepare_move_raw()
 #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)
+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);

From 28c91deb5c3813acb8f92f36afe3c3b975f01146 Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Wed, 18 Mar 2015 21:00:31 +0100
Subject: [PATCH 6/9] EEPROM saving of z_values. Tried to make it a little
 intelligent.

---
 Marlin/ConfigurationStore.cpp | 41 +++++++++++++++++++++++++++++++++--
 1 file changed, 39 insertions(+), 2 deletions(-)

diff --git a/Marlin/ConfigurationStore.cpp b/Marlin/ConfigurationStore.cpp
index 6e0eeb04cff..1d582d9ac20 100644
--- a/Marlin/ConfigurationStore.cpp
+++ b/Marlin/ConfigurationStore.cpp
@@ -113,7 +113,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
 
@@ -136,9 +136,26 @@ 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
@@ -277,9 +294,29 @@ 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, mbl.z_values);
+      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

From c87faa69ed9542d7ad5711f2ed85552792b7924f Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Thu, 19 Mar 2015 20:15:38 +0100
Subject: [PATCH 7/9] Shortened mesh_plan_buffer_line()

---
 Marlin/Marlin_main.cpp | 109 ++++++++++++++++++-----------------------
 1 file changed, 47 insertions(+), 62 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 6994184b861..0489c63eac8 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -5331,71 +5331,56 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
   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 (ix > pix && (x_splits)&(1<<ix)) {
-    float nx = mbl.get_x(ix);
-    float normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
-    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)) {
-    float nx = mbl.get_x(pix);
-    float normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
-    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)) {
-    float ny = mbl.get_y(iy);
-    float normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
-    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)) {
-    float ny = mbl.get_y(piy);
-    float normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
-    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);
+  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;
   }
-  plan_buffer_line(x, y, z, e, feed_rate, extruder);
-  for(int8_t i=0; i < NUM_AXIS; i++) {
-    current_position[i] = destination[i];
+  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
 

From 5112cf84227de2d8739c4368f7e33e7dea1cb2ad Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Fri, 20 Mar 2015 11:46:47 +0100
Subject: [PATCH 8/9] Added comment for the EEPROM storage

---
 Marlin/ConfigurationStore.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/Marlin/ConfigurationStore.cpp b/Marlin/ConfigurationStore.cpp
index 1d582d9ac20..16d94760b68 100644
--- a/Marlin/ConfigurationStore.cpp
+++ b/Marlin/ConfigurationStore.cpp
@@ -22,6 +22,8 @@
  *
  * Mesh bed leveling:
  *  active
+ *  mesh_num_x
+ *  mesh_num_y
  *  z_values[][]
  *
  * DELTA:

From 3d0a060a7a998b91d815a822bdf7774e3a8b9f6a Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Sat, 21 Mar 2015 14:50:47 +0100
Subject: [PATCH 9/9] Added G29 command

---
 Marlin/Marlin_main.cpp | 89 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 89 insertions(+)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 87eb77a1410..41955d86a67 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1983,6 +1983,89 @@ inline void gcode_G28() {
   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
@@ -4687,6 +4770,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.