Merge branch 'c' of https://106.52.207.92:9103/limingjie/OmniSocketGo into c

.gitignore

@@ -25,3 +25,4 @@ c/bin
 
 ros-control-py/install
 ros-control-py/log
+scripts/boot/modem_network_info.json

scripts/boot/modem_network_info.json

@@ -1,9 +1,9 @@
 {
-  "interface": "enx78886c7fbd46",
+  "interface": "enx08711b726c22",
   "ipv4": [
-    "192.168.225.62/22"
+    "192.168.225.66/22"
   ],
   "ipv6": [
-    "fe80::a335:b50d:622d:92e8/64"
+    "fe80::86e0:4771:425d:8b20/64"
   ]
 }

scripts/dev/load-env.sh

@@ -127,6 +127,7 @@ export OMNI_CAMERA_PROFILE="${OMNI_CAMERA_PROFILE:-night}"
 export OMNI_CAMERA_BRIGHTNESS="${OMNI_CAMERA_BRIGHTNESS:-}"
 export OMNI_CAMERA_CUSTOM_CTRL="${OMNI_CAMERA_CUSTOM_CTRL:-}"
 export OMNI_CAMERA_VERIFY="${OMNI_CAMERA_VERIFY:-0}"
+export OMNI_GPSD_HOST="${OMNI_GPSD_HOST:-127.0.0.1}"
 export OMNI_VIDEO_SERVER_ADDR="${OMNI_VIDEO_SERVER_ADDR:-${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR:-}}"
 export OMNI_VIDEO_RELAY_VIA="${OMNI_VIDEO_RELAY_VIA:-${ROBOT_SIDE_OMNISOCKET_RELAY_VIA:-}}"
 export OMNI_CONTROL_SERVER_ADDR="${OMNI_CONTROL_SERVER_ADDR:-${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR:-}}"

scripts/dev/robot-remote.env

@@ -30,7 +30,7 @@ OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC="3"
 FRONTEND_HOST="0.0.0.0"
 FRONTEND_PORT="5173"
 
-ROS_DISTRO="jazzy"
+ROS_DISTRO="humble"
 ROBOT_RECEIVER_TRANSPORT="unix_dgram"
 ROBOT_RECEIVER_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}"
 ROBOT_RECEIVER_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA}"
@@ -44,8 +44,9 @@ ROBOT_RECEIVER_PUBLISH_RATE_HZ="100.0"
 
 OMNI_VIDEO_PEER_ID="peer-b-video"
 OMNI_VIDEO_TARGET_PEER="peer-a-video"
-OMNI_CAMERA_DEVICE="/dev/video26"
+OMNI_GPSD_HOST="127.0.0.1"
-OMNI_CAMERA_PROFILE="night"
+OMNI_CAMERA_DEVICE="/dev/video0"
+OMNI_CAMERA_PROFILE="day"
 OMNI_CAMERA_BRIGHTNESS=""
 OMNI_CAMERA_CUSTOM_CTRL=""
 OMNI_CAMERA_VERIFY="0"

src/gps_buffer.c

@@ -42,8 +42,19 @@ static gps_video_sample_t load_gps(void) {
     return sample;
 }
 
