diff --git a/general/package/gpio-motors/src/gpio-motors.c b/general/package/gpio-motors/src/gpio-motors.c index 9a4e3bc3f8..feccbe1636 100644 --- a/general/package/gpio-motors/src/gpio-motors.c +++ b/general/package/gpio-motors/src/gpio-motors.c @@ -3,6 +3,7 @@ #include #include #include +#include #include int PAN_PINS[4]; @@ -128,6 +129,44 @@ void gpio_config() { pclose(fp); } +/* + * usleep() cannot deliver sub-tick delays on the kernels these cameras run: + * they are HZ=100 with no high-resolution timers, so every sleep rounds up to + * a 10ms tick. usleep(1500) waits ~10ms, and 8 micro-steps x 10ms puts a hard + * ~80ms floor under every step no matter how small the requested delay is + * (measured on Hi3518EV200: 200 steps took 33s at delay 15 and still 18s at + * delay 4). Spin on CLOCK_MONOTONIC for anything below a tick instead; moves + * are short and bounded, so burning the CPU for their duration is a fair + * trade, and longer delays still go to usleep so we do not spin needlessly. + */ +void delay_us(long us) { + if (us <= 0) { + return; + } + + if (us >= 10000) { + usleep(us); + return; + } + + struct timespec start, now; + if (clock_gettime(CLOCK_MONOTONIC, &start) != 0) { + usleep(us); + return; + } + + /* 64-bit on purpose: a 32-bit long overflows after ~2.1s of elapsed + * time, which a preemption in the middle of the spin can reach */ + long long target = (long long)us * 1000; + for (;;) { + clock_gettime(CLOCK_MONOTONIC, &now); + long long elapsed = (long long)(now.tv_sec - start.tv_sec) * 1000000000LL + (now.tv_nsec - start.tv_nsec); + if (elapsed >= target) { + return; + } + } +} + void axis_run(const int pins[4], int level, int steps, int delay) { int remaining = abs(steps); if (remaining == 0) { @@ -146,7 +185,7 @@ void axis_run(const int pins[4], int level, int steps, int delay) { gpio_set(pins[i], seq[micro][i]); } - usleep(delay); + delay_us(delay); if (++micro >= 8) { micro = 0; --remaining; @@ -166,7 +205,12 @@ int main(int argc, char *argv[]) { int pan_steps = atoi(argv[1]); int tilt_steps = atoi(argv[2]); - int delay = atoi(argv[3]) * 1000; + int delay_ms = atoi(argv[3]); + if (delay_ms < 0) { + fprintf(stderr, "delay must be >= 0\n"); + return 1; + } + int delay = delay_ms * 1000; gpio_config(); for (int i = 0; i < 4; i++) {