bed_mesh: Introduce fade_target option

To deal potential z scaling when fade is enabled, a fade_target option has been introduced.  This option may either be set to 0.0 or any z position within the range of the mesh.  A value of 0.0 will result in previous behavior, where z adjustment phases out until no further adjustment is added.  A non-zero value will phase out adjustment until the target has been reached, after which the rest of the print will be offset along the z axis by the fade_target.  By default the fade_target will be calculated as an average of the mesh.

Signed-off-by: Eric Callahan <arksine.code@gmail.com>

1 files changed, 76 insertions, 39 deletions

diff --git a/klippy/extras/bed_mesh.py b/klippy/extras/bed_mesh.py
index cfb5fc62..1625bbcc 100644
--- a/klippy/extras/bed_mesh.py
+++ b/klippy/extras/bed_mesh.py
@@ -63,6 +63,8 @@ class BedMesh:
         self.fade_dist = self.fade_end - self.fade_start
         if self.fade_dist <= 0.:
             self.fade_start = self.fade_end = self.FADE_DISABLE
+        self.base_fade_target = config.getfloat('fade_target', None)
+        self.fade_target = 0.
         self.gcode = self.printer.lookup_object('gcode')
         self.splitter = MoveSplitter(config, self.gcode)
         self.gcode.register_command(
@@ -77,12 +79,28 @@ class BedMesh:
             self.toolhead = self.printer.lookup_object('toolhead')
             self.calibrate.load_default_profile()
     def set_mesh(self, mesh):
-        # Assign the current mesh.  If set to None, no transform
-        # is applied
+        if mesh is not None:
+            if self.base_fade_target is None:
+                self.fade_target = mesh.avg_z
+            else:
+                self.fade_target = self.base_fade_target
+                mesh_min, mesh_max = mesh.get_z_range()
+                if (not mesh_min <= self.fade_target <= mesh_max and
+                        self.fade_target != 0.):
+                    # fade target is non-zero, out of mesh range
+                    err_target = self.fade_target
+                    self.z_mesh = None
+                    self.fade_target = 0.
+                    raise self.gcode.error(
+                        "bed_mesh: ERROR, fade_target lies outside of mesh z "
+                        "range\nmin: %.4f, max: %.4f, fade_target: %.4f"
+                        % (mesh_min, mesh_max, err_target))
+        else:
+            self.fade_target = 0.
         self.z_mesh = mesh
-        self.splitter.set_mesh(mesh)
+        self.splitter.initialize(mesh, self.fade_target)
         # cache the current position before a transform takes place
-        self.last_position[:] = self.toolhead.get_position()
+        self.gcode.reset_last_position()
     def get_z_factor(self, z_pos):
         if z_pos >= self.fade_end:
             return 0.
@@ -95,17 +113,21 @@ class BedMesh:
         if self.z_mesh is None:
             # No mesh calibrated, so send toolhead position
             self.last_position[:] = self.toolhead.get_position()
+            self.last_position[2] -= self.fade_target
         else:
             # return current position minus the current z-adjustment
             x, y, z, e = self.toolhead.get_position()
-            z_adjust = self.get_z_factor(z) * self.z_mesh.get_z(x, y)
-            self.last_position[:] = [x, y, z - z_adjust, e]
+            z_adj = self.z_mesh.calc_z(x, y)
+            z_adj = (self.get_z_factor(z) * (z_adj - self.fade_target) +
+                     self.fade_target)
+            self.last_position[:] = [x, y, z - z_adj, e]
         return list(self.last_position)
     def move(self, newpos, speed):
         factor = self.get_z_factor(newpos[2])
         if self.z_mesh is None or not factor:
             # No mesh calibrated, or mesh leveling phased out.
-            self.toolhead.move(newpos, speed)
+            x, y, z, e = newpos
+            self.toolhead.move([x, y, z + self.fade_target, e], speed)
         else:
             self.splitter.build_move(self.last_position, newpos, factor)
             while not self.splitter.traverse_complete:
@@ -377,21 +399,23 @@ class MoveSplitter:
             'move_check_distance', 5., minval=3.)
         self.z_mesh = None
         self.gcode = gcode
