diff --git a/bed_leveling.c b/bed_leveling.c
new file mode 100644
index 000000000..b3d386486
--- /dev/null
+++ b/bed_leveling.c
@@ -0,0 +1,199 @@
+/** \file
+  \brief Dynamic Z-height compensation for out-of-level print bed.
+*/
+
+#include "bed_leveling.h"
+
+#ifdef BED_LEVELING
+
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "dda_maths.h"
+#include "debug.h"
+#include "sersendf.h"
+#include "config_wrapper.h"
+
+struct point {
+  int32_t x, y, z;
+};
+
+// Accept exactly three points for bed leveling
+static uint8_t level_index = 0;
+static struct point bed_level_map[3];
+
+// Alias the three points
+static struct point const * const P = &bed_level_map[0];
+static struct point const * const Q = &bed_level_map[1];
+static struct point const * const R = &bed_level_map[2];
+
+int bed_plane_calculate(void);
+
+// Reset the bed level values to "unknown"
+void bed_level_reset() {
+  level_index = 0;
+}
+
+// Scale x and y down to tenths of mm to keep math from overflowing
+#define SCALE 100
+static int32_t scale_to_dum(int32_t a) {
+  if (a<0) return (a-SCALE/2)/SCALE;
+  return (a+SCALE/2)/SCALE;
+}
+
+// Register a point on the bed plane; coordinates in um
+void bed_level_register(int32_t x, int32_t y, int32_t z) {
+  // Scale x and y to tenths of mm;  keep z in um
+  x = scale_to_dum(x);
+  y = scale_to_dum(y);
+
+  // Find a previous registered point at the same location or use a new location
+  struct point * p = bed_level_map;
+  int i = 0;
+  for (; i < level_index; i++, p++) {
+    if (p->x == x && p->y == y)
+      break;
+  }
+
+  // We can only handle three points
+  if (i >= 3)
+    return;
+
+  p->x = x;
+  p->y = y;
+  p->z = z;
+
+  // Bump the index if we just used a new location
+  if (i == level_index)
+    ++level_index;
+
+  // Nothing more to do until we have three points
+  if (level_index < 3)
+    return;
+
+  // We have three points. Try to calculate the plane of the bed.
+  if (!bed_plane_calculate())
+    --level_index;
+}
+
+// Pre-scaled coefficients of the planar equation,
+//   Ax + By + Cz + K= 0
+//
+// When we have these coefficients, we're only going to use them relative to -1/C, so
+//   Ac = -A/C;  Bc = -B/C;  Kc = 0 (because we translate a point to the origin)
+//   f(x,y) = z = Ac*x + Bc*y + Kc
+static uint32_t Aq, Ar, Bq, Br, C;
+static int8_t Asign, Bsign;
+
+int bed_leveling_active() {
+  // No adjustment if not calibrated yet
+  return level_index == 3;
+}
+
+void bed_level_report() {
+  sersendf_P(PSTR("Bed leveling status:"));
+  if (!bed_leveling_active()) {
+    sersendf_P(PSTR(" not"));
+  }
+  sersendf_P(PSTR(" active (%d) positions registered\n"), level_index);
+  for (int i = 0; i < level_index; i++) {
+    sersendf_P(PSTR("  %d: G29 S1 X%lq Y%lq Z%lq\n"),
+                    i+1, bed_level_map[i].x * SCALE, bed_level_map[i].y * SCALE, bed_level_map[i].z);
+  }
+}
+
+int32_t bed_level_adjustment(int32_t x, int32_t y) {
+  int32_t za, zb;
+
+  // No adjustment if not calibrated yet
+  if (!bed_leveling_active())
+    return 0;
+
+  x = scale_to_dum(x);
+  y = scale_to_dum(y);
+
+  x -= P->x;
+  y -= P->y;
+
+  za = muldivQR(x, Aq, Ar, C);
+  if (Asign)
+    za = -za;
+
+  zb = muldivQR(y, Bq, Br, C);
+  if (Bsign)
+    zb = -zb;
+
+  return P->z - za - zb;
+}
+
+
+int bed_plane_calculate() {
+  // Coefficients of the planar equation,
+  //   Ax + By + Cz + K = 0
+  int32_t a, b, c;
+
+  // Draw two vectors on the plane, u = B-A and v = C-A
+  int32_t Ui, Uj, Uk, Vi, Vj, Vk;
+
+  // U = vector(QP)
+  Ui = Q->x - P->x;
+  Uj = Q->y - P->y;
+  Uk = Q->z - P->z;
+
+  // V = vector(RP)
+  Vi = R->x - P->x;
+  Vj = R->y - P->y;
+  Vk = R->z - P->z;
+
+  // Find normal vector (a,b,c) = (U x V) and is perpendicular to the plane
+  a = Uj*Vk - Uk*Vj;
+  b = Uk*Vi - Ui*Vk;
+  c = Ui*Vj - Uj*Vi;
+
+  // Notes:
+  //  * Ignore K (constant) by translating plane to pass through origin at P (0,0,0)
+  //  * if a==0 and b==0, the bed is already level; z-offset is still important
+  //  * if c==0 the bed is perpendicular or the points are colinear
+  if (c == 0)
+    return 0;
+
+  if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
+    sersendf_P(PSTR("Coefficients: A:%ld B:%ld C:%ld\n"), a, b, c);
+
+  // muldiv requires positive parameters
+  // remember the signs separately
+  Asign = a < 0;
+  Bsign = b < 0;
+  if (Asign) a = -a;
+  if (Bsign) b = -b;
+
+  // Combine A/C and B/C, so combine their signs, too
+  if (c < 0) {
+    c = -c;
+    Asign = !Asign;
+    Bsign = !Bsign;
+  }
+
+  // Pre-calc the coefficients A/C and B/C
+  Aq = a / c;
+  Ar = a % c;
+  Bq = b / c;
+  Br = b % c;
+  C = c;
+
+  int ret = 1;
+  // Sanity check
+  for (int i = 0; i < level_index; i++) {
+    int32_t x=bed_level_map[i].x * SCALE, y=bed_level_map[i].y * SCALE;
+    int32_t validation = bed_level_adjustment(x, y);
+    if (labs(validation - bed_level_map[i].z) > 10) {
+      sersendf_P(PSTR("!! Bed plane validation failed: Point %d: X:%lq Y:%lq  Expected Z:%lq  Calculated Z:%lq\n"),
+                      i+1, x, y, bed_level_map[i].z, validation);
+      ret = 0; // invalidate results on error
+    }
+  }
+
+  return ret;
+}
+
+#endif /* BED_LEVELING */
diff --git a/bed_leveling.h b/bed_leveling.h
new file mode 100644
index 000000000..18a44b166
--- /dev/null
+++ b/bed_leveling.h
@@ -0,0 +1,42 @@
+#ifndef	_BED_LEVELING_H
+#define	_BED_LEVELING_H
+
+#include "config_wrapper.h"
+
+#include <stdint.h>
+
+#include "dda.h"
+
+#ifdef BED_LEVELING
+
+// Clears all registered points and disables dynamic leveling
+void bed_level_reset(void);
+
+// Returns true if bed leveling a plane is mapped and leveling is active
+int bed_leveling_active(void);
+
+// Report information about bed leveling calculations
+void bed_level_report(void);
+
+// Read the z-adjustment for the given x,y position
+int32_t bed_level_adjustment(int32_t x, int32_t y);
+
+// Register a point as being "on the bed plane".  Three points are required
+// to define a plane.  After three non-colinear points are registered, the
+// adjustment is active and can be read from bed_level_adjustment.
+// Note: units for x, y and z are um but the three (X, Y) points should be
+// distinct enough in mm to define an accurate plane.
+void bed_level_register(int32_t x, int32_t y, int32_t z);
+
+#endif /* BED_LEVELING */
+
+static int32_t bed_level_offset(const axes_int32_t) __attribute__ ((always_inline));
+inline int32_t bed_level_offset(const axes_int32_t axis) {
+  #ifdef BED_LEVELING
+    return bed_level_adjustment(axis[X], axis[Y]);
+  #else
+    return 0;
+  #endif
+}
+
+#endif
diff --git a/config/printer.mendel.h b/config/printer.mendel.h
index 706c9e7f6..35d509133 100644
--- a/config/printer.mendel.h
+++ b/config/printer.mendel.h
@@ -184,9 +184,17 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
 */
 #define MAX_JERK_X               200
 #define MAX_JERK_Y               200
