input_shaper: Track kinematics updates by dual_carriage

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

docs/Config_Reference.md

@@ -2482,6 +2482,27 @@ carriages: u-y
 ...
 ```
 
+`[dual_carriage]` requires special configuration for the input shaper.
+In general, it is necessary to run input shaper calibration twice -
+for the `dual_carriage` and its `primary_carriage` for the axis they
+share. Then the input shaper can be configured as follows, assuming the
+example above:
+```
+[input_shaper]
+# Intentionally empty
+
+[delayed_gcode init_shaper]
+initial_duration: 0.1
+gcode:
+  SET_DUAL_CARRIAGE CARRIAGE=u
+  SET_INPUT_SHAPER SHAPER_TYPE_X=<dual_carriage_x_shaper> SHAPER_FREQ_X=<dual_carriage_x_freq> SHAPER_TYPE_Y=<y_shaper> SHAPER_FREQ_Y=<y_freq>
+  SET_DUAL_CARRIAGE CARRIAGE=x
+  SET_INPUT_SHAPER SHAPER_TYPE_X=<primary_carriage_x_shaper> SHAPER_FREQ_X=<primary_carriage_x_freq> SHAPER_TYPE_Y=<y_shaper> SHAPER_FREQ_Y=<y_freq>
+```
+Note that `SHAPER_TYPE_Y` and `SHAPER_FREQ_Y` must be the same in both
+commands in this case, since the same motors drive Y axis when either
+of the `x` and `u` carriages are active.
+
 It is worth noting that `generic_cartesian` kinematic can support two
 dual carriages for X and Y axes. For reference, see for instance a
 [sample](../config/sample-corexyuv.cfg) of CoreXYUV configuration.

klippy/chelper/__init__.py

@@ -160,6 +160,7 @@ defs_kin_shaper = """
         , int n, double a[], double t[]);
     int input_shaper_set_sk(struct stepper_kinematics *sk
         , struct stepper_kinematics *orig_sk);
+    void input_shaper_update_sk(struct stepper_kinematics *sk);
     struct stepper_kinematics * input_shaper_alloc(void);
 """
 

klippy/chelper/kin_shaper.c

@@ -166,6 +166,38 @@ shaper_commanded_pos_post_fixup(struct stepper_kinematics *sk)
     sk->commanded_pos = is->orig_sk->commanded_pos;
 }
 
+static void
+shaper_note_generation_time(struct input_shaper *is)
+{
+    double pre_active = 0., post_active = 0.;
+    if ((is->sk.active_flags & AF_X) && is->sx.num_pulses) {
+        pre_active = is->sx.pulses[is->sx.num_pulses-1].t;
+        post_active = -is->sx.pulses[0].t;
+    }
+    if ((is->sk.active_flags & AF_Y) && is->sy.num_pulses) {
+        pre_active = is->sy.pulses[is->sy.num_pulses-1].t > pre_active
+            ? is->sy.pulses[is->sy.num_pulses-1].t : pre_active;
+        post_active = -is->sy.pulses[0].t > post_active
+            ? -is->sy.pulses[0].t : post_active;
+    }
+    is->sk.gen_steps_pre_active = pre_active;
+    is->sk.gen_steps_post_active = post_active;
+}
+
+void __visible
+input_shaper_update_sk(struct stepper_kinematics *sk)
+{
+    struct input_shaper *is = container_of(sk, struct input_shaper, sk);
+    if ((is->orig_sk->active_flags & (AF_X | AF_Y)) == (AF_X | AF_Y))
+        is->sk.calc_position_cb = shaper_xy_calc_position;
+    else if (is->orig_sk->active_flags & AF_X)
+        is->sk.calc_position_cb = shaper_x_calc_position;
+    else if (is->orig_sk->active_flags & AF_Y)
+        is->sk.calc_position_cb = shaper_y_calc_position;
+    is->sk.active_flags = is->orig_sk->active_flags;
+    shaper_note_generation_time(is);
+}
+
 int __visible
 input_shaper_set_sk(struct stepper_kinematics *sk
                     , struct stepper_kinematics *orig_sk)
@@ -190,24 +222,6 @@ input_shaper_set_sk(struct stepper_kinematics *sk
     return 0;
 }
 
-static void
-shaper_note_generation_time(struct input_shaper *is)
-{
-    double pre_active = 0., post_active = 0.;
-    if ((is->sk.active_flags & AF_X) && is->sx.num_pulses) {
-        pre_active = is->sx.pulses[is->sx.num_pulses-1].t;
-        post_active = -is->sx.pulses[0].t;
-    }
-    if ((is->sk.active_flags & AF_Y) && is->sy.num_pulses) {
-        pre_active = is->sy.pulses[is->sy.num_pulses-1].t > pre_active
-            ? is->sy.pulses[is->sy.num_pulses-1].t : pre_active;
-        post_active = -is->sy.pulses[0].t > post_active
-            ? -is->sy.pulses[0].t : post_active;
-    }
-    is->sk.gen_steps_pre_active = pre_active;
-    is->sk.gen_steps_post_active = post_active;
-}
-
 int __visible
 input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
                                , int n, double a[], double t[])

klippy/extras/input_shaper.py

@@ -69,6 +69,8 @@ class AxisInputShaper:
             ffi_lib.input_shaper_set_shaper_params(
                     sk, self.axis.encode(), self.n, self.A, self.T)
         return success
+    def is_enabled(self):
+        return self.n > 0
     def disable_shaping(self):
         if self.saved is None and self.n:
             self.saved = (self.n, self.A, self.T)
@@ -89,6 +91,8 @@ class InputShaper:
     def __init__(self, config):
         self.printer = config.get_printer()
         self.printer.register_event_handler("klippy:connect", self.connect)
+        self.printer.register_event_handler("dual_carriage:update_kinematics",
+                                            self._update_kinematics)
         self.toolhead = None
         self.shapers = [AxisInputShaper('x', config),
                         AxisInputShaper('y', config)]
@@ -101,23 +105,25 @@ class InputShaper:
                                desc=self.cmd_SET_INPUT_SHAPER_help)
     def get_shapers(self):
         return self.shapers
-    def init_for_steppers(self, steppers):
-        ffi_main, ffi_lib = chelper.get_ffi()
-        for s in steppers:
-            self._get_input_shaper_stepper_kinematics(s)
     def connect(self):
         self.toolhead = self.printer.lookup_object("toolhead")
+        dual_carriage = self.printer.lookup_object('dual_carriage', None)
+        if dual_carriage is not None:
+            for shaper in self.shapers:
+                if shaper.is_enabled():
+                    raise printer.config_error(
+                            'Input shaper parameters cannot be configured via'
+                            ' [input_shaper] section with dual_carriage(s) '
+                            ' enabled. Refer to Klipper documentation on how '
+                            ' to configure input shaper for dual_carriage(s).')
+            return
         # Configure initial values
         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)
@@ -125,8 +131,27 @@ class InputShaper:
         if res < 0:
             stepper.set_stepper_kinematics(sk)
             return None
+        self.orig_stepper_kinematics.append(sk)
         self.input_shaper_stepper_kinematics.append(is_sk)
         return is_sk
+    def _update_kinematics(self):
+        if self.toolhead is None:
+            # Klipper initialization is not yet completed
+            return
+        ffi_main, ffi_lib = chelper.get_ffi()
+        kin = self.toolhead.get_kinematics()
+        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
+            old_delay = ffi_lib.input_shaper_get_step_generation_window(is_sk)
+            ffi_lib.input_shaper_update_sk(is_sk)
+            new_delay = ffi_lib.input_shaper_get_step_generation_window(is_sk)
+            if old_delay != new_delay:
+                self.toolhead.note_step_generation_scan_time(new_delay,
+                                                             old_delay)
     def _update_input_shaping(self, error=None):
         self.toolhead.flush_step_generation()
         ffi_main, ffi_lib = chelper.get_ffi()

klippy/kinematics/idex_modes.py

@@ -18,17 +18,18 @@ class DualCarriages:
                  safe_dist={}):
         self.printer = printer
         self.axes = axes
-        steppers = self._init_steppers(primary_rails + dual_rails)
+        self._init_steppers(primary_rails + dual_rails)
         self.primary_rails = [
-                DualCarriagesRail(c, dual_rails[i], axes[i], active=True)
+                DualCarriagesRail(printer, c, dual_rails[i],
+                                  axes[i], active=True)
                 for i, c in enumerate(primary_rails)]
         self.dual_rails = [
-                DualCarriagesRail(c, primary_rails[i], axes[i], active=False)
+                DualCarriagesRail(printer, c, primary_rails[i],
+                                  axes[i], active=False)
                 for i, c in enumerate(dual_rails)]
         self.dc_rails = collections.OrderedDict(
                 [(c.rail.get_name(short=True), c)
                  for c in self.primary_rails + self.dual_rails])
-        self._init_shapers(steppers)
         self.saved_states = {}
         self.safe_dist = {}
         for i, dc in enumerate(dual_rails):
@@ -76,13 +77,6 @@ class DualCarriages:
             self.dc_stepper_kinematics.append(sk)
             self.orig_stepper_kinematics.append(orig_sk)
             s.set_stepper_kinematics(sk)
-        return steppers
-    def _init_shapers(self, steppers):
-        input_shaper = self.printer.lookup_object("input_shaper", None)
-        if input_shaper is not None:
-            # Make sure to initialize input shaper stepper kinematics
-            # before modifying IDEX stepper kinematics.
-            input_shaper.init_for_steppers(steppers)
     def get_axes(self):
         return self.axes
     def get_primary_rail(self, axis):
@@ -324,7 +318,8 @@ class DualCarriages:
 
 class DualCarriagesRail:
     ENC_AXES = [b'x', b'y']
-    def __init__(self, rail, dual_rail, axis, active):
+    def __init__(self, printer, rail, dual_rail, axis, active):
+        self.printer = printer
         self.rail = rail
         self.dual_rail = dual_rail
         self.sks = [s.get_stepper_kinematics() for s in rail.get_steppers()]
@@ -343,6 +338,7 @@ class DualCarriagesRail:
         for sk in self.sks:
             ffi_lib.dual_carriage_set_transform(
                     sk, self.ENC_AXES[self.axis], self.scale, self.offset)
+        self.printer.send_event('dual_carriage:update_kinematics')
     def activate(self, mode, position, old_position=None):
         old_axis_position = self.get_axis_position(old_position or position)
         self.scale = -1. if mode == MIRROR else 1.