dmbutyugin/advanced-features v2

klippy/chelper/__init__.py

@@ -13,15 +13,18 @@
 
 GCC_CMD = "gcc"
 COMPILE_ARGS = (
-    "-Wall -g -O2 -shared -fPIC"
+    "-Wall -g -O3 -shared -fPIC"
     " -flto -fwhole-program -fno-use-linker-plugin"
+    " -march=native -mcpu=native -mtune=native"
     " -o %s %s"
 )
 SSE_FLAGS = "-mfpmath=sse -msse2"
+NEON_FLAGS = "-mfpu=neon"
 SOURCE_FILES = [
     "pyhelper.c",
     "serialqueue.c",
     "stepcompress.c",
+    "stepcompress_hp.c",
     "itersolve.c",
     "trapq.c",
     "pollreactor.c",
@@ -38,6 +41,7 @@
     "kin_extruder.c",
     "kin_shaper.c",
     "kin_idex.c",
+    "integrate.c",
 ]
 DEST_LIB = "c_helper.so"
 OTHER_FILES = [
@@ -49,16 +53,19 @@
     "trapq.h",
     "pollreactor.h",
     "msgblock.h",
+    "kin_shaper.h",
+    "integrate.h",
 ]
 
 defs_stepcompress = """
     struct pull_history_steps {
         uint64_t first_clock, last_clock;
         int64_t start_position;
-        int step_count, interval, add;
+        int step_count, interval, add, add2, shift;
     };
 
     struct stepcompress *stepcompress_alloc(uint32_t oid);
+    struct stepcompress *stepcompress_hp_alloc(uint32_t oid);
     void stepcompress_fill(struct stepcompress *sc, uint32_t max_error
         , int32_t queue_step_msgtag, int32_t set_next_step_dir_msgtag);
     void stepcompress_set_invert_sdir(struct stepcompress *sc
@@ -165,14 +172,29 @@
 defs_kin_extruder = """
     struct stepper_kinematics *extruder_stepper_alloc(void);
     void extruder_set_pressure_advance(struct stepper_kinematics *sk
-        , double pressure_advance, double smooth_time);
+        , int n_params, double params[], double time_offset);
+    struct pressure_advance_params;
+    double pressure_advance_linear_model_func(double position
+        , double pa_velocity, struct pressure_advance_params *pa_params);
+    double pressure_advance_tanh_model_func(double position
+        , double pa_velocity, struct pressure_advance_params *pa_params);
+    double pressure_advance_recipr_model_func(double position
+        , double pa_velocity, struct pressure_advance_params *pa_params);
+    void extruder_set_pressure_advance_model_func(struct stepper_kinematics *sk
+        , double (*func)(double, double, struct pressure_advance_params *));
+    int extruder_set_shaper_params(struct stepper_kinematics *sk, char axis
+        , int n, double a[], double t[]);
+    int extruder_set_smoothing_params(struct stepper_kinematics *sk, char axis
+        , int n, double a[], double t_sm, double t_offs);
+    double extruder_get_step_gen_window(struct stepper_kinematics *sk);
 """
 
 defs_kin_shaper = """
-    double input_shaper_get_step_generation_window(
-        struct stepper_kinematics *sk);
+    double input_shaper_get_step_gen_window(struct stepper_kinematics *sk);
     int input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
         , int n, double a[], double t[]);
+    int input_shaper_set_smoother_params(struct stepper_kinematics *sk
+        , char axis, int n, double a[], double t_sm);
     int input_shaper_set_sk(struct stepper_kinematics *sk
         , struct stepper_kinematics *orig_sk);
     struct stepper_kinematics * input_shaper_alloc(void);
@@ -325,6 +347,8 @@ def get_ffi():
         if check_build_code(srcfiles + ofiles + [__file__], destlib):
             if check_gcc_option(SSE_FLAGS):
                 cmd = "%s %s %s" % (GCC_CMD, SSE_FLAGS, COMPILE_ARGS)
+            elif check_gcc_option(NEON_FLAGS):
+                cmd = "%s %s %s" % (GCC_CMD, NEON_FLAGS, COMPILE_ARGS)
             else:
                 cmd = "%s %s" % (GCC_CMD, COMPILE_ARGS)
             logging.info("Building C code module %s", DEST_LIB)

klippy/chelper/integrate.c