-// 将经纬度规范化为 float，保留 6 位小数。
+static void gps_sleep_before_retry(void) {
-static int normalize_gps(float latitude, float longitude, gps_video_sample_t* sample) {
+    int retry_ms = 1000;
+    int step_ms = 100;
+    int elapsed_ms = 0;
+
+    while (g_running && elapsed_ms < retry_ms) {
+        usleep((useconds_t) step_ms * 1000U);
+        elapsed_ms += step_ms;
+    }
+}
+
+// 将经纬度规范化为 double，保留 6 位小数。
+static int normalize_gps(double latitude, double longitude, gps_video_sample_t* sample) {
     if (!isfinite(latitude) || !isfinite(longitude)) {
         return -1;
     }
@@ -150,76 +161,89 @@ static int is_tpv_class(const char* json) {
 // =================================================================
 void* gps_update_thread(void* arg) {
     const char* host = (const char*)arg;
-    int sockfd;
+    const char* gpsd_host = (host != NULL && host[0] != '\0') ? host : "127.0.0.1";
-    struct addrinfo hints, *res, *rp;
+
+    while (g_running) {
+        int sockfd = -1;
+        struct addrinfo hints;
+        struct addrinfo *res = NULL;
+        struct addrinfo *rp = NULL;
         int s;
+        char buffer[4096];
+        size_t offset = 0;
 
         // 1. 解析地址并连接 gpsd (默认端口 2947)
         memset(&hints, 0, sizeof(hints));
         hints.ai_family = AF_UNSPEC;    // 兼容 IPv4/IPv6
         hints.ai_socktype = SOCK_STREAM;
 
-    s = getaddrinfo(host, "2947", &hints, &res);
+        s = getaddrinfo(gpsd_host, "2947", &hints, &res);
         if (s != 0) {
-        fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(s));
+            fprintf(stderr, "GPS线程: 解析 gpsd 地址失败 %s:2947: %s\n", gpsd_host, gai_strerror(s));
-        return NULL;
+            gps_sleep_before_retry();
+            continue;
         }
 
         // 尝试连接每一个解析出来的地址
         for (rp = res; rp != NULL; rp = rp->ai_next) {
             sockfd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
-        if (sockfd == -1) continue;
+            if (sockfd == -1) {
+                continue;
+            }
 
             if (connect(sockfd, rp->ai_addr, rp->ai_addrlen) != -1) {
-            break; // 成功连接
+                break;
             }
             close(sockfd);
+            sockfd = -1;
         }
 
-    if (rp == NULL) { // 没有连接成功
-        fprintf(stderr, "无法连接到 %s:2947\n", host);
-        freeaddrinfo(res);
-        return NULL;
-    }
         freeaddrinfo(res);
 
-    printf("GPS线程: 已连接到 gpsd %s\n", host);
+        if (sockfd < 0) {
+            fprintf(stderr, "GPS线程: 无法连接到 %s:2947，1 秒后重试\n", gpsd_host);
+            gps_sleep_before_retry();
+            continue;
+        }
+
+        printf("GPS线程: 已连接到 gpsd %s\n", gpsd_host);
 
         // 2. 发送 WATCH 命令，开启 JSON 流
+        {
             const char* watch_cmd = "?WATCH={\"enable\":true,\"json\":true};\n";
+
             if (send(sockfd, watch_cmd, strlen(watch_cmd), 0) < 0) {
-        perror("发送 WATCH 命令失败");
+                perror("GPS线程: 发送 WATCH 命令失败");
                 close(sockfd);
-        return NULL;
+                gps_sleep_before_retry();
+                continue;
+            }
         }
 
         // 3. 主循环：读取并解析数据流
         // 注意：gpsd 数据是以 \n 结尾的，不能直接用固定长度 recv
-    char buffer[4096]; // 增大缓冲区以容纳长 JSON
-    size_t offset = 0; // 当前缓冲区数据长度
-
         while (g_running) {
             ssize_t len = recv(sockfd, buffer + offset, sizeof(buffer) - 1 - offset, 0);
 
             if (len <= 0) {
-            // 连接断开，进行重连逻辑
                 break;
             }
 
-        offset += len;
+            offset += (size_t) len;
             buffer[offset] = '\0'; // 确保字符串结束
 
             // 查找换行符 \n，因为一条完整的 JSON 消息以 \n 结尾
             char* start = buffer;
             char* end;
 
-        while ((end = memchr(start, '\n', buffer + offset - start)) != NULL) {
+            while ((end = memchr(start, '\n', (buffer + offset) - start)) != NULL) {
                 *end = '\0'; // 临时截断，形成独立字符串
 
                 // --- 核心解析逻辑 ---
                 // 1. 检查是否为 TPV 数据包
                 if (is_tpv_class(start)) {
-                double lat = 0.0, lon = 0.0;
+                    double lat = 0.0;
+                    double lon = 0.0;
                     int mode = 0;
                     int has_fix = 0;
 
@@ -253,9 +277,8 @@ void* gps_update_thread(void* arg) {
             }
 
             // 处理完所有完整消息后，将剩余未处理的数据移到缓冲区头部
-        // (这种情况很少见，但为了严谨性)
             if (start < buffer + offset) {
-            size_t remaining = (buffer + offset) - start;
+                size_t remaining = (size_t) ((buffer + offset) - start);
                 memmove(buffer, start, remaining);
                 offset = remaining;
             } else {
@@ -263,12 +286,13 @@ void* gps_update_thread(void* arg) {
             }
         }
 
-    // 循环结束，清理资源
         close(sockfd);
-    printf("GPS线程: 连接断开，尝试重连...\n");
+        if (g_running) {
+            fprintf(stderr, "GPS线程: 连接断开，1 秒后重连...\n");
+            gps_sleep_before_retry();
+        }
+    }
 
-    // 这里可以添加一个短暂的休眠防止重连风暴
-    // 但通常主程序会重启线程
     return NULL;
 }
 

src/video_pipeline.c

@@ -752,6 +752,8 @@ int video_pipeline_run(const video_pipeline_config_t *config, video_pipeline_sta
     int sws_src_format = -1;
     uint32_t hard_backpressure_since_ms = 0;
     uint32_t last_soft_drop_log_ms = 0;
+    const char *gpsd_host = env_or_default("OMNI_GPSD_HOST", "127.0.0.1");
+    int gps_buffer_started = 0;
 
     memset(&sender, 0, sizeof(sender));
     if (stats == NULL) {
@@ -799,6 +801,11 @@ int video_pipeline_run(const video_pipeline_config_t *config, video_pipeline_sta
         video_pipeline_set_errno_error(stats, "failed to start video sender");
         goto cleanup;
     }
+    if (gps_buffer_init(gpsd_host) != 0) {
+        fprintf(stderr, "[video_pipeline] failed to start GPS buffer using %s:2947\n", gpsd_host);
+    } else {
+        gps_buffer_started = 1;
+    }
 
     pthread_mutex_lock(&stats->mutex);
     stats->connected = 1;
@@ -1078,6 +1085,9 @@ cleanup:
     pthread_mutex_lock(&stats->mutex);
     stats->connected = 0;
     pthread_mutex_unlock(&stats->mutex);
+    if (gps_buffer_started) {
+        gps_buffer_cleanup();
+    }
     if (fd >= 0) {
         (void) ioctl(fd, VIDIOC_STREAMOFF, &type);
     }