skew_correction: initial implementation

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

1 files changed, 82 insertions, 0 deletions

diff --git a/klippy/extras/skew_correction.py b/klippy/extras/skew_correction.py
new file mode 100644
index 00000000..7d3d09d8
--- /dev/null
+++ b/klippy/extras/skew_correction.py
@@ -0,0 +1,82 @@
+# Printer Skew Correction
+#
+# This implementation is a port of Marlin's skew correction as
+# implemented in planner.h, Copyright (C) Marlin Firmware
+#
+# https://github.com/MarlinFirmware/Marlin/tree/1.1.x/Marlin
+#
+# Copyright (C) 2019  Eric Callahan <arksine.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+import math
+
+def calc_skew_factor(ac, bd, ad):
+    side = math.sqrt(2*ac*ac + 2*bd*bd - 4*ad*ad) / 2.
+    return math.tan(math.pi/2 - math.acos(
+        (ac*ac - side*side - ad*ad) / (2*side*ad)))
+
+def get_skew_from_plane(plane, config):
+    ac = config.getfloat(plane + "_ac_length", None, above=0.)
+    if ac is None:
+        return 0.
+    bd = config.getfloat(plane + "_bd_length", above=0.)
+    ad = config.getfloat(plane + "_ad_length", above=0.)
+    return calc_skew_factor(ac, bd, ad)
+
+class PrinterSkew:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.gcode = self.printer.lookup_object('gcode')
+        self.toolhead = None
+        self.xy_factor = get_skew_from_plane("xy", config)
+        self.xz_factor = get_skew_from_plane("xz", config)
+        self.yz_factor = get_skew_from_plane("yz", config)
+        self.printer.register_event_handler("klippy:ready",
+                                            self._handle_ready)
+        self.next_transform = None
+        self.gcode.register_command(
+            'GET_CURRENT_SKEW', self.cmd_GET_CURRENT_SKEW,
+            desc=self.cmd_GET_CURRENT_SKEW_help)
+        self.gcode.register_command(
+            'CALC_MEASURED_SKEW', self.cmd_CALC_MEASURED_SKEW,
+            desc=self.cmd_CALC_MEASURED_SKEW_help)
+    def _handle_ready(self):
+        self.next_transform = self.gcode.set_move_transform(self, force=True)
+    def calc_skew(self, pos):
+        skewed_x = pos[0] - pos[1] * self.xy_factor \
+            - pos[2] * (self.xz_factor - (self.xy_factor * self.yz_factor))
+        skewed_y = pos[1] - pos[2] * self.yz_factor
+        return [skewed_x, skewed_y, pos[2], pos[3]]
+    def calc_unskew(self, pos):
+        skewed_x = pos[0] + pos[1] * self.xy_factor \
+            + pos[2] * self.xz_factor
+        skewed_y = pos[1] + pos[2] * self.yz_factor
+        return [skewed_x, skewed_y, pos[2], pos[3]]
+    def get_position(self):
+        return self.calc_unskew(self.next_transform.get_position())
+    def move(self, newpos, speed):
+        corrected_pos = self.calc_skew(newpos)
+        self.next_transform.move(corrected_pos, speed)
+    cmd_GET_CURRENT_SKEW_help = "Report current printer skew"
+    def cmd_GET_CURRENT_SKEW(self, params):
+        out = "Current Printer Skew:"
+        planes = ["XY", "XZ", "YZ"]
+        factors = [self.xy_factor, self.xz_factor, self.yz_factor]
+        for plane, fac in zip(planes, factors):
+            out += '\n' + plane
+            out += " Skew: %.6f radians, %.2f degrees" % (
+                fac, math.degrees(fac))
+        self.gcode.respond_info(out)
+    cmd_CALC_MEASURED_SKEW_help = "Calculate skew from measured print"
+    def cmd_CALC_MEASURED_SKEW(self, params):
+        ac = self.gcode.get_float("AC", params, above=0.)
+        bd = self.gcode.get_float("BD", params, above=0.)
+        ad = self.gcode.get_float("AD", params, above=0.)
+        factor = calc_skew_factor(ac, bd, ad)
+        self.gcode.respond_info(
+            "Calculated Skew: %.6f radians, %.2f degrees" %
+            (factor, math.degrees(factor)))
+
+def load_config(config):
+    return PrinterSkew(config)