1 files changed, 65 insertions, 0 deletions

diff --git a/serial/usbdev.c b/serial/usbdev.c
new file mode 100644
index 0000000..2a543fa
--- /dev/null
+++ b/serial/usbdev.c
@@ -0,0 +1,65 @@
+#include <dirent.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <termios.h>
+#include <unistd.h>
+
+#define LIDAR_SERIAL "dce74d177fdc724ba5757727edc269bf"
+#define WHEEL_SERIAL "0001"
+
+enum usbDevice { lidar, wheel, other };
+char device_path[256];
+
+int identify_device(const char *port) {
+    char cmd[256];
+    snprintf(cmd, sizeof(cmd), "udevadm info --name=%s | grep 'E: ID_SERIAL='",
+             port);
+    FILE *fp = popen(cmd, "r");
+    if (!fp) {
+        perror("popen");
+        return other;
+    }
+
+    static char serial[256];
+    if (fgets(serial, sizeof(serial), fp) != NULL) {
+        if (strstr(serial, WHEEL_SERIAL)) {
+            pclose(fp);
+            return wheel;
+        } else if (strstr(serial, LIDAR_SERIAL)) {
+            pclose(fp);
+            return lidar;
+        }
+    }
+    pclose(fp);
+    return other;
+}
+
+const char *findUSBDev(const char *device_type) {
+    DIR *dir;
+    struct dirent *entry;
+
+    dir = opendir("/dev");
+    if (dir == NULL) {
+        perror("opendir");
+        return NULL;
+    }
+
+    while ((entry = readdir(dir)) != NULL) {
+        if (strncmp(entry->d_name, "ttyUSB", 6) == 0) {
+            snprintf(device_path, sizeof(device_path), "/dev/%s",
+                     entry->d_name);
+            int dev_type = identify_device(device_path);
+            if ((dev_type == lidar && strcmp(device_type, "lidar") == 0) ||
+                (dev_type == wheel && strcmp(device_type, "wheel") == 0)) {
+                closedir(dir);
+                return device_path;
+            }
+        }
+    }
+
+    closedir(dir);
+    return NULL;
+}