From 3059ff2107fcc5196a8e3b9d03442c6ec928d663 Mon Sep 17 00:00:00 2001 From: viktorxda <35473052+viktorxda@users.noreply.github.com> Date: Thu, 28 Aug 2025 16:12:05 +0200 Subject: [PATCH] [no ci] Package: serialize gpio-motors on run axis (#1872) --- general/package/gpio-motors/src/gpio-motors.c | 153 +++++++----------- 1 file changed, 59 insertions(+), 94 deletions(-) diff --git a/general/package/gpio-motors/src/gpio-motors.c b/general/package/gpio-motors/src/gpio-motors.c index 142c3d09..9a4e3bc3 100644 --- a/general/package/gpio-motors/src/gpio-motors.c +++ b/general/package/gpio-motors/src/gpio-motors.c @@ -19,8 +19,8 @@ int REVERSE_STEP_SEQUENCE[8][4] = { {0, 1, 1, 0}, {0, 1, 0, 0}, {1, 1, 0, 0}, {1, 0, 0, 0} }; -void release_gpio(int pin) { - char path[50]; +void gpio_release(int pin) { + char path[64]; snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/value", pin); FILE *file = fopen(path, "w"); if (file) { @@ -37,21 +37,23 @@ void release_gpio(int pin) { } } -void cleanup() { +void gpio_clean(int error) { for (int i = 0; i < 4; i++) { - release_gpio(PAN_PINS[i]); - release_gpio(TILT_PINS[i]); + gpio_release(PAN_PINS[i]); + gpio_release(TILT_PINS[i]); } if (SELECT_PIN != -1) { - release_gpio(SELECT_PIN); + gpio_release(SELECT_PIN); } - exit(EXIT_FAILURE); + if (error) { + exit(EXIT_FAILURE); + } } -void export_gpio(int pin) { - char path[50]; +void gpio_export(int pin) { + char path[64]; FILE *file; file = fopen("/sys/class/gpio/export", "w"); @@ -60,7 +62,7 @@ void export_gpio(int pin) { fclose(file); } else { printf("Unable export GPIO %d: [%d] %s\n", pin, errno, strerror(errno)); - cleanup(); + gpio_clean(1); } snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/direction", pin); @@ -70,23 +72,12 @@ void export_gpio(int pin) { fclose(file); } else { printf("Unable to set direction of GPIO %d: [%d] %s\n", pin, errno, strerror(errno)); - cleanup(); + gpio_clean(1); } } -void unexport_gpio(int pin) { - FILE *file = fopen("/sys/class/gpio/unexport", "w"); - if (file) { - fprintf(file, "%d", pin); - fclose(file); - } else { - printf("Unable to unexport GPIO %d: [%d] %s\n", pin, errno, strerror(errno)); - cleanup(); - } -} - -void set_gpio(int pin, int value) { - char path[50]; +void gpio_set(int pin, int value) { + char path[64]; snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/value", pin); FILE *file = fopen(path, "w"); if (file) { @@ -94,11 +85,11 @@ void set_gpio(int pin, int value) { fclose(file); } else { printf("Unable to set value of GPIO %d: [%d] %s\n", pin, errno, strerror(errno)); - cleanup(); + gpio_clean(1); } } -void get_gpio_config() { +void gpio_config() { FILE *fp = popen("fw_printenv -n gpio_motors", "r"); if (fp == NULL) { printf("Unable to run fw_printenv\n"); @@ -116,17 +107,15 @@ void get_gpio_config() { token = strtok(NULL, " "); } - if (count == 8) { + if (count == 8 || count == 5) { for (int i = 0; i < 4; i++) { PAN_PINS[i] = value[i]; - TILT_PINS[i] = value[i + 4]; + TILT_PINS[i] = value[i + (count == 8 ? 4 : 0)]; } - } else if (count == 5) { - for (int i = 0; i < 4; i++) { - PAN_PINS[i] = value[i]; - TILT_PINS[i] = value[i]; + + if (count == 5) { + SELECT_PIN = value[4]; } - SELECT_PIN = value[4]; } else { printf("Error: Expected 8 or 5 GPIO values, but got %d\n", count); exit(EXIT_FAILURE); @@ -139,6 +128,36 @@ void get_gpio_config() { pclose(fp); } +void axis_run(const int pins[4], int level, int steps, int delay) { + int remaining = abs(steps); + if (remaining == 0) { + return; + } + + const int (*seq)[4] = (steps < 0) ? REVERSE_STEP_SEQUENCE : STEP_SEQUENCE; + if (SELECT_PIN != -1) { + gpio_set(SELECT_PIN, level); + usleep(100); + } + + int micro = 0; + while (remaining > 0) { + for (int i = 0; i < 4; i++) { + gpio_set(pins[i], seq[micro][i]); + } + + usleep(delay); + if (++micro >= 8) { + micro = 0; + --remaining; + } + } + + for (int i = 0; i < 4; i++) { + gpio_set(pins[i], 0); + } +} + int main(int argc, char *argv[]) { if (argc != 4) { fprintf(stderr, "Usage: %s \n", argv[0]); @@ -149,73 +168,19 @@ int main(int argc, char *argv[]) { int tilt_steps = atoi(argv[2]); int delay = atoi(argv[3]) * 1000; - get_gpio_config(); - + gpio_config(); for (int i = 0; i < 4; i++) { - export_gpio(PAN_PINS[i]); - export_gpio(TILT_PINS[i]); + gpio_export(PAN_PINS[i]); + gpio_export(TILT_PINS[i]); } if (SELECT_PIN != -1) { - export_gpio(SELECT_PIN); + gpio_export(SELECT_PIN); } - int pan_remaining = abs(pan_steps); - int tilt_remaining = abs(tilt_steps); - int pan_reverse = (pan_steps < 0); - int tilt_reverse = (tilt_steps < 0); - int pan_micro = 0; - int tilt_micro = 0; - - while (pan_remaining > 0 || tilt_remaining > 0) { - int pan_has = pan_remaining > 0; - int tilt_has = tilt_remaining > 0; - - int pan_eff = pan_has ? (tilt_has ? delay : delay / 2) : 0; - int tilt_eff = tilt_has ? (pan_has ? delay : delay / 2) : 0; - int eff_delay = (pan_eff > tilt_eff) ? pan_eff : tilt_eff; - - if (pan_has) { - if (SELECT_PIN != -1) { - set_gpio(SELECT_PIN, 0); - } - - const int (*seq)[4] = pan_reverse ? REVERSE_STEP_SEQUENCE : STEP_SEQUENCE; - for (int k = 0; k < 4; k++) { - set_gpio(PAN_PINS[k], seq[pan_micro][k]); - } - if (++pan_micro >= 8) { - pan_micro = 0; - pan_remaining--; - } - } - - if (tilt_has) { - if (SELECT_PIN != -1) { - set_gpio(SELECT_PIN, 1); - } - - const int (*seq)[4] = tilt_reverse ? REVERSE_STEP_SEQUENCE : STEP_SEQUENCE; - for (int k = 0; k < 4; k++) { - set_gpio(TILT_PINS[k], seq[tilt_micro][k]); - } - if (++tilt_micro >= 8) { - tilt_micro = 0; - tilt_remaining--; - } - } - - usleep(eff_delay); - } - - for (int i = 0; i < 4; i++) { - release_gpio(PAN_PINS[i]); - release_gpio(TILT_PINS[i]); - } - - if (SELECT_PIN != -1) { - release_gpio(SELECT_PIN); - } + axis_run(PAN_PINS, 0, pan_steps, delay); + axis_run(TILT_PINS, 1, tilt_steps, delay); + gpio_clean(0); return 0; }