idex_modes: Native input shaping support with dual carriages

Signed-off-by: Dmitry Butyugin <dmbutyugin@google.com>

3 files changed, 79 insertions, 42 deletions

diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py
index 2eebb59c..fdfdf546 100644
--- a/klippy/extras/input_shaper.py
+++ b/klippy/extras/input_shaper.py
@@ -61,7 +61,6 @@ class AxisInputShaper:
         self.params.update(gcmd)
         old_n, old_A, old_T = self.n, self.A, self.T
         self.n, self.A, self.T = self.params.get_shaper()
-        return (old_n, old_A, old_T) != (self.n, self.A, self.T)
     def set_shaper_kinematics(self, sk):
         ffi_main, ffi_lib = chelper.get_ffi()
         success = ffi_lib.input_shaper_set_shaper_params(
@@ -98,7 +97,7 @@ class InputShaper:
         self.toolhead = None
         self.shapers = [AxisInputShaper('x', config),
                         AxisInputShaper('y', config)]
-        self.stepper_kinematics = []
+        self.input_shaper_stepper_kinematics = []
         self.orig_stepper_kinematics = []
         # Register gcode commands
         gcode = self.printer.lookup_object('gcode')
@@ -109,38 +108,50 @@ class InputShaper:
         return self.shapers
     def connect(self):
         self.toolhead = self.printer.lookup_object("toolhead")
-        kin = self.toolhead.get_kinematics()
-        # Lookup stepper kinematics
-        ffi_main, ffi_lib = chelper.get_ffi()
-        steppers = kin.get_steppers()
-        for s in steppers:
-            sk = ffi_main.gc(ffi_lib.input_shaper_alloc(), ffi_lib.free)
-            orig_sk = s.set_stepper_kinematics(sk)
-            res = ffi_lib.input_shaper_set_sk(sk, orig_sk)
-            if res < 0:
-                s.set_stepper_kinematics(orig_sk)
-                continue
-            self.stepper_kinematics.append(sk)
-            self.orig_stepper_kinematics.append(orig_sk)
         # Configure initial values
         self.old_delay = 0.
         self._update_input_shaping(error=self.printer.config_error)
+    def _get_input_shaper_stepper_kinematics(self, stepper):
+        # Lookup stepper kinematics
+        sk = stepper.get_stepper_kinematics()
+        if sk in self.orig_stepper_kinematics:
+            # Already processed this stepper kinematics unsuccessfully
+            return None
+        if sk in self.input_shaper_stepper_kinematics:
+            return sk
+        self.orig_stepper_kinematics.append(sk)
+        ffi_main, ffi_lib = chelper.get_ffi()
+        is_sk = ffi_main.gc(ffi_lib.input_shaper_alloc(), ffi_lib.free)
+        stepper.set_stepper_kinematics(is_sk)
+        res = ffi_lib.input_shaper_set_sk(is_sk, sk)
+        if res < 0:
+            stepper.set_stepper_kinematics(sk)
+            return None
+        self.input_shaper_stepper_kinematics.append(is_sk)
+        return is_sk
     def _update_input_shaping(self, error=None):
         self.toolhead.flush_step_generation()
         new_delay = max([s.get_step_generation_window() for s in self.shapers])
         self.toolhead.note_step_generation_scan_time(new_delay,
                                                      old_delay=self.old_delay)
-        failed = []
-        for sk in self.stepper_kinematics:
+        self.old_delay = new_delay
+        kin = self.toolhead.get_kinematics()
+        failed_shapers = []
+        for s in kin.get_steppers():
+            if s.get_trapq() is None:
+                continue
+            is_sk = self._get_input_shaper_stepper_kinematics(s)
+            if is_sk is None:
+                continue
             for shaper in self.shapers:
-                if shaper in failed:
+                if shaper in failed_shapers:
                     continue
-                if not shaper.set_shaper_kinematics(sk):
-                    failed.append(shaper)
-        if failed:
+                if not shaper.set_shaper_kinematics(is_sk):
+                    failed_shapers.append(shaper)
+        if failed_shapers:
             error = error or self.printer.command_error
             raise error("Failed to configure shaper(s) %s with given parameters"
-                        % (', '.join([s.get_name() for s in failed])))
+                        % (', '.join([s.get_name() for s in failed_shapers])))
     def disable_shaping(self):
         for shaper in self.shapers:
             shaper.disable_shaping()
@@ -151,10 +162,9 @@ class InputShaper:
         self._update_input_shaping()
     cmd_SET_INPUT_SHAPER_help = "Set cartesian parameters for input shaper"
     def cmd_SET_INPUT_SHAPER(self, gcmd):
