Add dynamic 3-point bed-leveling support

Adds G29 commands to register bed level points.  When three points
are registered, the plane of the bed is calculated and dynamic bed
leveling takes effect.

Add a warning if bed-leveling is enabled when MAX_JERK_Z is zero.
In this case lookahead will always fail when bed-leveling is active
since the Z-axis is not allowed to move during lookahead.

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
+//   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 */

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 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

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
+
 
 /***************************************************************************\
 *                                                                           *

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
+
 
 /***************************************************************************\
 *                                                                           *

config/printer.pcbscriber.h

@@ -187,6 +187,14 @@ DEFINE_HOMING(z_negative, x_negative, y_negative, none)
 #define MAX_JERK_Z               0
 #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
+
 
 /***************************************************************************\
 *                                                                           *

config/printer.tronxy.h

@@ -187,6 +187,14 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
 #define MAX_JERK_Z               0
 #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
+
 
 /***************************************************************************\
 *                                                                           *

config/printer.wolfstrap.h

@@ -187,6 +187,14 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
 #define MAX_JERK_Z               0
 #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
+
 
 /***************************************************************************\
 *                                                                           *

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.
 
@@ -130,9 +139,9 @@
  * With GCC major >= 5 we can calculate constant sqrt expressions
  * lower versions, e.g. in the Arduino IDE, need for this a
  * __builtin_ function.
- * 
+ *
  * "warning: initializer element is not a constant expression"
- * 
+ *
  * Someone may extend this later also for sin/cos.
 */
 #if (__GNUC__ <= 4)

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)

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
+
 
 /***************************************************************************\
 *                                                                           *