diff --git a/general/package/gpio-motors/src/gpio-motors.c b/general/package/gpio-motors/src/gpio-motors.c index 9a4e3bc3f8..b483ca6dc9 100644 --- a/general/package/gpio-motors/src/gpio-motors.c +++ b/general/package/gpio-motors/src/gpio-motors.c @@ -1,14 +1,24 @@ #include #include +#include #include #include #include +#include #include int PAN_PINS[4]; int TILT_PINS[4]; int SELECT_PIN = -1; +int PAN_FDS[4] = {-1, -1, -1, -1}; +int TILT_FDS[4] = {-1, -1, -1, -1}; +int SELECT_FD = -1; + +/* CLOCK_MONOTONIC resolution, read once in main(): 1ns on kernels with + * high-resolution timers, one jiffy (10ms at HZ=100) without them */ +long CLOCK_RES_NS = 0; + int STEP_SEQUENCE[8][4] = { {1, 0, 0, 0}, {1, 1, 0, 0}, {0, 1, 0, 0}, {0, 1, 1, 0}, {0, 0, 1, 0}, {0, 0, 1, 1}, {0, 0, 0, 1}, {1, 0, 0, 1} @@ -39,10 +49,19 @@ void gpio_release(int pin) { void gpio_clean(int error) { for (int i = 0; i < 4; i++) { + if (PAN_FDS[i] != -1) { + close(PAN_FDS[i]); + } + if (TILT_FDS[i] != -1) { + close(TILT_FDS[i]); + } gpio_release(PAN_PINS[i]); gpio_release(TILT_PINS[i]); } + if (SELECT_FD != -1) { + close(SELECT_FD); + } if (SELECT_PIN != -1) { gpio_release(SELECT_PIN); } @@ -76,14 +95,22 @@ void gpio_export(int pin) { } } -void gpio_set(int pin, int value) { +/* the value fds stay open for the whole run: a path lookup plus open/close per + * write costs ~0.5ms on these SoCs, which at 4 writes per micro-step dwarfs + * the step delay itself */ +int gpio_open(int pin) { char path[64]; snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/value", pin); - FILE *file = fopen(path, "w"); - if (file) { - fprintf(file, "%d", value); - fclose(file); - } else { + int fd = open(path, O_WRONLY); + if (fd == -1) { + printf("Unable to open value of GPIO %d: [%d] %s\n", pin, errno, strerror(errno)); + gpio_clean(1); + } + return fd; +} + +void gpio_set(int fd, int pin, int value) { + if (lseek(fd, 0, SEEK_SET) == -1 || write(fd, value ? "1" : "0", 1) != 1) { printf("Unable to set value of GPIO %d: [%d] %s\n", pin, errno, strerror(errno)); gpio_clean(1); } @@ -128,7 +155,48 @@ void gpio_config() { pclose(fp); } -void axis_run(const int pins[4], int level, int steps, int delay) { +/* + * On kernels built without high-resolution timers every sleep rounds up to a + * whole tick, so at HZ=100 usleep(1500) waits ~10ms and 8 micro-steps x 10ms + * put 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). Kernels with hrtimers deliver usleep(1500) in ~1.5ms, and + * sleeping is strictly better there. clock_getres() tells the two apart, so + * sleep whenever the kernel can honour the delay - or when the delay is at + * least a tick, where rounding no longer dominates - and spin on + * CLOCK_MONOTONIC only for sub-tick delays on a coarse-timer kernel. Moves + * are short and bounded, so burning the CPU for their duration is a fair + * trade in that remaining case. + */ +void delay_us(long us) { + if (us <= 0) { + return; + } + + if (CLOCK_RES_NS <= 1000000 || us >= CLOCK_RES_NS / 1000) { + 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], const int fds[4], int level, int steps, int delay) { int remaining = abs(steps); if (remaining == 0) { return; @@ -136,17 +204,17 @@ void axis_run(const int pins[4], int level, int steps, int delay) { const int (*seq)[4] = (steps < 0) ? REVERSE_STEP_SEQUENCE : STEP_SEQUENCE; if (SELECT_PIN != -1) { - gpio_set(SELECT_PIN, level); - usleep(100); + gpio_set(SELECT_FD, SELECT_PIN, level); + delay_us(100); } int micro = 0; while (remaining > 0) { for (int i = 0; i < 4; i++) { - gpio_set(pins[i], seq[micro][i]); + gpio_set(fds[i], pins[i], seq[micro][i]); } - usleep(delay); + delay_us(delay); if (++micro >= 8) { micro = 0; --remaining; @@ -154,7 +222,7 @@ void axis_run(const int pins[4], int level, int steps, int delay) { } for (int i = 0; i < 4; i++) { - gpio_set(pins[i], 0); + gpio_set(fds[i], pins[i], 0); } } @@ -166,7 +234,17 @@ 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 || delay_ms > INT_MAX / 1000) { + fprintf(stderr, "delay must be between 0 and %d ms\n", INT_MAX / 1000); + return 1; + } + int delay = delay_ms * 1000; + + struct timespec res; + if (clock_getres(CLOCK_MONOTONIC, &res) == 0) { + CLOCK_RES_NS = res.tv_sec ? 1000000000L : res.tv_nsec; + } gpio_config(); for (int i = 0; i < 4; i++) { @@ -178,8 +256,17 @@ int main(int argc, char *argv[]) { gpio_export(SELECT_PIN); } - axis_run(PAN_PINS, 0, pan_steps, delay); - axis_run(TILT_PINS, 1, tilt_steps, delay); + for (int i = 0; i < 4; i++) { + PAN_FDS[i] = gpio_open(PAN_PINS[i]); + TILT_FDS[i] = gpio_open(TILT_PINS[i]); + } + + if (SELECT_PIN != -1) { + SELECT_FD = gpio_open(SELECT_PIN); + } + + axis_run(PAN_PINS, PAN_FDS, 0, pan_steps, delay); + axis_run(TILT_PINS, TILT_FDS, 1, tilt_steps, delay); gpio_clean(0); return 0;