3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

kin_shaper: Allocate space for maximum number of shaper pulses in advance 

Combine the shaper pulses and count of those pulses into a `struct
shaper_pulses`.  Allocate space for the maximum number of pulses in
`struct input_shaper`.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2020-08-30 11:19:06 -04:00
parent b6a25393d3
commit 0e21188cb2
1 changed files with 99 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

209
klippy/chelper/kin_shaper.c
View File

@ -40,20 +40,24 @@ get_axis_position_across_moves(struct move *m, int axis, double time)
return get_axis_position(m, axis, time); return get_axis_position(m, axis, time);
} }
struct shaper_pulse { struct shaper_pulses {
double t, a; int num_pulses;
struct {
double t, a;
} pulses[5];
}; };
// Calculate the position from the convolution of the shaper with input signal // Calculate the position from the convolution of the shaper with input signal
static inline double static inline double
calc_position(struct move *m, int axis, double move_time calc_position(struct move *m, int axis, double move_time
, struct shaper_pulse *pulses, int n) , struct shaper_pulses *sp)
{ {
double res = 0.; double res = 0.;
int i; int num_pulses = sp->num_pulses, i;
for (i = 0; i < n; ++i) for (i = 0; i < num_pulses; ++i) {
res += pulses[i].a * get_axis_position_across_moves( double t = sp->pulses[i].t, a = sp->pulses[i].a;
m, axis, move_time + pulses[i].t); res += a * get_axis_position_across_moves(m, axis, move_time + t);
}
return res; return res;
} }
@ -76,13 +80,12 @@ struct input_shaper {
struct stepper_kinematics sk; struct stepper_kinematics sk;
struct stepper_kinematics *orig_sk; struct stepper_kinematics *orig_sk;
struct move m; struct move m;
struct shaper_pulse *x_pulses, *y_pulses; struct shaper_pulses sx, sy;
int x_n, y_n;
}; };
typedef void (*is_init_shaper_callback)(double shaper_freq typedef void (*is_init_shaper_callback)(double shaper_freq
, double damping_ratio , double damping_ratio
, struct shaper_pulse **pulses, int *n); , struct shaper_pulses *sp);
static inline double static inline double
calc_ZV_K(double damping_ratio) calc_ZV_K(double damping_ratio)
@ -100,49 +103,46 @@ calc_half_period(double shaper_freq, double damping_ratio)
static void static void
init_shaper_zv(double shaper_freq, double damping_ratio init_shaper_zv(double shaper_freq, double damping_ratio
, struct shaper_pulse **pulses, int *n) , struct shaper_pulses *sp)
{ {
*n = 2; sp->num_pulses = 2;
*pulses = malloc(*n * sizeof(struct shaper_pulse));
double half_period = calc_half_period(shaper_freq, damping_ratio); double half_period = calc_half_period(shaper_freq, damping_ratio);
double K = calc_ZV_K(damping_ratio); double K = calc_ZV_K(damping_ratio);
double inv_D = 1. / (1. + K); double inv_D = 1. / (1. + K);
(*pulses)[0].t = -half_period; sp->pulses[0].t = -half_period;
(*pulses)[1].t = 0.; sp->pulses[1].t = 0.;
(*pulses)[0].a = K * inv_D; sp->pulses[0].a = K * inv_D;
(*pulses)[1].a = inv_D; sp->pulses[1].a = inv_D;
} }
static void static void
init_shaper_zvd(double shaper_freq, double damping_ratio init_shaper_zvd(double shaper_freq, double damping_ratio
, struct shaper_pulse **pulses, int *n) , struct shaper_pulses *sp)
{ {
*n = 3; sp->num_pulses = 3;
*pulses = malloc(*n * sizeof(struct shaper_pulse));
double half_period = calc_half_period(shaper_freq, damping_ratio); double half_period = calc_half_period(shaper_freq, damping_ratio);
double K = calc_ZV_K(damping_ratio); double K = calc_ZV_K(damping_ratio);
double K2 = K * K; double K2 = K * K;
double inv_D = 1. / (K2 + 2. * K + 1.); double inv_D = 1. / (K2 + 2. * K + 1.);
(*pulses)[0].t = -2. * half_period; sp->pulses[0].t = -2. * half_period;
(*pulses)[1].t = -half_period; sp->pulses[1].t = -half_period;
(*pulses)[2].t = 0.; sp->pulses[2].t = 0.;
(*pulses)[0].a = K2 * inv_D; sp->pulses[0].a = K2 * inv_D;
(*pulses)[1].a = 2. * K * inv_D; sp->pulses[1].a = 2. * K * inv_D;
(*pulses)[2].a = inv_D; sp->pulses[2].a = inv_D;
} }
static void static void
init_shaper_mzv(double shaper_freq, double damping_ratio init_shaper_mzv(double shaper_freq, double damping_ratio
, struct shaper_pulse **pulses, int *n) , struct shaper_pulses *sp)
{ {
*n = 3; sp->num_pulses = 3;
*pulses = malloc(*n * sizeof(struct shaper_pulse));
double half_period = calc_half_period(shaper_freq, damping_ratio); double half_period = calc_half_period(shaper_freq, damping_ratio);
double K = exp(-.75 * damping_ratio * M_PI double K = exp(-.75 * damping_ratio * M_PI
@ -153,21 +153,20 @@ init_shaper_mzv(double shaper_freq, double damping_ratio
double a3 = a1 * K * K; double a3 = a1 * K * K;
double inv_D = 1. / (a1 + a2 + a3); double inv_D = 1. / (a1 + a2 + a3);
(*pulses)[0].t = -1.5 * half_period; sp->pulses[0].t = -1.5 * half_period;
(*pulses)[1].t = -.75 * half_period; sp->pulses[1].t = -.75 * half_period;
(*pulses)[2].t = 0.; sp->pulses[2].t = 0.;
(*pulses)[0].a = a3 * inv_D; sp->pulses[0].a = a3 * inv_D;
(*pulses)[1].a = a2 * inv_D; sp->pulses[1].a = a2 * inv_D;
(*pulses)[2].a = a1 * inv_D; sp->pulses[2].a = a1 * inv_D;
} }
static void static void
init_shaper_ei(double shaper_freq, double damping_ratio init_shaper_ei(double shaper_freq, double damping_ratio
, struct shaper_pulse **pulses, int *n) , struct shaper_pulses *sp)
{ {
*n = 3; sp->num_pulses = 3;
*pulses = malloc(*n * sizeof(struct shaper_pulse));
double half_period = calc_half_period(shaper_freq, damping_ratio); double half_period = calc_half_period(shaper_freq, damping_ratio);
double K = calc_ZV_K(damping_ratio); double K = calc_ZV_K(damping_ratio);
@ -176,21 +175,20 @@ init_shaper_ei(double shaper_freq, double damping_ratio
double a3 = a1 * K * K; double a3 = a1 * K * K;
double inv_D = 1. / (a1 + a2 + a3); double inv_D = 1. / (a1 + a2 + a3);
(*pulses)[0].t = -2. * half_period; sp->pulses[0].t = -2. * half_period;
(*pulses)[1].t = -half_period; sp->pulses[1].t = -half_period;
(*pulses)[2].t = 0.; sp->pulses[2].t = 0.;
(*pulses)[0].a = a3 * inv_D; sp->pulses[0].a = a3 * inv_D;
(*pulses)[1].a = a2 * inv_D; sp->pulses[1].a = a2 * inv_D;
(*pulses)[2].a = a1 * inv_D; sp->pulses[2].a = a1 * inv_D;
} }
static void static void
init_shaper_2hump_ei(double shaper_freq, double damping_ratio init_shaper_2hump_ei(double shaper_freq, double damping_ratio
, struct shaper_pulse **pulses, int *n) , struct shaper_pulses *sp)
{ {
*n = 4; sp->num_pulses = 4;
*pulses = malloc(*n * sizeof(struct shaper_pulse));
double half_period = calc_half_period(shaper_freq, damping_ratio); double half_period = calc_half_period(shaper_freq, damping_ratio);
double K = calc_ZV_K(damping_ratio); double K = calc_ZV_K(damping_ratio);
@ -203,23 +201,22 @@ init_shaper_2hump_ei(double shaper_freq, double damping_ratio
double a4 = a1 * K * K * K; double a4 = a1 * K * K * K;
double inv_D = 1. / (a1 + a2 + a3 + a4); double inv_D = 1. / (a1 + a2 + a3 + a4);
(*pulses)[0].t = -3. * half_period; sp->pulses[0].t = -3. * half_period;
(*pulses)[1].t = -2. * half_period; sp->pulses[1].t = -2. * half_period;
(*pulses)[2].t = -half_period; sp->pulses[2].t = -half_period;
(*pulses)[3].t = 0.; sp->pulses[3].t = 0.;
(*pulses)[0].a = a4 * inv_D; sp->pulses[0].a = a4 * inv_D;
(*pulses)[1].a = a3 * inv_D; sp->pulses[1].a = a3 * inv_D;
(*pulses)[2].a = a2 * inv_D; sp->pulses[2].a = a2 * inv_D;
(*pulses)[3].a = a1 * inv_D; sp->pulses[3].a = a1 * inv_D;
} }
static void static void
init_shaper_3hump_ei(double shaper_freq, double damping_ratio init_shaper_3hump_ei(double shaper_freq, double damping_ratio
, struct shaper_pulse **pulses, int *n) , struct shaper_pulses *sp)
{ {
*n = 5; sp->num_pulses = 5;
*pulses = malloc(*n * sizeof(struct shaper_pulse));
double half_period = calc_half_period(shaper_freq, damping_ratio); double half_period = calc_half_period(shaper_freq, damping_ratio);
double K = calc_ZV_K(damping_ratio); double K = calc_ZV_K(damping_ratio);
@ -233,30 +230,30 @@ init_shaper_3hump_ei(double shaper_freq, double damping_ratio
double a5 = a1 * K2 * K2; double a5 = a1 * K2 * K2;
double inv_D = 1. / (a1 + a2 + a3 + a4 + a5); double inv_D = 1. / (a1 + a2 + a3 + a4 + a5);
(*pulses)[0].t = -4. * half_period; sp->pulses[0].t = -4. * half_period;
(*pulses)[1].t = -3. * half_period; sp->pulses[1].t = -3. * half_period;
(*pulses)[2].t = -2. * half_period; sp->pulses[2].t = -2. * half_period;
(*pulses)[3].t = -half_period; sp->pulses[3].t = -half_period;
(*pulses)[4].t = 0.; sp->pulses[4].t = 0.;
(*pulses)[0].a = a5 * inv_D; sp->pulses[0].a = a5 * inv_D;
(*pulses)[1].a = a4 * inv_D; sp->pulses[1].a = a4 * inv_D;
(*pulses)[2].a = a3 * inv_D; sp->pulses[2].a = a3 * inv_D;
(*pulses)[3].a = a2 * inv_D; sp->pulses[3].a = a2 * inv_D;
(*pulses)[4].a = a1 * inv_D; sp->pulses[4].a = a1 * inv_D;
} }
// Shift pulses around 'mid-point' t=0 so that the input shaper is an identity // Shift pulses around 'mid-point' t=0 so that the input shaper is an identity
// transformation for constant-speed motion (i.e. input_shaper(v * T) = v * T) // transformation for constant-speed motion (i.e. input_shaper(v * T) = v * T)
static void static void
shift_pulses(int n, struct shaper_pulse *pulses) shift_pulses(struct shaper_pulses *sp)
{ {
int i; int i;
double ts = 0.; double ts = 0.;
for (i = 0; i < n; ++i) for (i = 0; i < sp->num_pulses; ++i)
ts += pulses[i].a * pulses[i].t; ts += sp->pulses[i].a * sp->pulses[i].t;
for (i = 0; i < n; ++i) for (i = 0; i < sp->num_pulses; ++i)
pulses[i].t -= ts; sp->pulses[i].t -= ts;
} }
/**************************************************************** /****************************************************************
@ -271,9 +268,9 @@ shaper_x_calc_position(struct stepper_kinematics *sk, struct move *m
, double move_time) , double move_time)
{ {
struct input_shaper *is = container_of(sk, struct input_shaper, sk); struct input_shaper *is = container_of(sk, struct input_shaper, sk);
if (!is->x_n) if (!is->sx.num_pulses)
return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time);
is->m.start_pos.x = calc_position(m, 'x', move_time, is->x_pulses, is->x_n); is->m.start_pos.x = calc_position(m, 'x', move_time, &is->sx);
return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
} }
@ -283,9 +280,9 @@ shaper_y_calc_position(struct stepper_kinematics *sk, struct move *m
, double move_time) , double move_time)
{ {
struct input_shaper *is = container_of(sk, struct input_shaper, sk); struct input_shaper *is = container_of(sk, struct input_shaper, sk);
if (!is->y_n) if (!is->sy.num_pulses)
return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time);
is->m.start_pos.y = calc_position(m, 'y', move_time, is->y_pulses, is->y_n); is->m.start_pos.y = calc_position(m, 'y', move_time, &is->sy);
return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
} }
@ -295,15 +292,13 @@ shaper_xy_calc_position(struct stepper_kinematics *sk, struct move *m
, double move_time) , double move_time)
{ {
struct input_shaper *is = container_of(sk, struct input_shaper, sk); struct input_shaper *is = container_of(sk, struct input_shaper, sk);
if (!is->x_n && !is->y_n) if (!is->sx.num_pulses && !is->sy.num_pulses)
return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time);
is->m.start_pos = move_get_coord(m, move_time); is->m.start_pos = move_get_coord(m, move_time);
if (is->x_n) if (is->sx.num_pulses)
is->m.start_pos.x = calc_position(m, 'x', move_time is->m.start_pos.x = calc_position(m, 'x', move_time, &is->sx);
, is->x_pulses, is->x_n); if (is->sy.num_pulses)
if (is->y_n) is->m.start_pos.y = calc_position(m, 'y', move_time, &is->sy);
is->m.start_pos.y = calc_position(m, 'y', move_time
, is->y_pulses, is->y_n);
return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
} }
@ -311,15 +306,15 @@ static void
shaper_note_generation_time(struct input_shaper *is) shaper_note_generation_time(struct input_shaper *is)
{ {
double pre_active = 0., post_active = 0.; double pre_active = 0., post_active = 0.;
if ((is->sk.active_flags & AF_X) && is->x_n) { if ((is->sk.active_flags & AF_X) && is->sx.num_pulses) {
pre_active = is->x_pulses[is->x_n-1].t; pre_active = is->sx.pulses[is->sx.num_pulses-1].t;
post_active = -is->x_pulses[0].t; post_active = -is->sx.pulses[0].t;
} }
if ((is->sk.active_flags & AF_Y) && is->y_n) { if ((is->sk.active_flags & AF_Y) && is->sy.num_pulses) {
pre_active = is->y_pulses[is->y_n-1].t > pre_active pre_active = is->sy.pulses[is->sy.num_pulses-1].t > pre_active
? is->y_pulses[is->y_n-1].t : pre_active; ? is->sy.pulses[is->sy.num_pulses-1].t : pre_active;
post_active = -is->y_pulses[0].t > post_active post_active = -is->sy.pulses[0].t > post_active
? -is->y_pulses[0].t : post_active; ? -is->sy.pulses[0].t : post_active;
} }
is->sk.gen_steps_pre_active = pre_active; is->sk.gen_steps_pre_active = pre_active;
is->sk.gen_steps_post_active = post_active; is->sk.gen_steps_post_active = post_active;
@ -370,23 +365,19 @@ input_shaper_set_shaper_params(struct stepper_kinematics *sk
return -1; return -1;
int af = is->orig_sk->active_flags & (AF_X | AF_Y); int af = is->orig_sk->active_flags & (AF_X | AF_Y);
free(is->x_pulses);
if ((af & AF_X) && shaper_freq_x > 0.) { if ((af & AF_X) && shaper_freq_x > 0.) {
init_shaper_callbacks[shaper_type_x]( init_shaper_callbacks[shaper_type_x](
shaper_freq_x, damping_ratio_x, &is->x_pulses, &is->x_n); shaper_freq_x, damping_ratio_x, &is->sx);
shift_pulses(is->x_n, is->x_pulses); shift_pulses(&is->sx);
} else { } else {
is->x_pulses = NULL; is->sx.num_pulses = 0;
is->x_n = 0;
} }
free(is->y_pulses);
if ((af & AF_Y) && shaper_freq_y > 0.) { if ((af & AF_Y) && shaper_freq_y > 0.) {
init_shaper_callbacks[shaper_type_y]( init_shaper_callbacks[shaper_type_y](
shaper_freq_y, damping_ratio_y, &is->y_pulses, &is->y_n); shaper_freq_y, damping_ratio_y, &is->sy);
shift_pulses(is->y_n, is->y_pulses); shift_pulses(&is->sy);
} else { } else {
is->y_pulses = NULL; is->sy.num_pulses = 0;
is->y_n = 0;
} }
shaper_note_generation_time(is); shaper_note_generation_time(is);
return 0; return 0;
@ -401,14 +392,12 @@ input_shaper_get_step_generation_window(int shaper_type, double shaper_freq
if (shaper_type >= ARRAY_SIZE(init_shaper_callbacks) || shaper_type < 0) if (shaper_type >= ARRAY_SIZE(init_shaper_callbacks) || shaper_type < 0)
return 0.; return 0.;
is_init_shaper_callback init_shaper_cb = init_shaper_callbacks[shaper_type]; is_init_shaper_callback init_shaper_cb = init_shaper_callbacks[shaper_type];
int n; struct shaper_pulses sp;
struct shaper_pulse *pulses; init_shaper_cb(shaper_freq, damping_ratio, &sp);
init_shaper_cb(shaper_freq, damping_ratio, &pulses, &n); shift_pulses(&sp);
shift_pulses(n, pulses); double window = -sp.pulses[0].t;
double window = -pulses[0].t; if (sp.pulses[sp.num_pulses-1].t > window)
if (pulses[n-1].t > window) window = sp.pulses[sp.num_pulses-1].t;
window = pulses[n-1].t;
free(pulses);
return window; return window;
} }