-        updated = False
-        for shaper in self.shapers:
-            updated |= shaper.update(gcmd)
-        if updated:
+        if gcmd.get_command_parameters():
+            for shaper in self.shapers:
+                shaper.update(gcmd)
             self._update_input_shaping()
         for shaper in self.shapers:
             shaper.report(gcmd)
diff --git a/klippy/kinematics/idex_modes.py b/klippy/kinematics/idex_modes.py
index 1ca79c86..73fac776 100644
--- a/klippy/kinematics/idex_modes.py
+++ b/klippy/kinematics/idex_modes.py
@@ -4,6 +4,7 @@
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import math
+import chelper
 
 class DualCarriages:
     def __init__(self, printer, rail_0, rail_1, axis):
@@ -24,7 +25,8 @@ class DualCarriages:
         for i, dc in enumerate(self.dc):
             dc_rail = dc.get_rail()
             if i != index:
-                dc.inactivate(pos)
+                if dc.is_active():
+                    dc.inactivate(pos)
                 kin.override_rail(3, dc_rail)
             elif dc.is_active() is False:
                 newpos = pos[:self.axis] + [dc.axis_position] \
@@ -79,27 +81,50 @@ class DualCarriagesRail:
         self.stepper_alloc_active = stepper_alloc_active
         self.stepper_alloc_inactive = stepper_alloc_inactive
         self.axis_position = -1
-    def _stepper_alloc(self, position, active=True):
+        self.stepper_active_sk = {}
+        self.stepper_inactive_sk = {}
+        for s in rail.get_steppers():
+            self._save_sk(self.status, s, s.get_stepper_kinematics())
+    def _alloc_sk(self, alloc_func, *params):
+        ffi_main, ffi_lib = chelper.get_ffi()
+        return ffi_main.gc(getattr(ffi_lib, alloc_func)(*params), ffi_lib.free)
+    def _get_sk(self, status, stepper):
+        sk = None
+        if status == self.ACTIVE:
+            sk = self.stepper_active_sk.get(stepper, None)
+            if sk is None and self.stepper_alloc_active:
+                sk = self._alloc_sk(*self.stepper_alloc_active)
+                self._save_sk(status, stepper, sk)
+        elif status == self.INACTIVE:
+            sk = self.stepper_inactive_sk.get(stepper, None)
+            if sk is None and self.stepper_alloc_inactive:
+                sk = self._alloc_sk(*self.stepper_alloc_inactive)
+                self._save_sk(status, stepper, sk)
+        return sk
+    def _save_sk(self, status, stepper, sk):
+        if status == self.ACTIVE:
+            self.stepper_active_sk[stepper] = sk
+        elif status == self.INACTIVE:
+            self.stepper_inactive_sk[stepper] = sk
+    def _update_stepper_alloc(self, position, active=True):
         toolhead = self.printer.lookup_object('toolhead')
         self.axis_position = position[self.axis]
         self.rail.set_trapq(None)
-        if active is True:
-            self.status = self.ACTIVE
-            if self.stepper_alloc_active is not None:
-                self.rail.setup_itersolve(*self.stepper_alloc_active)
-                self.rail.set_position(position)
-                self.rail.set_trapq(toolhead.get_trapq())
-        else:
-            self.status = self.INACTIVE
-            if self.stepper_alloc_inactive is not None:
-                self.rail.setup_itersolve(*self.stepper_alloc_inactive)
-                self.rail.set_position(position)
-                self.rail.set_trapq(toolhead.get_trapq())
+        old_status = self.status
+        self.status = (self.INACTIVE, self.ACTIVE)[active]
+        for s in self.rail.get_steppers():
+            sk = self._get_sk(self.status, s)
+            if sk is None:
+                return
+            old_sk = s.set_stepper_kinematics(sk)
+            self._save_sk(old_status, s, old_sk)
+        self.rail.set_position(position)
+        self.rail.set_trapq(toolhead.get_trapq())
     def get_rail(self):
         return self.rail
     def is_active(self):
         return self.status == self.ACTIVE
     def activate(self, position):
-        self._stepper_alloc(position, active=True)
+        self._update_stepper_alloc(position, active=True)
     def inactivate(self, position):
-        self._stepper_alloc(position, active=False)
+        self._update_stepper_alloc(position, active=False)
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 0f8c0275..56c8ec75 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -160,6 +160,8 @@ class MCU_stepper:
         count = ffi_lib.stepcompress_extract_old(self._stepqueue, data, count,
                                                  start_clock, end_clock)
         return (data, count)
+    def get_stepper_kinematics(self):
+        return self._stepper_kinematics
     def set_stepper_kinematics(self, sk):
         old_sk = self._stepper_kinematics
         mcu_pos = 0