-#define MAX_JERK_Z               0
+#define MAX_JERK_Z               20
 #define MAX_JERK_E               200
 
+/** \def BED_LEVELING
+  Define this to enable dynamic bed leveling using the G29 command and
+  3-point planar bed mapping. Allows the printer to compensate dynamically
+  for a print bed which is flat but is not quite level.
+  Enabling bed-leveling requires about 2400 bytes of flash memory.
+*/
+//#define BED_LEVELING
+
 
 /***************************************************************************\
 *                                                                           *
diff --git a/config/printer.mendel90.h b/config/printer.mendel90.h
index 239705a58..3a01a3a31 100644
--- a/config/printer.mendel90.h
+++ b/config/printer.mendel90.h
@@ -184,9 +184,17 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
 */
 #define MAX_JERK_X               300
 #define MAX_JERK_Y               300
-#define MAX_JERK_Z               0
+#define MAX_JERK_Z               20
 #define MAX_JERK_E               300
 
+/** \def BED_LEVELING
+  Define this to enable dynamic bed leveling using the G29 command and
+  3-point planar bed mapping. Allows the printer to compensate dynamically
+  for a print bed which is flat but is not quite level.
+  Enabling bed-leveling requires about 2400 bytes of flash memory.
+*/
+//#define BED_LEVELING
+
 
 /***************************************************************************\
 *                                                                           *
diff --git a/config/printer.pcbscriber.h b/config/printer.pcbscriber.h
index 7030669f6..03dd26cab 100644
--- a/config/printer.pcbscriber.h
+++ b/config/printer.pcbscriber.h
@@ -184,9 +184,17 @@ DEFINE_HOMING(z_negative, x_negative, y_negative, none)
 */
 #define MAX_JERK_X               100
 #define MAX_JERK_Y               100