-    def set_mesh(self, mesh):
+    def initialize(self, mesh, fade_offset):
         self.z_mesh = mesh
+        self.fade_offset = fade_offset
     def build_move(self, prev_pos, next_pos, factor):
         self.prev_pos = tuple(prev_pos)
         self.next_pos = tuple(next_pos)
         self.current_pos = list(prev_pos)
         self.z_factor = factor
-        self.z_offset =  \
-            self.z_factor \
-            * self.z_mesh.get_z(self.prev_pos[0], self.prev_pos[1])
+        self.z_offset = self._calc_z_offset(prev_pos)
         self.traverse_complete = False
         self.distance_checked = 0.
         axes_d = [self.next_pos[i] - self.prev_pos[i] for i in range(4)]
         self.total_move_length = math.sqrt(sum([d*d for d in axes_d[:3]]))
         self.axis_move = [not isclose(d, 0., abs_tol=1e-10) for d in axes_d]
+    def _calc_z_offset(self, pos):
+        z = self.z_mesh.calc_z(pos[0], pos[1])
+        return self.z_factor * (z - self.fade_offset) + self.fade_offset
     def _set_next_move(self, distance_from_prev):
         t = distance_from_prev / self.total_move_length
         if t > 1. or t < 0.:
@@ -410,10 +434,7 @@ class MoveSplitter:
                         < self.total_move_length:
                     self.distance_checked += self.move_check_distance
                     self._set_next_move(self.distance_checked)
-                    next_z = \
-                        self.z_factor \
-                        * self.z_mesh.get_z(
-                            self.current_pos[0], self.current_pos[1])
+                    next_z = self._calc_z_offset(self.current_pos)
                     if abs(next_z - self.z_offset) >= self.split_delta_z:
                         self.z_offset = next_z
                         return self.current_pos[0], self.current_pos[1], \
@@ -421,9 +442,7 @@ class MoveSplitter:
                             self.current_pos[3]
             # end of move reached
             self.current_pos[:] = self.next_pos
-            self.z_offset = \
-                self.z_factor \
-                * self.z_mesh.get_z(self.current_pos[0], self.current_pos[1])
+            self.z_offset = self._calc_z_offset(self.current_pos)
             # Its okay to add Z-Offset to the final move, since it will not be
             # used again.
             self.current_pos[2] += self.z_offset
@@ -438,6 +457,7 @@ class ZMesh:
     def __init__(self, params):
         self.mesh_z_table = None
         self.probe_params = params
+        self.avg_z = 0.
         logging.debug('bed_mesh: probe/mesh parameters:')
         for key, value in self.probe_params.iteritems():
             logging.debug("%s :  %s" % (key, value))
@@ -450,9 +470,9 @@ class ZMesh:
             % (self.mesh_x_min, self.mesh_y_min,
                self.mesh_x_max, self.mesh_y_max))
         if params['algo'] == 'bicubic':
-            self.build_mesh = self._sample_bicubic
+            self._sample = self._sample_bicubic
         else:
-            self.build_mesh = self._sample_lagrange
+            self._sample = self._sample_lagrange
         # Nummber of points to interpolate per segment
         mesh_x_pps = params['mesh_x_pps']
         mesh_y_pps = params['mesh_y_pps']
@@ -463,11 +483,11 @@ class ZMesh:
         if px_cnt == 3 or py_cnt == 3:
             # a mesh with 3 points on either axis defaults to legrange
             # upsampling
-            self.build_mesh = self._sample_lagrange
+            self._sample = self._sample_lagrange
             self.probe_params['algo'] = 'lagrange'
         if mesh_x_mult == 1 and mesh_y_mult == 1:
             # No interpolation, sample the probed points directly
-            self.build_mesh = self._sample_direct
+            self._sample = self._sample_direct
             self.probe_params['algo'] = 'direct'
         self.mesh_x_count = px_cnt * mesh_x_mult - (mesh_x_mult - 1)
         self.mesh_y_count = py_cnt * mesh_y_mult - (mesh_y_mult - 1)