@@ -0,0 +1,106 @@
+// Helpers to integrate the smoothing weight function
+//
+// Copyright (C) 2019-2020  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2020-2023  Dmitry Butyugin <dmbutyugin@google.com>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include "compiler.h" // unlikely
+#include "integrate.h"
+#include "trapq.h" // struct move
+
+#include <string.h>
+
+/****************************************************************
+ * Generic smoother integration
+ ****************************************************************/
+
+static double coeffs[] = {
+    1./1., 1./2., 1./3., 1./4., 1./5., 1./6., 1./7., 1./8., 1./9., 1./10.,
+    1./11., 1./12., 1./13., 1./14., 1./15., 1./16., 1./17., 1./18., 1./19.,
+};
+
+inline smoother_antiderivatives
+calc_antiderivatives(const struct smoother* sm, double t)
+{
+    int n = sm->n, i;
+    double it0 = (sm->c0[0] * t + sm->c0[1]) * t;
+    double it1 = (sm->c1[0] * t + sm->c1[1]) * t;
+    double it2 = (sm->c2[0] * t + sm->c2[1]) * t;
+    for (i = 2; i < n; ++i) {
+        it0 = (it0 + sm->c0[i]) * t;
+        it1 = (it1 + sm->c1[i]) * t;
+        it2 = (it2 + sm->c2[i]) * t;
+    }
+    it1 *= t;
+    it2 *= t * t;
+    return (smoother_antiderivatives) {
+        .it0 = it0, .it1 = it1, .it2 = it2 };
+}
+
+inline smoother_antiderivatives
+diff_antiderivatives(const smoother_antiderivatives* ad1
+                     , const smoother_antiderivatives* ad2)
+{
+    return (smoother_antiderivatives) {
+        .it0 = ad2->it0 - ad1->it0,
+        .it1 = ad2->it1 - ad1->it1,
+        .it2 = ad2->it2 - ad1->it2 };
+}
+
+inline double
+integrate_move(const struct move* m, int axis, double base, double t0
+               , const smoother_antiderivatives* s
+               , double* smooth_velocity)
+{
+    double axis_r = m->axes_r.axis[axis - 'x'];
+    double start_v = m->start_v * axis_r;
+    double half_accel = m->half_accel * axis_r;
+    // Substitute the integration variable tnew = t0 - t to simplify integrals
+    double accel = 2. * half_accel;
+    base += (half_accel * t0 + start_v) * t0;
+    start_v += accel * t0;
+    double smooth_pos = base * s->it0 - start_v * s->it1 + half_accel * s->it2;
+    if (smooth_velocity)
+        *smooth_velocity = start_v * s->it0 - accel * s->it1;
+    return smooth_pos;
+}
+
+/****************************************************************
+ * Smoother initialization
+ ****************************************************************/
+
+int
+init_smoother(int n, const double a[], double t_sm, struct smoother* sm)
+{
+    if ((t_sm && n < 2) || n > ARRAY_SIZE(sm->c0))
+        return -1;
+    memset(sm, 0, sizeof(*sm));
+    sm->n = n;
+    sm->hst = 0.5 * t_sm;
+    if (!t_sm) return 0;
+    double inv_t_sm = 1. / t_sm;
+    double inv_t_sm_n = inv_t_sm;
+    int i, symm = n & 1;
+    for (i = 0; i < n; ++i) {
+        if ((i & 1) && a[i]) symm = 0;
+        double c = a[i] * inv_t_sm_n;
+        sm->c0[n-1-i] = c * coeffs[i];
+        sm->c1[n-1-i] = c * coeffs[i+1];
+        sm->c2[n-1-i] = c * coeffs[i+2];
+        inv_t_sm_n *= inv_t_sm;
+    }
+    sm->symm = symm;
+    double inv_norm = 1. / (calc_antiderivatives(sm, sm->hst).it0
+                            - calc_antiderivatives(sm, -sm->hst).it0);
+    for (i = 0; i < n; ++i) {
+        sm->c0[i] *= inv_norm;
+        sm->c1[i] *= inv_norm;
+        sm->c2[i] *= inv_norm;
+    }
+    sm->p_hst = calc_antiderivatives(sm, sm->hst);
+    sm->m_hst = calc_antiderivatives(sm, -sm->hst);
+    sm->pm_diff = diff_antiderivatives(&sm->m_hst, &sm->p_hst);
+    sm->t_offs = sm->pm_diff.it1;
+    return 0;
+}

klippy/chelper/integrate.h

@@ -0,0 +1,29 @@
+#ifndef INTEGRATE_H
+#define INTEGRATE_H
+
+typedef struct {
+    double it0, it1, it2;
+} smoother_antiderivatives;
+
+struct smoother {
+    double c0[12], c1[12], c2[12];
+    double hst, t_offs;
+    smoother_antiderivatives m_hst, p_hst, pm_diff;
+    int n, symm;
+};
+
+struct move;
+
+int init_smoother(int n, const double a[], double t_sm, struct smoother* sm);
+
+double integrate_move(const struct move* m, int axis, double base, double t0
+                      , const smoother_antiderivatives* s
+                      , double* smooth_velocity);
+
+smoother_antiderivatives
+calc_antiderivatives(const struct smoother* sm, double t);
+smoother_antiderivatives
+diff_antiderivatives(const smoother_antiderivatives* ad1
+                     , const smoother_antiderivatives* ad2);
+
+#endif // integrate.h