-#define MAX_JERK_Z               0
+#define MAX_JERK_Z               2
 #define MAX_JERK_E               200
 
+/** \def BED_LEVELING
+  Define this to enable dynamic bed leveling using the G29 command and
+  3-point planar bed mapping. Allows the printer to compensate dynamically
+  for a print bed which is flat but is not quite level.
+  Enabling bed-leveling requires about 2400 bytes of flash memory.
+*/
+//#define BED_LEVELING
+
 
 /***************************************************************************\
 *                                                                           *
diff --git a/config/printer.tronxy.h b/config/printer.tronxy.h
index db7094311..7912be8ab 100644
--- a/config/printer.tronxy.h
+++ b/config/printer.tronxy.h
@@ -184,9 +184,17 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
 */
 #define MAX_JERK_X               200
 #define MAX_JERK_Y               200
-#define MAX_JERK_Z               0
+#define MAX_JERK_Z               20
 #define MAX_JERK_E               200
 
+/** \def BED_LEVELING
+  Define this to enable dynamic bed leveling using the G29 command and
+  3-point planar bed mapping. Allows the printer to compensate dynamically
+  for a print bed which is flat but is not quite level.
+  Enabling bed-leveling requires about 2400 bytes of flash memory.
+*/
+//#define BED_LEVELING
+
 
 /***************************************************************************\
 *                                                                           *
diff --git a/config/printer.wolfstrap.h b/config/printer.wolfstrap.h
index a03cff3d8..ca103ec51 100644
--- a/config/printer.wolfstrap.h
+++ b/config/printer.wolfstrap.h
@@ -184,9 +184,17 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
 */
 #define MAX_JERK_X               20
 #define MAX_JERK_Y               20
-#define MAX_JERK_Z               0
+#define MAX_JERK_Z               2
 #define MAX_JERK_E               200
 
