bed_mesh: introduce "faulty_regions" option

Users may define "faulty regions", locations within the mesh where a probed value is unreliable. When bed mesh generates points it will substitute points in faulty regions with up to 4 points nearest to the region. After calibration is complete the Z values at these points will be averaged and assigned to the original value inside the faulty region.

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

1 files changed, 167 insertions, 5 deletions

diff --git a/klippy/extras/bed_mesh.py b/klippy/extras/bed_mesh.py
index 1c25a1ea..11a8a748 100644
--- a/klippy/extras/bed_mesh.py
+++ b/klippy/extras/bed_mesh.py
@@ -21,6 +21,12 @@ class BedMeshError(Exception):
 def isclose(a, b, rel_tol=1e-09, abs_tol=0.0):
     return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol)
 
+# return true if a coordinate is within the region
+# specified by min_c and max_c
+def within(coord, min_c, max_c, tol=0.0):
+    return (max_c[0] + tol) >= coord[0] >= (min_c[0] - tol) and \
+        (max_c[1] + tol) >= coord[1] >= (min_c[1] - tol)
+
 # Constrain value between min and max
 def constrain(val, min_val, max_val):
     return min(max_val, max(min_val, val))
@@ -240,6 +246,8 @@ class BedMeshCalibrate:
         self.mesh_min = self.mesh_max = (0., 0.)
         self.relative_reference_index = config.getint(
             'relative_reference_index', None)
+        self.faulty_regions = []
+        self.substituted_indices = collections.OrderedDict()
         self.orig_config['rri'] = self.relative_reference_index
         self.bedmesh = bedmesh
         self.mesh_config = collections.OrderedDict()
@@ -247,7 +255,7 @@ class BedMeshCalibrate:
         self._generate_points(config.error)
         self.orig_points = self.points
         self.probe_helper = probe.ProbePointsHelper(
-            config, self.probe_finalize, self.points)
+            config, self.probe_finalize, self._get_adjusted_points())
         self.probe_helper.minimum_points(3)
         self.probe_helper.use_xy_offsets(True)
         self.gcode = self.printer.lookup_object('gcode')
@@ -297,6 +305,45 @@ class BedMeshCalibrate:
                             (self.origin[0] + pos_x, self.origin[1] + pos_y))
             pos_y += y_dist
         self.points = points
+        if not self.faulty_regions:
+            return
+        # Check to see if any points fall within faulty regions
+        last_y = self.points[0][1]
+        is_reversed = False
+        for i, coord in enumerate(self.points):
+            if not isclose(coord[1], last_y):
+                is_reversed = not is_reversed
+            last_y = coord[1]
+            adj_coords = []
+            for min_c, max_c in self.faulty_regions:
+                if within(coord, min_c, max_c, tol=.00001):
+                    # Point lies within a faulty region
+                    adj_coords = [
+                        (min_c[0], coord[1]), (coord[0], min_c[1]),
+                        (coord[0], max_c[1]), (max_c[0], coord[1])]
+                    if is_reversed:
+                        # Swap first and last points for zig-zag pattern
+                        first = adj_coords[0]
+                        adj_coords[0] = adj_coords[-1]
+                        adj_coords[-1] = first
+                    break
+            if not adj_coords:
+                # coord is not located within a faulty region
+                continue
+            valid_coords = []
+            for ac in adj_coords:
+                # make sure that coordinates are within the mesh boundary
+                if self.radius is None:
+                    if within(ac, (min_x, min_y), (max_x, max_y), .000001):
+                        valid_coords.append(ac)
+                else:
+                    dist_from_origin = math.sqrt(ac[0]*ac[0] + ac[1]*ac[1])
+                    if dist_from_origin <= self.radius:
+                        valid_coords.append(ac)
+            if not valid_coords:
+                raise error("bed_mesh: Unable to generate coordinates"
+                            " for faulty region at index: %d" % (i))
+            self.substituted_indices[i] = valid_coords
     def print_generated_points(self, print_func):
         x_offset = y_offset = 0.
         probe = self.printer.lookup_object('probe', None)
@@ -314,6 +361,12 @@ class BedMeshCalibrate:
             print_func(
                 "bed_mesh: relative_reference_index %d is (%.2f, %.2f)"
                 % (rri, self.points[rri][0], self.points[rri][1]))
