Mirror
/
klipper-dgus
mirror of https://github.com/Desuuuu/klipper.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

stepcompress: Simplify delta Z only move calculations 

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-04-20 00:30:28 -04:00
parent 19ffaa9ff0
commit 4b1a530330
1 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
klippy/stepcompress.c
View File

@ -567,16 +567,15 @@ _stepcompress_push_delta(
// Calculate each step time // Calculate each step time
clock_offset += 0.5; clock_offset += 0.5;
height += (sdir ? .5 : -.5); height += (sdir ? .5 : -.5);
double start_pos = movexy_r*startxy_sd;
uint64_t *qnext = sc->queue_next, *qend = sc->queue_end; uint64_t *qnext = sc->queue_next, *qend = sc->queue_end;
if (!accel) { if (!accel) {
// Move at constant velocity (zero acceleration) // Move at constant velocity (zero acceleration)
double inv_cruise_sv = 1. / start_sv; double inv_cruise_sv = 1. / start_sv;
if (!movez_r) { if (!movez_r) {
// Optmized case for common XY only moves (no Z movement) // Optimized case for common XY only moves (no Z movement)
while (count--) { while (count--) {
double v = safe_sqrt(arm_sd2 - height*height); double v = safe_sqrt(arm_sd2 - height*height);
double pos = start_pos + (sdir ? -v : v); double pos = startxy_sd + (sdir ? -v : v);
uint64_t c = clock_offset + pos * inv_cruise_sv; uint64_t c = clock_offset + pos * inv_cruise_sv;
int ret = check_push(sc, &qnext, &qend, c); int ret = check_push(sc, &qnext, &qend, c);
if (ret) if (ret)
@ -584,20 +583,20 @@ _stepcompress_push_delta(
height += (sdir ? 1. : -1.); height += (sdir ? 1. : -1.);
} }
} else if (!movexy_r) { } else if (!movexy_r) {
// Optmized case for Z only moves // Optimized case for Z only moves
double v = (sdir ? -end_height : end_height); double pos = (sdir ? height-end_height : end_height-height);
while (count--) { while (count--) {
double pos = start_pos + movez_r*height + v;
uint64_t c = clock_offset + pos * inv_cruise_sv; uint64_t c = clock_offset + pos * inv_cruise_sv;
int ret = check_push(sc, &qnext, &qend, c); int ret = check_push(sc, &qnext, &qend, c);
if (ret) if (ret)
return ret; return ret;
height += (sdir ? 1. : -1.); pos += 1.;
} }
} else { } else {
// General case (handles XY+Z moves) // General case (handles XY+Z moves)
double start_pos = movexy_r*startxy_sd, zoffset = movez_r*startxy_sd;
while (count--) { while (count--) {
double relheight = movexy_r*height - movez_r*startxy_sd; double relheight = movexy_r*height - zoffset;
double v = safe_sqrt(arm_sd2 - relheight*relheight); double v = safe_sqrt(arm_sd2 - relheight*relheight);
double pos = start_pos + movez_r*height + (sdir ? -v : v); double pos = start_pos + movez_r*height + (sdir ? -v : v);
uint64_t c = clock_offset + pos * inv_cruise_sv; uint64_t c = clock_offset + pos * inv_cruise_sv;
@ -609,12 +608,13 @@ _stepcompress_push_delta(
} }
} else { } else {
// Move with constant acceleration // Move with constant acceleration
double start_pos = movexy_r*startxy_sd, zoffset = movez_r*startxy_sd;
double inv_accel = 1. / accel; double inv_accel = 1. / accel;
clock_offset -= start_sv * inv_accel; clock_offset -= start_sv * inv_accel;
start_pos += 0.5 * start_sv*start_sv * inv_accel; start_pos += 0.5 * start_sv*start_sv * inv_accel;
double accel_multiplier = 2. * inv_accel; double accel_multiplier = 2. * inv_accel;
while (count--) { while (count--) {
double relheight = movexy_r*height - movez_r*startxy_sd; double relheight = movexy_r*height - zoffset;
double v = safe_sqrt(arm_sd2 - relheight*relheight); double v = safe_sqrt(arm_sd2 - relheight*relheight);
double pos = start_pos + movez_r*height + (sdir ? -v : v); double pos = start_pos + movez_r*height + (sdir ? -v : v);
v = safe_sqrt(pos * accel_multiplier); v = safe_sqrt(pos * accel_multiplier);