OpenIPC
/
firmware
mirror of https://github.com/OpenIPC/firmware.git
1
0
Fork 0
Code Activity

[no ci] Package: serialize gpio-motors on run axis (#1872)

pull/1876/head
viktorxda 2025-08-28 16:12:05 +02:00 committed by GitHub
parent 51eb9bb48c
commit 3059ff2107
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 59 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

153
general/package/gpio-motors/src/gpio-motors.c
View File

@ -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 <pan steps> <tilt steps> <delay (ms)>\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;
}