+        if self.substituted_indices:
+            print_func("bed_mesh: faulty region points")
+            for i, v in self.substituted_indices.items():
+                pt = self.points[i]
+                print_func("%d (%.2f, %.2f), substituted points: %s"
+                           % (i, pt[0], pt[1], repr(v)))
     def _init_mesh_config(self, config):
         mesh_cfg = self.mesh_config
         orig_cfg = self.orig_config
@@ -352,6 +405,41 @@ class BedMeshCalibrate:
             config.get('algorithm', 'lagrange').strip().lower()
         orig_cfg['tension'] = mesh_cfg['tension'] = config.getfloat(
             'bicubic_tension', .2, minval=0., maxval=2.)
+        for i in list(range(1, 100, 1)):
+            min_opt = "faulty_region_%d_min" % (i,)
+            max_opt = "faulty_region_%d_max" % (i,)
+            if config.get(min_opt, None) is None:
+                break
+            start = parse_pair(config, (min_opt,))
+            end = parse_pair(config, (max_opt,))
+            # Validate the corners.  If necessary reorganize them.
+            # c1 = min point, c3 = max point
+            #  c4 ---- c3
+            #  |        |
+            #  c1 ---- c2
+            c1 = [min([s, e]) for s, e in zip(start, end)]
+            c3 = [max([s, e]) for s, e in zip(start, end)]
+            c2 = [c1[0], c3[1]]
+            c4 = [c3[0], c1[1]]
+            # Check for overlapping regions
+            for j, (prev_c1, prev_c3) in enumerate(self.faulty_regions):
+                prev_c2 = [prev_c1[0], prev_c3[1]]
+                prev_c4 = [prev_c3[0], prev_c1[1]]
+                # Validate that no existing corner is within the new region
+                for coord in [prev_c1, prev_c2, prev_c3, prev_c4]:
+                    if within(coord, c1, c3):
+                        raise config.error(
+                            "bed_mesh: Existing faulty_region_%d %s overlaps "
+                            "added faulty_region_%d %s"
+                            % (j, repr([prev_c1, prev_c3]), i, repr([c1, c3])))
+                # Validate that no new corner is within an existing region
+                for coord in [c1, c2, c3, c4]:
+                    if within(coord, prev_c1, prev_c3):
+                        raise config.error(
+                            "bed_mesh: Added faulty_region_%d %s overlaps "
+                            "existing faulty_region_%d %s"
+                            % (i, repr([c1, c3]), j, repr([prev_c1, prev_c3])))
+            self.faulty_regions.append((c1, c3))
         self._verify_algorithm(config.error)
     def _verify_algorithm(self, error):
         params = self.mesh_config
@@ -445,7 +533,8 @@ class BedMeshCalibrate:
             self._generate_points(gcmd.error)
             gcmd.respond_info("Generating new points...")
             self.print_generated_points(gcmd.respond_info)
