diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index c3f58308ac43..3294797a2e0f 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -144,18 +144,19 @@ """ defs_kin_extruder = """ + struct pressure_advance_params; struct stepper_kinematics *extruder_stepper_alloc(void); + void extruder_stepper_free(struct stepper_kinematics *sk); void extruder_set_pressure_advance(struct stepper_kinematics *sk - , int n_params, double params[], double time_offset); - struct pressure_advance_params; + , double print_time, int n_params, double params[] + , double (*func)(double, double, struct pressure_advance_params *) + , double time_offset); 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 diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index 50f33f967111..6e1dcce3d481 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -11,10 +11,32 @@ #include "compiler.h" // __visible #include "itersolve.h" // struct stepper_kinematics #include "integrate.h" // struct smoother +#include "list.h" // list_node #include "kin_shaper.h" // struct shaper_pulses #include "pyhelper.h" // errorf #include "trapq.h" // move_get_distance +struct pressure_advance_params; +typedef double (*pressure_advance_func)( + double, double, struct pressure_advance_params *pa_params); + +struct pressure_advance_params { + union { + struct { + double pressure_advance; + }; + struct { + double linear_advance, linear_offset, linearization_velocity; + }; + double params[3]; + }; + double active_print_time; + pressure_advance_func pa_func; + struct list_node node; +}; + +static const double pa_smoother_coeffs[] = {15./8., 0., -15., 0., 30.}; + // Without pressure advance, the extruder stepper position is: // extruder_position(t) = nominal_position(t) // When pressure advance is enabled, additional filament is pushed @@ -136,27 +158,11 @@ shaper_pa_range_integrate(const struct move *m, int axis, double move_time } } -struct pressure_advance_params { - union { - struct { - double pressure_advance; - }; - struct { - double linear_advance, linear_offset, linearization_velocity; - }; - double params[3]; - }; -}; - -typedef double (*pressure_advance_func)( - double, double, struct pressure_advance_params *pa_params); - struct extruder_stepper { struct stepper_kinematics sk; struct shaper_pulses sp[3]; - struct smoother sm[3]; - struct pressure_advance_params pa_params; - pressure_advance_func pa_func; + struct smoother sm[3], pa_model_smoother; + struct list_head pa_list; double time_offset; }; @@ -191,6 +197,61 @@ pressure_advance_recipr_model_func(double position, double pa_velocity return position; } +static double +pa_model_integrate(struct list_head *pa_list, double print_time + , const struct smoother *sm, double e_pos, double pa_vel) +{ + print_time += sm->t_offs; + double start = print_time - sm->hst, end = print_time + sm->hst; + // Calculate integral for the current move + struct pressure_advance_params *pa = list_last_entry( + pa_list, struct pressure_advance_params, node); + struct pressure_advance_params *next_pa = NULL; + while (unlikely(pa->active_print_time > print_time && + !list_is_first(&pa->node, pa_list))) { + next_pa = pa; + pa = list_prev_entry(pa, node); + } + if (likely(pa->active_print_time <= start && + (next_pa == NULL || end <= next_pa->active_print_time))) { + return pa->pa_func(e_pos, pa_vel, pa); + } + smoother_antiderivatives left = likely(start < pa->active_print_time) + ? calc_antiderivatives(sm, print_time - pa->active_print_time) + : sm->p_hst; + smoother_antiderivatives right = likely( + next_pa != NULL && end > next_pa->active_print_time) + ? calc_antiderivatives(sm, print_time - next_pa->active_print_time) + : sm->m_hst; + smoother_antiderivatives diff = diff_antiderivatives(&right, &left); + double res = pa->pa_func(e_pos, pa_vel, pa) * diff.it0; + + // Integrate over previous PA configs + while (likely(start < pa->active_print_time && + !list_is_first(&pa->node, pa_list))) { + pa = list_prev_entry(pa, node); + smoother_antiderivatives r = left; + left = likely(start < pa->active_print_time) + ? calc_antiderivatives(sm, print_time - pa->active_print_time) + : sm->p_hst; + diff = diff_antiderivatives(&r, &left); + res += pa->pa_func(e_pos, pa_vel, pa) * diff.it0; + } + // Integrate over next PA configs + while (likely(next_pa != NULL && end >= next_pa->active_print_time)) { + pa = next_pa; + next_pa = list_is_last(&next_pa->node, pa_list) + ? NULL : list_next_entry(next_pa, node); + smoother_antiderivatives l = right; + right = likely(next_pa != NULL && end >= next_pa->active_print_time) + ? calc_antiderivatives(sm, print_time - next_pa->active_print_time) + : sm->m_hst; + diff = diff_antiderivatives(&right, &l); + res += pa->pa_func(e_pos, pa_vel, pa) * diff.it0; + } + return res; +} + static double extruder_calc_position(struct stepper_kinematics *sk, struct move *m , double move_time) @@ -230,9 +291,11 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m } double position = e_pos.x + e_pos.y + e_pos.z; double pa_velocity = pa_vel.x + pa_vel.y + pa_vel.z; - if (pa_velocity > 0.) - return es->pa_func(position, pa_velocity, &es->pa_params); - return position; + if (pa_velocity <= 0.) + return position; + return pa_model_integrate( + &es->pa_list, m->print_time + move_time, + &es->pa_model_smoother, position, pa_velocity); } static void @@ -254,29 +317,50 @@ extruder_note_generation_time(struct extruder_stepper *es) } es->sk.gen_steps_pre_active = pre_active; es->sk.gen_steps_post_active = post_active; + init_smoother(ARRAY_SIZE(pa_smoother_coeffs), pa_smoother_coeffs, + pre_active + post_active, &es->pa_model_smoother); + es->pa_model_smoother.t_offs += 0.5 * (post_active - pre_active); } void __visible -extruder_set_pressure_advance(struct stepper_kinematics *sk +extruder_set_pressure_advance(struct stepper_kinematics *sk, double print_time , int n_params, double params[] - , double time_offset) + , pressure_advance_func func, double time_offset) { struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); + + // Cleanup old pressure advance parameters + double cleanup_time = sk->last_flush_time - es->sk.gen_steps_pre_active; + struct pressure_advance_params *first_pa = list_first_entry( + &es->pa_list, struct pressure_advance_params, node); + while (!list_is_last(&first_pa->node, &es->pa_list)) { + struct pressure_advance_params *next_pa = list_next_entry( + first_pa, node); + if (next_pa->active_print_time >= cleanup_time) break; + list_del(&first_pa->node); + first_pa = next_pa; + } + es->time_offset = time_offset; - memset(&es->pa_params, 0, sizeof(es->pa_params)); extruder_note_generation_time(es); - if (n_params < 0 || n_params > ARRAY_SIZE(es->pa_params.params)) - return; - memcpy(&es->pa_params, params, n_params * sizeof(params[0])); -} -void __visible -extruder_set_pressure_advance_model_func(struct stepper_kinematics *sk - , pressure_advance_func func) -{ - struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); - memset(&es->pa_params, 0, sizeof(es->pa_params)); - es->pa_func = func; + struct pressure_advance_params *last_pa = list_last_entry( + &es->pa_list, struct pressure_advance_params, node); + if (n_params < 0 || n_params > ARRAY_SIZE(last_pa->params)) + return; + size_t param_size = n_params * sizeof(params[0]); + if (last_pa->pa_func == func && + memcmp(&last_pa->params, params, param_size) == 0) { + // Retain old pa_params + return; + } + // Add new pressure advance parameters + struct pressure_advance_params *pa_params = malloc(sizeof(*pa_params)); + memset(pa_params, 0, sizeof(*pa_params)); + memcpy(&pa_params->params, params, param_size); + pa_params->pa_func = func; + pa_params->active_print_time = print_time; + list_add_tail(&pa_params->node, &es->pa_list); } int __visible @@ -320,7 +404,24 @@ extruder_stepper_alloc(void) struct extruder_stepper *es = malloc(sizeof(*es)); memset(es, 0, sizeof(*es)); es->sk.calc_position_cb = extruder_calc_position; - es->pa_func = pressure_advance_tanh_model_func; es->sk.active_flags = AF_X | AF_Y | AF_Z; + list_init(&es->pa_list); + struct pressure_advance_params *pa_params = malloc(sizeof(*pa_params)); + memset(pa_params, 0, sizeof(*pa_params)); + pa_params->pa_func = pressure_advance_linear_model_func; + list_add_tail(&pa_params->node, &es->pa_list); return &es->sk; } + +void __visible +extruder_stepper_free(struct stepper_kinematics *sk) +{ + struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); + while (!list_empty(&es->pa_list)) { + struct pressure_advance_params *pa = list_first_entry( + &es->pa_list, struct pressure_advance_params, node); + list_del(&pa->node); + free(pa); + } + free(sk); +} diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index 44d673b37af4..d4157f7b99a7 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -16,7 +16,9 @@ def __init__(self, config, pa_model): self.pa_model = pa_model self.axes = ['x', 'y', 'z'] def update(self, gcmd): + old_smooth_time = self.smooth_time self.smooth_time = gcmd.get_float('SMOOTH_TIME', self.smooth_time) + return self.smooth_time != old_smooth_time def update_pa_model(self, pa_model): self.pa_model = pa_model def enable_axis(self, axis): @@ -138,6 +140,7 @@ def __init__(self, config): self.pa_model = config.getchoice('pressure_advance_model', self.pa_models, PALinearModel.name)(config) + self.pa_enabled = False self.smoother = ExtruderSmoother(config, self.pa_model) self.pressure_advance_time_offset = config.getfloat( 'pressure_advance_time_offset', 0.0, minval=-0.2, maxval=0.2) @@ -145,10 +148,8 @@ def __init__(self, config): self.stepper = stepper.PrinterStepper(config) ffi_main, ffi_lib = chelper.get_ffi() self.sk_extruder = ffi_main.gc(ffi_lib.extruder_stepper_alloc(), - ffi_lib.free) + ffi_lib.extruder_stepper_free) self.stepper.set_stepper_kinematics(self.sk_extruder) - ffi_lib.extruder_set_pressure_advance_model_func( - self.sk_extruder, self.pa_model.get_func()) self.motion_queue = None self.extruder = None # Register commands @@ -202,16 +203,14 @@ def sync_to_extruder(self, extruder_name): self.extruder = extruder def _update_pressure_advance(self, pa_model, time_offset): toolhead = self.printer.lookup_object('toolhead') - toolhead.flush_step_generation() ffi_main, ffi_lib = chelper.get_ffi() + espa = ffi_lib.extruder_set_pressure_advance old_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) - if self.pa_model.name != pa_model.name: - pa_func = pa_model.get_func() - ffi_lib.extruder_set_pressure_advance_model_func(self.sk_extruder, - pa_func) pa_params = pa_model.get_pa_params() - ffi_lib.extruder_set_pressure_advance( - self.sk_extruder, len(pa_params), pa_params, time_offset) + toolhead.register_lookahead_callback( + lambda print_time: espa(self.sk_extruder, print_time, + len(pa_params), pa_params, + pa_model.get_func(), time_offset)) self.smoother.update_pa_model(pa_model) self.smoother.update_extruder_kinematics(self.sk_extruder) new_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) @@ -219,6 +218,7 @@ def _update_pressure_advance(self, pa_model, time_offset): toolhead.note_step_generation_scan_time(new_delay, old_delay) self.pa_model = pa_model self.pressure_advance_time_offset = time_offset + self.pa_enabled = pa_model.enabled() def update_input_shaping(self, shapers, exact_mode): ffi_main, ffi_lib = chelper.get_ffi() old_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) @@ -251,6 +251,9 @@ def cmd_default_SET_PRESSURE_ADVANCE(self, gcmd): raise gcmd.error("Unable to infer active extruder stepper") extruder_stepper.cmd_SET_PRESSURE_ADVANCE(gcmd) def cmd_SET_PRESSURE_ADVANCE(self, gcmd): + time_offset = gcmd.get_float('TIME_OFFSET', + self.pressure_advance_time_offset, + minval=-0.2, maxval=0.2) pa_model_name = gcmd.get('MODEL', self.pa_model.name) if pa_model_name not in self.pa_models: raise gcmd.error("Invalid MODEL='%s' choice" % (pa_model_name,)) @@ -258,10 +261,10 @@ def cmd_SET_PRESSURE_ADVANCE(self, gcmd): if pa_model_name != self.pa_model.name: pa_model = self.pa_models[pa_model_name]() pa_model.update(gcmd) - self.smoother.update(gcmd) - time_offset = gcmd.get_float('TIME_OFFSET', - self.pressure_advance_time_offset, - minval=-0.2, maxval=0.2) + smoother_updated = self.smoother.update(gcmd) + if self.pressure_advance_time_offset != time_offset or \ + self.pa_enabled != pa_model.enabled() or smoother_updated: + self.printer.lookup_object('toolhead').flush_step_generation() self._update_pressure_advance(pa_model, time_offset) msg = ("pressure_advance_model: %s\n" % (pa_model.name,) + pa_model.get_msg() + "\n" + @@ -343,6 +346,7 @@ def __init__(self, config, extruder_num): or config.get('dir_pin', None) is not None or config.get('rotation_distance', None) is not None): self.link_extruder_stepper(ExtruderStepper(config)) + self.extruder_steppers[-1].motion_queue = self.name # Register commands gcode = self.printer.lookup_object('gcode') if self.name == 'extruder':