+/** \def BED_LEVELING
+  Define this to enable dynamic bed leveling using the G29 command and
+  3-point planar bed mapping. Allows the printer to compensate dynamically
+  for a print bed which is flat but is not quite level.
+  Enabling bed-leveling requires about 2400 bytes of flash memory.
+*/
+//#define BED_LEVELING
+
 
 /***************************************************************************\
 *                                                                           *
diff --git a/config_wrapper.h b/config_wrapper.h
index c9ec569b6..8558a43e6 100644
--- a/config_wrapper.h
+++ b/config_wrapper.h
@@ -85,6 +85,15 @@
   #undef LOOKAHEAD
 #endif
 
+/**
+  LOOKAHEAD won't work if Z-jerk is zero and bed leveling is active
+  because most moves will have Z-steps and lookahead will be skipped.
+*/
+#if defined BED_LEVELING && defined LOOKAHEAD && MAX_JERK_Z==0
+  #warning When bed-leveling is activated, lookahead will be ineffective \
+           because MAX_JERK_Z is zero.
+#endif
+
 /**
   Silently discard EECONFIG on ARM. Silently to not disturb regression tests.
 
diff --git a/configtool/miscellaneouspage.py b/configtool/miscellaneouspage.py
index c9e6e9e21..79113b348 100644
--- a/configtool/miscellaneouspage.py
+++ b/configtool/miscellaneouspage.py
@@ -13,6 +13,7 @@ def __init__(self, parent, nb, idPg, font):
     self.font = font
 
     self.labels = {'USE_INTERNAL_PULLUPS': "Use Internal Pullups",
+                   'BED_LEVELING': "Enable dynamic bed leveling",
                    'Z_AUTODISABLE': "Z Autodisable",
                    'EECONFIG': "Enable EEPROM Storage",
                    'BANG_BANG': "Enable",
@@ -79,6 +80,10 @@ def __init__(self, parent, nb, idPg, font):
     cb = self.addCheckBox(k, self.onCheckBox)
     sz.Add(cb, pos = (7, 1))
 
+    k = 'BED_LEVELING'
+    cb = self.addCheckBox(k, self.onCheckBox)
+    sz.Add(cb, pos = (8, 1))
+
     b = wx.StaticBox(self, wx.ID_ANY, "BANG BANG Bed Control")
     b.SetFont(font)
     sbox = wx.StaticBoxSizer(b, wx.VERTICAL)
diff --git a/configtool/printer.generic.h b/configtool/printer.generic.h
index 48ad46690..e4a518bff 100644
--- a/configtool/printer.generic.h
+++ b/configtool/printer.generic.h
@@ -184,9 +184,17 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
 */
 #define MAX_JERK_X               20
 #define MAX_JERK_Y               20
-#define MAX_JERK_Z               0
+#define MAX_JERK_Z               5
 #define MAX_JERK_E               200
 
+/** \def BED_LEVELING
+  Define this to enable dynamic bed leveling using the G29 command and
+  3-point planar bed mapping. Allows the printer to compensate dynamically
+  for a print bed which is flat but is not quite level.
+  Enabling bed-leveling requires about 2400 bytes of flash memory.
+*/
+#define BED_LEVELING
+
 
 /***************************************************************************\
 *                                                                           *
diff --git a/dda.c b/dda.c
index 9c5200af6..a3b4cf0ac 100644
--- a/dda.c
+++ b/dda.c
@@ -21,6 +21,7 @@
 #include "pinio.h"
 #include "memory_barrier.h"
 #include "home.h"
+#include "bed_leveling.h"
 //#include "graycode.c"
 
 #ifdef DC_EXTRUDER
@@ -112,7 +113,13 @@ void dda_init(void) {
 
   This is needed for example after homing or a G92. The new location must be in startpoint already.
 */
-void dda_new_startpoint(void) {
+void dda_new_startpoint() {
+  if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) {
+    int32_t z_offset = bed_level_offset(startpoint.axis);
+    sersendf_P(PSTR("\nReset: X %lq  Y %lq  Z %lq  Zofs %lq  F %lu\n"),
+               startpoint.axis[X], startpoint.axis[Y],
+               startpoint.axis[Z], z_offset, startpoint.F);
+  }
   axes_um_to_steps(startpoint.axis, startpoint_steps.axis);
   startpoint_steps.axis[E] = um_to_steps(startpoint.axis[E], E);
 }