@@ -479,26 +499,14 @@ class ZMesh:
                            (self.mesh_x_count - 1)
         self.mesh_y_dist = (self.mesh_y_max - self.mesh_y_min) / \
                            (self.mesh_y_count - 1)
-    def get_x_coordinate(self, index):
-        return self.mesh_x_min + self.mesh_x_dist * index
-    def get_y_coordinate(self, index):
-        return self.mesh_y_min + self.mesh_y_dist * index
-    def get_z(self, x, y):
-        if self.mesh_z_table:
-            tbl = self.mesh_z_table
-            tx, xidx = self._get_linear_index(x, 0)
-            ty, yidx = self._get_linear_index(y, 1)
-            z0 = lerp(tx, tbl[yidx][xidx], tbl[yidx][xidx+1])
-            z1 = lerp(tx, tbl[yidx+1][xidx], tbl[yidx+1][xidx+1])
-            return lerp(ty, z0, z1)
-        else:
-            # No mesh table generated, no z-adjustment
-            return 0.
     def print_mesh(self, print_func, move_z=None):
         if self.mesh_z_table is not None:
             msg = "Mesh X,Y: %d,%d\n" % (self.mesh_x_count, self.mesh_y_count)
             if move_z is not None:
                 msg += "Search Height: %d\n" % (move_z)
+            msg += "Mesh Average: %.2f\n" % (self.avg_z)
+            rng = self.get_z_range()
+            msg += "Mesh Range: min=%.4f max=%.4f\n" % (rng[0], rng[1])
             msg += "Interpolation Algorithm: %s\n" \
                    % (self.probe_params['algo'])
             msg += "Measured points:\n"
@@ -509,6 +517,37 @@ class ZMesh:
             print_func(msg)
         else:
             print_func("bed_mesh: Z Mesh not generated")
+    def build_mesh(self, z_table):
+        self._sample(z_table)
+        self.avg_z = (sum([sum(x) for x in self.mesh_z_table]) /
+                      sum([len(x) for x in self.mesh_z_table]))
+        # Round average to the nearest 100th.  This
+        # should produce an offset that is divisible by common
+        # z step distances
+        self.avg_z = round(self.avg_z, 2)
+        self.print_mesh(logging.debug)
+    def get_x_coordinate(self, index):
+        return self.mesh_x_min + self.mesh_x_dist * index
+    def get_y_coordinate(self, index):
+        return self.mesh_y_min + self.mesh_y_dist * index
+    def calc_z(self, x, y):
+        if self.mesh_z_table is not None:
+            tbl = self.mesh_z_table
+            tx, xidx = self._get_linear_index(x, 0)
+            ty, yidx = self._get_linear_index(y, 1)
+            z0 = lerp(tx, tbl[yidx][xidx], tbl[yidx][xidx+1])
+            z1 = lerp(tx, tbl[yidx+1][xidx], tbl[yidx+1][xidx+1])
+            return lerp(ty, z0, z1)
+        else:
+            # No mesh table generated, no z-adjustment
+            return 0.
+    def get_z_range(self):
+        if self.mesh_z_table is not None:
+            mesh_min = min([min(x) for x in self.mesh_z_table])
+            mesh_max = max([max(x) for x in self.mesh_z_table])
+            return mesh_min, mesh_max
+        else:
+            return 0., 0.
     def _get_linear_index(self, coord, axis):
         if axis == 0:
             # X-axis
@@ -555,7 +594,6 @@ class ZMesh:
                     continue
                 y = self.get_y_coordinate(j)
                 self.mesh_z_table[j][i] = self._calc_lagrange(ypts, y, i, 1)
-        self.print_mesh(logging.debug)
     def _get_lagrange_coords(self, z_table):
         xpts = []
         ypts = []
@@ -609,7 +647,6 @@ class ZMesh:
                     continue
                 pts = self._get_y_ctl_pts(x, y)
                 self.mesh_z_table[y][x] = self._cardinal_spline(pts, c)
-        self.print_mesh(logging.debug)
     def _get_x_ctl_pts(self, x, y):
         # Fetch control points and t for a X value in the mesh
         x_mult = self.x_mult