diff options
Diffstat (limited to 'klippy/chelper')
| -rw-r--r-- | klippy/chelper/kin_extruder.c | 18 |
1 files changed, 11 insertions, 7 deletions
diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index 2fce006b..63dca909 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -52,7 +52,8 @@ extruder_integrate_time(double base, double start_v, double half_accel // Calculate the definitive integral of extruder for a given move static double -pa_move_integrate(struct move *m, double start, double end, double time_offset) +pa_move_integrate(struct move *m, double base, double start, double end, + double time_offset) { if (start < 0.) start = 0.; @@ -60,7 +61,7 @@ pa_move_integrate(struct move *m, double start, double end, double time_offset) end = m->move_t; // Calculate base position and velocity with pressure advance double pressure_advance = m->axes_r.y; - double base = m->start_pos.x + pressure_advance * m->start_v; + base += pressure_advance * m->start_v; double start_v = m->start_v + pressure_advance * 2. * m->half_accel; // Calculate definitive integral double ha = m->half_accel; @@ -75,20 +76,23 @@ pa_range_integrate(struct move *m, double move_time, double hst) { // Calculate integral for the current move double res = 0., start = move_time - hst, end = move_time + hst; - res += pa_move_integrate(m, start, move_time, start); - res -= pa_move_integrate(m, move_time, end, end); + double start_base = m->start_pos.x; + res += pa_move_integrate(m, 0., start, move_time, start); + res -= pa_move_integrate(m, 0., 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 += pa_move_integrate(prev, start, prev->move_t, start); + double base = prev->start_pos.x - start_base; + res += pa_move_integrate(prev, base, 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 -= pa_move_integrate(m, 0., end, end); + double base = m->start_pos.x - start_base; + res -= pa_move_integrate(m, base, 0., end, end); } return res; } @@ -109,7 +113,7 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m return m->start_pos.x + move_get_distance(m, move_time); // Apply pressure advance and average over smooth_time double area = pa_range_integrate(m, move_time, hst); - return area * es->inv_half_smooth_time2; + return m->start_pos.x + area * es->inv_half_smooth_time2; } void __visible |