@@ -943,22 +950,12 @@ void update_current_position() {
   DDA *dda = mb_tail_dda;
   enum axis_e i;
 
-  static const axes_uint32_t PROGMEM steps_per_m_P = {
-    STEPS_PER_M_X,
-    STEPS_PER_M_Y,
-    STEPS_PER_M_Z,
-    STEPS_PER_M_E
-  };
-
   if (dda != NULL) {
     uint32_t axis_um;
     axes_int32_t delta_um;
 
     for (i = X; i < AXIS_COUNT; i++) {
-      axis_um = muldiv(move_state.steps[i], 
-                      1000000, 
-                      pgm_read_dword(&steps_per_m_P[i]));
-
+      axis_um = steps_to_um(move_state.steps[i], i);
       delta_um[i] = (int32_t)get_direction(dda, i) * axis_um;
     }
 
@@ -968,6 +965,9 @@ void update_current_position() {
       current_position.axis[i] = dda->endpoint.axis[i] - delta_um[i];
     }
 
+    // Compensate for bed-leveling z-offset
+    current_position.axis[Z] -= bed_level_offset(current_position.axis);
+
     current_position.F = dda->endpoint.F;
   }
   else {
diff --git a/dda_kinematics.c b/dda_kinematics.c
index 43d2506c8..904e42353 100644
--- a/dda_kinematics.c
+++ b/dda_kinematics.c
@@ -6,28 +6,16 @@
 #include <stdlib.h>
 
 #include "dda_maths.h"
-
+#include "bed_leveling.h"
 
 void
 carthesian_to_carthesian(const TARGET *startpoint, const TARGET *target,
                          axes_uint32_t delta_um, axes_int32_t steps) {
-  enum axis_e i;
-
-  for (i = X; i < E; i++) {
-    delta_um[i] = (uint32_t)labs(target->axis[i] - startpoint->axis[i]);
-    steps[i] = um_to_steps(target->axis[i], i);
-  }
-
-  /* Replacing the above five lines with this costs about 200 bytes binary
-     size on AVR, but also takes about 120 clock cycles less during movement
-     preparation. The smaller version was kept for our Arduino Nano friends.
   delta_um[X] = (uint32_t)labs(target->axis[X] - startpoint->axis[X]);
-  steps[X] = um_to_steps(target->axis[X], X);
   delta_um[Y] = (uint32_t)labs(target->axis[Y] - startpoint->axis[Y]);
-  steps[Y] = um_to_steps(target->axis[Y], Y);
   delta_um[Z] = (uint32_t)labs(target->axis[Z] - startpoint->axis[Z]);
-  steps[Z] = um_to_steps(target->axis[Z], Z);
-  */
+
+  axes_um_to_steps_cartesian(target->axis, steps);
 }
 
 void
@@ -43,17 +31,15 @@ carthesian_to_corexy(const TARGET *startpoint, const TARGET *target,
 }
 
 void axes_um_to_steps_cartesian(const axes_int32_t um, axes_int32_t steps) {
-  enum axis_e i;
-
-  for (i = X; i < E; i++) {
-    steps[i] = um_to_steps(um[i], i);
-  }
+  steps[X] = um_to_steps(um[X], X);
+  steps[Y] = um_to_steps(um[Y], Y);
+  steps[Z] = um_to_steps(um[Z] + bed_level_offset(um), Z);
 }
 
 void axes_um_to_steps_corexy(const axes_int32_t um, axes_int32_t steps) {
   steps[X] = um_to_steps(um[X] + um[Y], X);
   steps[Y] = um_to_steps(um[X] - um[Y], Y);
-  steps[Z] = um_to_steps(um[Z], Z);
+  steps[Z] = um_to_steps(um[Z] + bed_level_offset(um), Z);
 }
 
 void delta_to_axes_cartesian(axes_int32_t delta) {
diff --git a/dda_maths.c b/dda_maths.c
index d3649401b..3b646cd12 100644
--- a/dda_maths.c
+++ b/dda_maths.c
@@ -28,6 +28,13 @@ const axes_uint32_t PROGMEM axis_qr_P = {
   (uint32_t)STEPS_PER_M_E % UM_PER_METER
 };
 
+const axes_uint32_t PROGMEM steps_per_m_P = {
+  (uint32_t)STEPS_PER_M_X,
+  (uint32_t)STEPS_PER_M_Y,
+  (uint32_t)STEPS_PER_M_Z,
+  (uint32_t)STEPS_PER_M_E
+};
+
 /*!
   Integer multiply-divide algorithm. Returns the same as muldiv(multiplicand, multiplier, divisor), but also allowing to use precalculated quotients and remainders.
 
diff --git a/dda_maths.h b/dda_maths.h
index ccb571e53..7e330f45a 100644
--- a/dda_maths.h
+++ b/dda_maths.h
@@ -34,6 +34,13 @@ inline int32_t um_to_steps(int32_t distance, enum axis_e a) {
                   pgm_read_dword(&axis_qr_P[a]), UM_PER_METER);
 }
 
+extern const axes_uint32_t steps_per_m_P;
+
+static int32_t steps_to_um(int32_t, enum axis_e) __attribute__ ((always_inline));
+inline int32_t steps_to_um(int32_t steps, enum axis_e a) {
+  return muldiv(steps, UM_PER_METER, pgm_read_dword(&steps_per_m_P[a]));
+}
+
 // approximate 2D distance
 uint32_t approx_distance(uint32_t dx, uint32_t dy);
 
diff --git a/gcode_process.c b/gcode_process.c
index d670f7150..f7e01477a 100644
--- a/gcode_process.c
+++ b/gcode_process.c
@@ -24,7 +24,7 @@
 #include	"config_wrapper.h"
 #include	"home.h"
 #include "sd.h"
-
+#include "bed_leveling.h"
 
 /// the current tool
 uint8_t tool;
@@ -224,6 +224,62 @@ void process_gcode_command() {
 				}
 				break;
 
+#ifdef BED_LEVELING
+      case 29:
+        //? --- G29: Bed leveling registration ---
+        //?
+        //? Example: G29 S1
+        //?
+        //? Registers the Z-offset for a specific point on the print bed.
+        //? In this case the current position is used as the registration
+        //? point, but a different position can be specified by including
+        //? the X, Y and Z coordinate values.
+        //?
+        //? Three points must be registered before the dynamic bed leveling
+        //? feature is activated. Once three points are registered, the bed
+        //? is mapped assuming a flat plane and Z-offsets are adjusted
+        //? automatically during movements to follow the mapped plane. The
+        //? adjusted position is not displayed to the client, for example
+        //? in M114 results.
+        //?
+        //? The S value controls the action as follows:
+        //?   S0 displays the current bed leveling status
+        //?   S1 registers a new point on the 3-point plane mapping
+        //?   S5 clears all registered points and disables dynamic leveling
+        //?
+        //?   G29 S1 X100 Y50 Z-0.3
+        //?
+        //? This command registers the specific point 100,50 => -0.3
+        //?
+        //?   G29 S1
+        //?
+        //? This command registers the current head position as a point in
+        //? the plane map.
+        //?
+
+        queue_wait();
+
+        if (next_target.seen_S) {
+          switch (next_target.S) {
+            case 5:   // reset bed leveling registration points
+              bed_level_reset();
+              break;
+
+            case 1:   // Register a new registration point
+              bed_level_register(next_target.target.axis[X], next_target.target.axis[Y], next_target.target.axis[Z]);
+              break;
+
+            case 0:   // Report leveling status
+              bed_level_report();
+              break;
+          }
+        }
+
+        // Restore position, ignoring any axes included in G29 cmd
+        next_target.target = startpoint;
+        break;
+#endif /* BED_LEVELING */
+
 			case 90:
 				//? --- G90: Set to Absolute Positioning ---
 				//?
diff --git a/home.c b/home.c
index 6a5348dee..4019fd9ca 100644
--- a/home.c
+++ b/home.c
@@ -1,5 +1,5 @@
 #include "home.h"
-
+#include "bed_leveling.h"
 /** \file
   \brief Homing routines
 */
@@ -159,6 +159,12 @@ void home_axis(enum axis_e n, int8_t dir, enum axis_endstop_e endstop_check) {
   queue_wait();
   set_axis_home_position(n, dir);
   dda_new_startpoint();
+
+#ifdef BED_LEVELING
+  // Move to calculated Z-plane offset
+  if (n==Z)
+    enqueue(&next_target.target);
+#endif /* BED_LEVELING */
 }
 
 void set_axis_home_position(enum axis_e n, int8_t dir) {
@@ -198,4 +204,8 @@ void set_axis_home_position(enum axis_e n, int8_t dir) {
     }
   }
   startpoint.axis[n] = next_target.target.axis[n] = home_position;
+  if (n == Z) {
+    // Compensate for z-offset that will be added in by next move
+    startpoint.axis[n] -= bed_level_offset(startpoint.axis);
+  }
 }
diff --git a/research/planes.py b/research/planes.py
new file mode 100644
index 000000000..148af302a
--- /dev/null
+++ b/research/planes.py
@@ -0,0 +1,107 @@
+#!/usr/bin/env python
+
+# Experiments with coefficients of a geometric plane
+
+# Resources:
+# http://www.wolframalpha.com/input/?i=plane+through+(1,-2,0),(4,-2,-2),(4,1,4)&lk=3
+#    ==> 2 x - 6 y + 3 z + 14 == 0
+
+
+# Translate a point relative to some origin
+def translate(point, origin):
+  return tuple([a-b for a,b in zip(point, origin)])
+
+# Given two points in 3d space, define a vector
+def vector(p1, p2):
+  return tuple([b-a for a,b in zip(p1,p2)])
+
+# Given two vectors in a plane, find the normal vector
+def normal(u, v):
+  # A normal vector is the cross-product of two coplanar vectors
+  return tuple([
+    u[1]*v[2] - u[2]*v[1],
+    u[2]*v[0] - u[0]*v[2],
+    u[0]*v[1] - u[1]*v[0]
+    ])
+
+def plane_from_three_points(P, Q, R):
+  u = vector(P, Q)
+  v = vector(P, R)
+  n = normal(u, v)
+
+  # Find the coefficients
+  (A,B,C) = n
+
+  # The equation of the plane is thus Ax+By+Cz+K=0.
+  # Solve for K to get the final coefficient
+  (x,y,z) = P
+  K = -(A*x + B*y + C*z)
+
+  return (A, B, C, K)
+
+# find the Z offset for any x,y
+# z = -(Ax + By + K) / C
+def calcz(x, y, plane, translation=(0,0,0)):
+    (A,B,C,K) = plane
+    (tx, ty, tz) = translation
+    return -(A*(x-tx) + B*(y-ty) + K) / C + tz
+
+
+# Verify a point is on this plane
+def validate(plane, point):
+  (A, B, C, K) = plane
+  (x, y, z) = point
+  return z == calcz(x, y, plane)
+
+
+def verify_plane(points):
+  print '  ', '\n   '.join([str(p) for p in points])
+
+  plane = plane_from_three_points( *points)
+  print 'Plane coordinates: ', plane
+
+  if plane[2] == 0:
+    print '   Error: points are colinear'
+    return
+
+  valid = True
+  for p in points:
+    if not validate(plane, p):
+      print "Failed: sample point not on plane, ", p
+      valid = False
+  print "Validation:", "Failed" if not valid else "Passed"
+
+
+
+samples = [
+  # canonical example
+  [ (1,-2,0), (4,-2,-2), (4,1,4) ],
+
+  # three colinear points (infinite planes)
+  [ (2,2,2), (4,4,4), (10,10,10) ],
+
+  # Extreme tilt example in mm
+  [ (57,123,-5), (200,0,35), (0,207,2) ],
+
+  # Some more examples in um
+  [ (0, 0, 1300), (200000, 200000, 3500), (0, 150000, -1000) ],
+  [ (20000, 20000, -300), (220000, 120000, -1700), (120000, 220000, -700) ],
+
+  # some example in tenths of mm
+  [ (200, 200, -300), (2200, 1200, -1700), (1200, 2200, -700) ],
+
+  [ (20000,  20000 , -300 ), (220000,  120000 , -1700 ), (120000,  220000 , -700 ) ],
+  [ (200,  200, -300 ), (2200,  1200, -1700 ), (1200,  2200, -700 ) ]
+]
+
+for points in samples:
+  verify_plane(points)
+
+  print "====[Translated]========="
+  # Translate plane to origin at P (simplifies by removing K coefficient)
+  # A*x' + B*y' + C*z' = 0
+  P = points[0]
+  T = translate((0,0,0), P)
+  xpoints = [translate(p, P) for p in points]
+  verify_plane(xpoints)
+  print "=========================\n"
diff --git a/testcases/bed_leveling.gcode b/testcases/bed_leveling.gcode
new file mode 100644
index 000000000..004fd917b
--- /dev/null
+++ b/testcases/bed_leveling.gcode
@@ -0,0 +1,46 @@
+(Test bed leveling)
+m111 s32
+
+G28
+G29 S5
+G29 S1 X20.000 Y20.000  Z-2.000
+G29 S1 X220.000 Y120.000  Z-1.700
+G29 S1 X120.000 Y220.000  Z-0.700
+
+g29 s0
+
+m114
+g1 x100 y100
+m114
+G28
+M114
+
+G4 ; dwell
+G1 x200 y0
+G4 ; dwell
+m114
+G1 x199
+G4 ; dwell
+G1 x198
+G4 ; dwell
+G1 x197
+G4 ; dwell
+G1 x196
+G4 ; dwell
+G1 x195
+G4 ; dwell
+G1 x194
+G4 ; dwell
+g28
+M114
+G1 x1 y1
+G4 ; dwell
+m114
+
+G28
+G29 S5
+G29 S1 X20.000 Y20.000  Z-0.300
+G29 S1 X220.000 Y120.000  Z-1.700
+G29 S1 X120.000 Y220.000  Z-0.700
+
+g29 s0