aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--klippy/chelper.py2
-rw-r--r--klippy/pyhelper.h3
-rw-r--r--klippy/stepcompress.c83
3 files changed, 45 insertions, 43 deletions
diff --git a/klippy/chelper.py b/klippy/chelper.py
index 230f6a3d..b05bc74c 100644
--- a/klippy/chelper.py
+++ b/klippy/chelper.py
@@ -6,7 +6,7 @@
import os, logging
import cffi
-COMPILE_CMD = "gcc -Wall -g -O -shared -fPIC -o %s %s"
+COMPILE_CMD = "gcc -Wall -g -O2 -shared -fPIC -o %s %s"
SOURCE_FILES = ['stepcompress.c', 'serialqueue.c', 'pyhelper.c']
DEST_LIB = "c_helper.so"
OTHER_FILES = ['list.h', 'serialqueue.h', 'pyhelper.h']
diff --git a/klippy/pyhelper.h b/klippy/pyhelper.h
index 1042214b..d564c78b 100644
--- a/klippy/pyhelper.h
+++ b/klippy/pyhelper.h
@@ -1,6 +1,9 @@
#ifndef PYHELPER_H
#define PYHELPER_H
+#define likely(x) __builtin_expect(!!(x), 1)
+#define unlikely(x) __builtin_expect(!!(x), 0)
+
double get_monotonic(void);
struct timespec fill_time(double time);
void set_python_logging_callback(void (*func)(const char *));
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
index 87f5a45f..fcdeb640 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -362,16 +362,13 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
return 0;
}
-#define likely(x) __builtin_expect(!!(x), 1)
-
// Check if the internal queue needs to be expanded, and expand if so
static int
-_check_expand(struct stepcompress *sc, uint64_t *qn)
+_check_push(struct stepcompress *sc)
{
- sc->queue_next = qn;
- if (qn - sc->queue_pos > 65535 + 2000) {
+ if (sc->queue_next - sc->queue_pos > 65535 + 2000) {
// No point in keeping more than 64K steps in memory
- int ret = stepcompress_flush(sc, *(qn - 65535));
+ int ret = stepcompress_flush(sc, *(sc->queue_next - 65535));
if (ret)
return ret;
}
@@ -379,15 +376,18 @@ _check_expand(struct stepcompress *sc, uint64_t *qn)
return 0;
}
static inline int
-check_expand(struct stepcompress *sc, uint64_t **pqn, uint64_t **pqend)
+check_push(struct stepcompress *sc, uint64_t **pqnext, uint64_t **pqend
+ , uint64_t c)
{
- if (likely(*pqn < *pqend))
- return 0;
- int ret = _check_expand(sc, *pqn);
- if (ret)
- return ret;
- *pqn = sc->queue_next;
- *pqend = sc->queue_end;
+ if (unlikely(*pqnext >= *pqend)) {
+ sc->queue_next = *pqnext;
+ int ret = _check_push(sc);
+ if (ret)
+ return ret;
+ *pqnext = sc->queue_next;
+ *pqend = sc->queue_end;
+ }
+ *(*pqnext)++ = c;
return 0;
}
@@ -455,12 +455,11 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
if (ret)
return ret;
step_clock += 0.5;
- uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
- ret = check_expand(sc, &qn, &qend);
+ uint64_t *qnext = sc->queue_next, *qend = sc->queue_end;
+ ret = check_push(sc, &qnext, &qend, step_clock);
if (ret)
return ret;
- *qn++ = step_clock;
- sc->queue_next = qn;
+ sc->queue_next = qnext;
return 0;
}
@@ -500,15 +499,15 @@ stepcompress_push_const(
// Calculate each step time
clock_offset += 0.5;
double pos = step_offset + .5;
- uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
+ uint64_t *qnext = sc->queue_next, *qend = sc->queue_end;
if (!accel) {
// Move at constant velocity (zero acceleration)
double inv_cruise_sv = 1. / start_sv;
while (count--) {
- int ret = check_expand(sc, &qn, &qend);
+ uint64_t c = clock_offset + pos*inv_cruise_sv;
+ int ret = check_push(sc, &qnext, &qend, c);
if (ret)
return ret;
- *qn++ = clock_offset + pos*inv_cruise_sv;
pos += 1.0;
}
} else {
@@ -518,15 +517,15 @@ stepcompress_push_const(
pos += .5 * start_sv*start_sv * inv_accel;
double accel_multiplier = 2. * inv_accel;
while (count--) {
- int ret = check_expand(sc, &qn, &qend);
+ double v = safe_sqrt(pos * accel_multiplier);
+ uint64_t c = clock_offset + (accel_multiplier >= 0. ? v : -v);
+ int ret = check_push(sc, &qnext, &qend, c);
if (ret)
return ret;
- double v = safe_sqrt(pos * accel_multiplier);
- *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
pos += 1.0;
}
}
- sc->queue_next = qn;
+ sc->queue_next = qnext;
return res;
}
@@ -561,42 +560,42 @@ _stepcompress_push_delta(
clock_offset += 0.5;
height += (sdir ? .5 : -.5);
double start_pos = movexy_r*startxy_sd;
- uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
+ uint64_t *qnext = sc->queue_next, *qend = sc->queue_end;
if (!accel) {
// Move at constant velocity (zero acceleration)
double inv_cruise_sv = 1. / start_sv;
if (!movez_r) {
// Optmized case for common XY only moves (no Z movement)
while (count--) {
- int ret = check_expand(sc, &qn, &qend);
- if (ret)
- return ret;
double v = safe_sqrt(arm_sd2 - height*height);
double pos = start_pos + (sdir ? -v : v);
- *qn++ = clock_offset + pos * inv_cruise_sv;
+ uint64_t c = clock_offset + pos * inv_cruise_sv;
+ int ret = check_push(sc, &qnext, &qend, c);
+ if (ret)
+ return ret;
height += (sdir ? 1. : -1.);
}
} else if (!movexy_r) {
// Optmized case for Z only moves
double v = (sdir ? -end_height : end_height);
while (count--) {
- int ret = check_expand(sc, &qn, &qend);
+ double pos = start_pos + movez_r*height + v;
+ uint64_t c = clock_offset + pos * inv_cruise_sv;
+ int ret = check_push(sc, &qnext, &qend, c);
if (ret)
return ret;
- double pos = start_pos + movez_r*height + v;
- *qn++ = clock_offset + pos * inv_cruise_sv;
height += (sdir ? 1. : -1.);
}
} else {
// General case (handles XY+Z moves)
while (count--) {
- int ret = check_expand(sc, &qn, &qend);
- if (ret)
- return ret;
double relheight = movexy_r*height - movez_r*startxy_sd;
double v = safe_sqrt(arm_sd2 - relheight*relheight);
double pos = start_pos + movez_r*height + (sdir ? -v : v);
- *qn++ = clock_offset + pos * inv_cruise_sv;
+ uint64_t c = clock_offset + pos * inv_cruise_sv;
+ int ret = check_push(sc, &qnext, &qend, c);
+ if (ret)
+ return ret;
height += (sdir ? 1. : -1.);
}
}
@@ -607,18 +606,18 @@ _stepcompress_push_delta(
start_pos += 0.5 * start_sv*start_sv * inv_accel;
double accel_multiplier = 2. * inv_accel;
while (count--) {
- int ret = check_expand(sc, &qn, &qend);
- if (ret)
- return ret;
double relheight = movexy_r*height - movez_r*startxy_sd;
double v = safe_sqrt(arm_sd2 - relheight*relheight);
double pos = start_pos + movez_r*height + (sdir ? -v : v);
v = safe_sqrt(pos * accel_multiplier);
- *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
+ uint64_t c = clock_offset + (accel_multiplier >= 0. ? v : -v);
+ int ret = check_push(sc, &qnext, &qend, c);
+ if (ret)
+ return ret;
height += (sdir ? 1. : -1.);
}
}
- sc->queue_next = qn;
+ sc->queue_next = qnext;
return res;
}
generated by cgit v1.2.3-70-g09d2 (git 2.50.1) at 2025-09-02 16:02:36 +0000