-            self.probe_helper.update_probe_points(self.points, 3)
+            pts = self._get_adjusted_points()
+            self.probe_helper.update_probe_points(pts, 3)
             msg = "relative_reference_index: %s\n" % \
                 (self.relative_reference_index)
             msg += "\n".join(["%s: %s" % (k, v) for k, v
@@ -453,8 +542,21 @@ class BedMeshCalibrate:
             logging.info("Updated Mesh Configuration:\n" + msg)
         else:
             self.points = self.orig_points
-            self.probe_helper.update_probe_points(self.points, 3)
-
+            pts = self._get_adjusted_points()
+            self.probe_helper.update_probe_points(pts, 3)
+    def _get_adjusted_points(self):
+        if not self.substituted_indices:
+            return self.points
+        adj_pts = []
+        last_index = 0
+        for i, pts in self.substituted_indices.items():
+            adj_pts.extend(self.points[last_index:i])
+            adj_pts.extend(pts)
+            # Add one to the last index to skip the point
+            # we are replacing
+            last_index = i + 1
+        adj_pts.extend(self.points[last_index:])
+        return adj_pts
     cmd_BED_MESH_CALIBRATE_help = "Perform Mesh Bed Leveling"
     def cmd_BED_MESH_CALIBRATE(self, gcmd):
         self.bedmesh.set_mesh(None)
@@ -462,7 +564,7 @@ class BedMeshCalibrate:
         self.probe_helper.start_probe(gcmd)
     def probe_finalize(self, offsets, positions):
         x_offset, y_offset, z_offset = offsets
-        positions = [(round(p[0], 2), round(p[1], 2), p[2])
+        positions = [[round(p[0], 2), round(p[1], 2), p[2]]
                      for p in positions]
         params = dict(self.mesh_config)
         params['min_x'] = min(positions, key=lambda p: p[0])[0] + x_offset
@@ -472,6 +574,48 @@ class BedMeshCalibrate:
         x_cnt = params['x_count']
         y_cnt = params['y_count']
 
+        if self.substituted_indices:
+            # Replace substituted points with the original generated
+            # point.  Its Z Value is the average probed Z of the
+            # substituted points.
+            corrected_pts = []
+            idx_offset = 0
+            start_idx = 0
+            for i, pts in self.substituted_indices.items():
+                fpt = [p - o for p, o in zip(self.points[i], offsets[:2])]
+                # offset the index to account for additional samples
+                idx = i + idx_offset
+                # Add "normal" points
+                corrected_pts.extend(positions[start_idx:idx])
+                avg_z = sum([p[2] for p in positions[idx:idx+len(pts)]]) \
+                    / len(pts)
+                idx_offset += len(pts) - 1
+                start_idx = idx + len(pts)
+                fpt.append(avg_z)
+                logging.info(
+                    "bed_mesh: Replacing value at faulty index %d"
+                    " (%.4f, %.4f): avg value = %.6f, avg w/ z_offset = %.6f"
+                    % (i, fpt[0], fpt[1], avg_z, avg_z - z_offset))
+                corrected_pts.append(fpt)
+            corrected_pts.extend(positions[start_idx:])
+            # validate corrected positions
+            if len(self.points) != len(corrected_pts):
+                self._dump_points(positions, corrected_pts, offsets)
+                raise self.gcode.error(
+                    "bed_mesh: invalid position list size, "
+                    "generated count: %d, probed count: %d"
+                    % (len(self.points), len(corrected_pts)))
+            for gen_pt, probed in zip(self.points, corrected_pts):
+                off_pt = [p - o for p, o in zip(gen_pt, offsets[:2])]
+                if not isclose(off_pt[0], probed[0], abs_tol=.1) or \
+                        not isclose(off_pt[1], probed[1], abs_tol=.1):
+                    self._dump_points(positions, corrected_pts, offsets)
+                    raise self.gcode.error(
+                        "bed_mesh: point mismatch, orig = (%.2f, %.2f)"
+                        ", probed = (%.2f, %.2f)"
+                        % (off_pt[0], off_pt[1], probed[0], probed[1]))
+            positions = corrected_pts
+
         if self.relative_reference_index is not None:
             # zero out probe z offset and
             # set offset relative to reference index
@@ -536,6 +680,24 @@ class BedMeshCalibrate:
         self.bedmesh.set_mesh(z_mesh)
         self.gcode.respond_info("Mesh Bed Leveling Complete")
         self.bedmesh.save_profile("default")
+    def _dump_points(self, probed_pts, corrected_pts, offsets):
+        # logs generated points with offset applied, points received
+        # from the finalize callback, and the list of corrected points
+        max_len = max([len(self.points), len(probed_pts), len(corrected_pts)])
+        logging.info(
+            "bed_mesh: calibration point dump\nIndex | %-17s| %-25s|"
+            " Corrected Point" % ("Generated Point", "Probed Point"))
+        for i in list(range(max_len)):
+            gen_pt = probed_pt = corr_pt = ""
+            if i < len(self.points):
+                off_pt = [p - o for p, o in zip(self.points[i], offsets[:2])]
+                gen_pt = "(%.2f, %.2f)" % tuple(off_pt)
+            if i < len(probed_pts):
+                probed_pt = "(%.2f, %.2f, %.4f)" % tuple(probed_pts[i])
+            if i < len(corrected_pts):
+                corr_pt = "(%.2f, %.2f, %.4f)" % tuple(corrected_pts[i])
+            logging.info(
+                "  %-4d| %-17s| %-25s| %s" % (i, gen_pt, probed_pt, corr_pt))
 
 
 class MoveSplitter: