Added stealthchop lag compensation code

klippy/chelper/__init__.py

@@ -21,12 +21,14 @@
     'itersolve.c', 'trapq.c', 'pollreactor.c', 'msgblock.c', 'trdispatch.c',
     'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c',
     'kin_deltesian.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c',
-    'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c', 'kin_generic.c'
+    'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c', 'kin_generic.c',
+    'integrate.c', 'stepcorr.c'
 ]
 DEST_LIB = "c_helper.so"
 OTHER_FILES = [
     'list.h', 'serialqueue.h', 'stepcompress.h', 'steppersync.h',
-    'itersolve.h', 'pyhelper.h', 'trapq.h', 'pollreactor.h', 'msgblock.h'
+    'itersolve.h', 'pyhelper.h', 'trapq.h', 'pollreactor.h', 'msgblock.h',
+    'integrate.h', 'stepcorr.h'
 ]
 
 defs_stepcompress = """
@@ -179,6 +181,12 @@
     struct stepper_kinematics * dual_carriage_alloc(void);
 """
 
+defs_stepcorr = """
+    int stepcorr_set_lag_correction(struct stepper_kinematics *sk
+        , double rad_per_mm, double stealthchop_lag_const
+        , double spreadcycle_lag_const, double velocity_smooth_time);
+"""
+
 defs_serialqueue = """
     #define MESSAGE_MAX 64
     struct pull_queue_message {
@@ -239,7 +247,7 @@
     defs_kin_cartesian, defs_kin_corexy, defs_kin_corexz, defs_kin_delta,
     defs_kin_deltesian, defs_kin_polar, defs_kin_rotary_delta, defs_kin_winch,
     defs_kin_extruder, defs_kin_shaper, defs_kin_idex,
-    defs_kin_generic_cartesian,
+    defs_kin_generic_cartesian, defs_stepcorr,
 ]
 
 # Update filenames to an absolute path

klippy/chelper/integrate.c

@@ -0,0 +1,95 @@
+// Move integration utilities
+//
+// Copyright (C) 2018-2025  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2025  Dmitry Butyugin <dmbutyugin@google.com>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stddef.h> // offsetof
+#include "compiler.h" // unlikely
+#include "integrate.h" // struct stepper_kinematics
+#include "list.h" // list_node
+#include "trapq.h" // move_get_velocity
+
+inline double
+move_integrate_weighted(double base, double start_v, double half_accel
+                        , double start, double end, double time_offset)
+{
+    // Calculate the definitive integral of the motion formula:
+    //   position(t) = base + t * (start_v + t * half_accel)
+    double half_v = .5 * start_v, sixth_a = (1. / 3.) * half_accel;
+    double start_p_end = start + end;
+    double end2 = end * end;
+    double start_p_end_2 = start_p_end * start + end2;
+    double iext_avg = base + half_v * start_p_end + sixth_a * start_p_end_2;
+
+    // Calculate the definitive integral of time weighted position:
+    //   weighted_position(t) = t * (base + t * (start_v + t * half_accel))
+    double half_b = .5 * base, third_v = (1. / 3.) * start_v;
+    double eighth_a = .25 * half_accel;
+    double end3 = end2 * end;
+    double start_p_end_3 = start_p_end_2 * start + end3;
+    double wgt_ext_avg = half_b * start_p_end + third_v * start_p_end_2
+        + eighth_a * start_p_end_3;
+
+    return (wgt_ext_avg - time_offset * iext_avg) * (end - start);
+}
+
+inline double
+move_no_accel_integrate_weighted(double base, double start_v
+                                 , double start, double end, double time_offset)
+{
+    // Calculate the definitive integral of the motion formula:
+    //   position(t) = base + t * start_v
+    double half_v = .5 * start_v;
+    double start_p_end = start + end;
+    double iext_avg = base + half_v * start_p_end;
+
+    // Calculate the definitive integral of time weighted position:
+    //   weighted_position(t) = t * (base + t * start_v)
+    double half_b = .5 * base, third_v = (1. / 3.) * start_v;
+    double start_p_end_2 = start_p_end * start + end * end;
+    double wgt_ext_avg = half_b * start_p_end + third_v * start_p_end_2;
+
+    return (wgt_ext_avg - time_offset * iext_avg) * (end - start);
+}
+
+static inline double
+move_velocity_integrate(struct move *m, int axis, double start, double end
+                        , double time_offset)
+{
+    if (start < 0.)
+        start = 0.;
+    if (end > m->move_t)
+        end = m->move_t;
+    // Calculate definitive integral
+    double axis_r = m->axes_r.axis[axis - 'x'];
+    return axis_r * move_no_accel_integrate_weighted(
+            m->start_v, 2. * m->half_accel, start, end, time_offset);
+}
+
+inline double
+calc_smoothed_velocity(struct move* m, int axis, double move_time, double hst)
+{
+    if (!hst)
+        return m->axes_r.axis[axis - 'x'] * move_get_velocity(m, move_time);
+    double inv_half_smooth_time2 = 1. / (hst * hst);
+    // Calculate integral for the current move
+    double res = 0., start = move_time - hst, end = move_time + hst;
+    res += move_velocity_integrate(m, axis, start, move_time, start);
+    res -= move_velocity_integrate(m, axis, move_time, end, end);
+    // Integrate over previous moves
+    struct move *prev = m;
+    while (unlikely(start < 0.)) {
+        prev = list_prev_entry(prev, node);
+        start += prev->move_t;
+        res += move_velocity_integrate(prev, axis, start, prev->move_t, start);
+    }
+    // Integrate over future moves
+    while (unlikely(end > m->move_t)) {
+        end -= m->move_t;
+        m = list_next_entry(m, node);
+        res -= move_velocity_integrate(m, axis, 0., end, end);
+    }
+    return res * inv_half_smooth_time2;
+}

klippy/chelper/integrate.h

@@ -0,0 +1,12 @@
+#ifndef INTEGRATE_H
+#define INTEGRATE_H
+
+struct move;
+
+double calc_smoothed_velocity(struct move* m, int axis, double move_time
+                              , double hst);
+
+double move_integrate_weighted(double base, double start_v, double half_accel
+                               , double start, double end, double time_offset);
+
+#endif // integrate.h

klippy/chelper/itersolve.c

@@ -11,6 +11,7 @@
 #include "itersolve.h" // itersolve_generate_steps
 #include "pyhelper.h" // errorf
 #include "stepcompress.h" // queue_append_start
+#include "stepcorr.h" // stepcorr_calc_position
 #include "trapq.h" // struct move
 
 
@@ -29,7 +30,6 @@ static int32_t
 itersolve_gen_steps_range(struct stepper_kinematics *sk, struct stepcompress *sc
                           , struct move *m, double abs_start, double abs_end)
 {
-    sk_calc_callback calc_position_cb = sk->calc_position_cb;
     double half_step = .5 * sk->step_dist;
     double start = abs_start - m->print_time, end = abs_end - m->print_time;
     if (start < 0.)
@@ -69,7 +69,7 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct stepcompress *sc
         // Calculate position at next_time guess
         old_guess = guess;
         guess.time = next_time;
-        guess.position = calc_position_cb(sk, m, next_time);
+        guess.position = stepcorr_calc_position(sk, m, next_time);
         guess_dist = guess.position - target;
         if (fabs(guess_dist) > .000000001) {
             // Guess does not look close enough - update bounds

klippy/chelper/itersolve.h

@@ -2,6 +2,7 @@
 #define ITERSOLVE_H
 
 #include <stdint.h> // int32_t
+#include "stepcorr.h"  // stepper_corrections
 
 enum {
     AF_X = 1 << 0, AF_Y = 1 << 1, AF_Z = 1 << 2,
@@ -12,6 +13,9 @@ struct move;
 typedef double (*sk_calc_callback)(struct stepper_kinematics *sk, struct move *m
                                    , double move_time);
 typedef void (*sk_post_callback)(struct stepper_kinematics *sk);
+typedef double (*sk_calc_smooth_callback)(struct stepper_kinematics *sk
+                                          , struct move *m, double move_time
+                                          , double hst);
 struct stepper_kinematics {
     double step_dist, commanded_pos;
     struct stepcompress *sc;
@@ -21,7 +25,10 @@ struct stepper_kinematics {
     int active_flags;
     double gen_steps_pre_active, gen_steps_post_active;
 
+    struct stepper_corrections step_corr;
+
     sk_calc_callback calc_position_cb;
+    sk_calc_smooth_callback calc_smoothed_velocity_cb;
     sk_post_callback post_cb;
 };
 

klippy/chelper/kin_cartesian.c

@@ -7,6 +7,7 @@
 #include <stdlib.h> // malloc
 #include <string.h> // memset
 #include "compiler.h" // __visible
+#include "integrate.h" // calc_smoothed_velocity
 #include "itersolve.h" // struct stepper_kinematics
 #include "pyhelper.h" // errorf
 #include "trapq.h" // move_get_coord
@@ -18,33 +19,57 @@ cart_stepper_x_calc_position(struct stepper_kinematics *sk, struct move *m
     return move_get_coord(m, move_time).x;
 }
 
+static double
+cart_stepper_x_calc_velocity(struct stepper_kinematics *sk, struct move *m
+                             , double move_time, double hst)
+{
+    return calc_smoothed_velocity(m, 'x', move_time, hst);
+}
+
 static double
 cart_stepper_y_calc_position(struct stepper_kinematics *sk, struct move *m
                              , double move_time)
 {
     return move_get_coord(m, move_time).y;
 }
 
+static double
+cart_stepper_y_calc_velocity(struct stepper_kinematics *sk, struct move *m
+                             , double move_time, double hst)
+{
+    return calc_smoothed_velocity(m, 'y', move_time, hst);
+}
+
 static double
 cart_stepper_z_calc_position(struct stepper_kinematics *sk, struct move *m
                              , double move_time)
 {
     return move_get_coord(m, move_time).z;
 }
 
+static double
+cart_stepper_z_calc_velocity(struct stepper_kinematics *sk, struct move *m
+                             , double move_time, double hst)
+{
+    return calc_smoothed_velocity(m, 'z', move_time, hst);
+}
+
 struct stepper_kinematics * __visible
 cartesian_stepper_alloc(char axis)
 {
     struct stepper_kinematics *sk = malloc(sizeof(*sk));
     memset(sk, 0, sizeof(*sk));
     if (axis == 'x') {
         sk->calc_position_cb = cart_stepper_x_calc_position;
+        sk->calc_smoothed_velocity_cb = cart_stepper_x_calc_velocity;
         sk->active_flags = AF_X;
     } else if (axis == 'y') {
         sk->calc_position_cb = cart_stepper_y_calc_position;
+        sk->calc_smoothed_velocity_cb = cart_stepper_y_calc_velocity;
         sk->active_flags = AF_Y;
     } else if (axis == 'z') {
         sk->calc_position_cb = cart_stepper_z_calc_position;
+        sk->calc_smoothed_velocity_cb = cart_stepper_z_calc_velocity;
         sk->active_flags = AF_Z;
     }
     return sk;

klippy/chelper/kin_corexy.c

@@ -7,6 +7,7 @@
 #include <stdlib.h> // malloc
 #include <string.h> // memset
 #include "compiler.h" // __visible
+#include "integrate.h" // calc_smoothed_velocity
 #include "itersolve.h" // struct stepper_kinematics
 #include "trapq.h" // move_get_coord
 
@@ -18,6 +19,15 @@ corexy_stepper_plus_calc_position(struct stepper_kinematics *sk, struct move *m
     return c.x + c.y;
 }
 
+static double
+corexy_stepper_plus_calc_velocity(struct stepper_kinematics *sk, struct move *m
+                                  , double move_time, double hst)
+{
+    double v_x = calc_smoothed_velocity(m, 'x', move_time, hst);
+    double v_y = calc_smoothed_velocity(m, 'y', move_time, hst);
+    return v_x + v_y;
+}
+
 static double
 corexy_stepper_minus_calc_position(struct stepper_kinematics *sk, struct move *m
                                    , double move_time)
@@ -26,15 +36,27 @@ corexy_stepper_minus_calc_position(struct stepper_kinematics *sk, struct move *m
     return c.x - c.y;
 }
 
+static double
+corexy_stepper_minus_calc_velocity(struct stepper_kinematics *sk, struct move *m
+                                   , double move_time, double hst)
+{
+    double v_x = calc_smoothed_velocity(m, 'x', move_time, hst);
+    double v_y = calc_smoothed_velocity(m, 'y', move_time, hst);
+    return v_x - v_y;
+}
+
 struct stepper_kinematics * __visible
 corexy_stepper_alloc(char type)
 {
     struct stepper_kinematics *sk = malloc(sizeof(*sk));
     memset(sk, 0, sizeof(*sk));
-    if (type == '+')
+    if (type == '+') {
         sk->calc_position_cb = corexy_stepper_plus_calc_position;
-    else if (type == '-')
+        sk->calc_smoothed_velocity_cb = corexy_stepper_plus_calc_velocity;
+    } else if (type == '-') {
         sk->calc_position_cb = corexy_stepper_minus_calc_position;
+        sk->calc_smoothed_velocity_cb = corexy_stepper_minus_calc_velocity;
+    }
     sk->active_flags = AF_X | AF_Y;
     return sk;
 }

klippy/chelper/kin_extruder.c

@@ -8,9 +8,11 @@
 #include <stdlib.h> // malloc
 #include <string.h> // memset
 #include "compiler.h" // __visible
+#include "integrate.h" // move_integrate
 #include "itersolve.h" // struct stepper_kinematics
 #include "list.h" // list_node
 #include "pyhelper.h" // errorf
+#include "stepcorr.h" // stepcorr_update_gen_steps_window
 #include "trapq.h" // move_get_distance
 
 struct pa_params {
@@ -31,31 +33,6 @@ struct pa_params {
 //                             from=t-smooth_time/2, to=t+smooth_time/2)
 //         / ((smooth_time/2)**2))
 
-// Calculate the definitive integral of the motion formula:
-//   position(t) = base + t * (start_v + t * half_accel)
-static double
-extruder_integrate(double base, double start_v, double half_accel
-                   , double start, double end)
-{
-    double half_v = .5 * start_v, sixth_a = (1. / 3.) * half_accel;
-    double si = start * (base + start * (half_v + start * sixth_a));
-    double ei = end * (base + end * (half_v + end * sixth_a));
-    return ei - si;
-}
-
-// Calculate the definitive integral of time weighted position:
-//   weighted_position(t) = t * (base + t * (start_v + t * half_accel))
-static double
-extruder_integrate_time(double base, double start_v, double half_accel
-                        , double start, double end)
-{
-    double half_b = .5 * base, third_v = (1. / 3.) * start_v;
-    double eighth_a = .25 * half_accel;
-    double si = start * start * (half_b + start * (third_v + start * eighth_a));
-    double ei = end * end * (half_b + end * (third_v + end * eighth_a));
-    return ei - si;
-}
-
 // Calculate the definitive integral of extruder for a given move
 static double
 pa_move_integrate(struct move *m, struct list_head *pa_list
@@ -81,9 +58,7 @@ pa_move_integrate(struct move *m, struct list_head *pa_list
     double start_v = m->start_v + pressure_advance * 2. * m->half_accel;
     // Calculate definitive integral
     double ha = m->half_accel;
-    double iext = extruder_integrate(base, start_v, ha, start, end);
-    double wgt_ext = extruder_integrate_time(base, start_v, ha, start, end);
-    return wgt_ext - time_offset * iext;
+    return move_integrate_weighted(base, start_v, ha, start, end, time_offset);
 }
 
 // Calculate the definitive integral of the extruder over a range of moves
@@ -143,6 +118,7 @@ extruder_set_pressure_advance(struct stepper_kinematics *sk, double print_time
     double hst = smooth_time * .5, old_hst = es->half_smooth_time;
     es->half_smooth_time = hst;
     es->sk.gen_steps_pre_active = es->sk.gen_steps_post_active = hst;
+    stepcorr_update_gen_steps_window(&es->sk);
 
     // Cleanup old pressure advance parameters
     double cleanup_time = sk->last_flush_time - (old_hst > hst ? old_hst : hst);