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_CAMERA_PROFILE="night"
+OMNI_GPSD_HOST="127.0.0.1"
+OMNI_CAMERA_DEVICE="/dev/video0"
+OMNI_CAMERA_PROFILE="day"
 OMNI_CAMERA_BRIGHTNESS=""
 OMNI_CAMERA_CUSTOM_CTRL=""
 OMNI_CAMERA_VERIFY="0"

src/gps_buffer.c

@@ -33,20 +33,31 @@ static void clear_gps(void) {
     pthread_mutex_unlock(&g_gps_mutex);
 }
 
-static gps_video_sample_t load_gps(void) {
-    gps_video_sample_t sample;
+static gps_video_sample_t load_gps(void) {
+    gps_video_sample_t sample;
 
     pthread_mutex_lock(&g_gps_mutex);
     sample = g_current_gps_data;
     pthread_mutex_unlock(&g_gps_mutex);
     return sample;
-}
-
-// 将经纬度规范化为 float，保留 6 位小数。
-static int normalize_gps(float latitude, float longitude, gps_video_sample_t* sample) {
-    if (!isfinite(latitude) || !isfinite(longitude)) {
-        return -1;
-    }
+}
+
+static void gps_sleep_before_retry(void) {
+    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;
+    }
     // 过滤掉 0,0 这种无效坐标
     if (fabs(latitude) < 1e-6 && fabs(longitude) < 1e-6) {
         return -1;
@@ -148,129 +159,142 @@ static int is_tpv_class(const char* json) {
 // =================================================================
 // 后台线程函数：负责连接 gpsd 并更新全局变量
 // =================================================================
-void* gps_update_thread(void* arg) {
-    const char* host = (const char*)arg;
-    int sockfd;
-    struct addrinfo hints, *res, *rp;
-    int s;
-    
-    // 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);
-    if (s != 0) {
-        fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(s));
-        return NULL;
-    }
-
-    // 尝试连接每一个解析出来的地址
-    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 (connect(sockfd, rp->ai_addr, rp->ai_addrlen) != -1) {
-            break; // 成功连接
-        }
-        close(sockfd);
-    }
-
-    if (rp == NULL) { // 没有连接成功
-        fprintf(stderr, "无法连接到 %s:2947\n", host);
-        freeaddrinfo(res);
-        return NULL;
-    }
-    freeaddrinfo(res);
-
-    printf("GPS线程: 已连接到 gpsd %s\n", 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 命令失败");
-        close(sockfd);
-        return NULL;
-    }
-
-    // 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;
-        buffer[offset] = '\0'; // 确保字符串结束
-
-        // 查找换行符 \n，因为一条完整的 JSON 消息以 \n 结尾
-        char* start = buffer;
-        char* end;
-        
-        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;
-                int mode = 0;
-                int has_fix = 0;
-
-                // 2. 提取定位模式 (mode: 1=无定位, 2=2D, 3=3D)
-                if (json_extract_int(start, "mode", &mode)) {
-                    has_fix = (mode >= 2); 
-                }
-
-                // 3. 如果有定位，提取经纬度
-                if (has_fix) {
-                    int got_lat = json_extract_double(start, "lat", &lat);
-                    int got_lon = json_extract_double(start, "lon", &lon);
-                    
-                    if (got_lat && got_lon) {
-                        gps_video_sample_t sample;
-
-                        // 4. 更新全局共享变量，使用 double 直接携带经纬度。
-                        if (normalize_gps(lat, lon, &sample) == 0) {
-                            store_gps(sample.latitude, sample.longitude);
-                        }
-                        // 调试：取消注释可查看实时经纬度
-                        // printf("更新GPS: lat=%.6f, lon=%.6f\n", lat, lon);
-                    }
-                }
-                // 如果无定位，这里不操作，保持上一次的有效值
-            }
-            // --- 解析结束 ---
-
-            // 移动指针到下一条消息
-            start = end + 1;
-        }
-
-        // 处理完所有完整消息后，将剩余未处理的数据移到缓冲区头部
-        // (这种情况很少见，但为了严谨性)
-        if (start < buffer + offset) {
-            size_t remaining = (buffer + offset) - start;
-            memmove(buffer, start, remaining);
-            offset = remaining;
-        } else {
-            offset = 0; // 缓冲区已清空
-        }
-    }
-
-    // 循环结束，清理资源
-    close(sockfd);
-    printf("GPS线程: 连接断开，尝试重连...\n");
-    
-    // 这里可以添加一个短暂的休眠防止重连风暴
-    // 但通常主程序会重启线程
-    return NULL;
-}
+void* gps_update_thread(void* arg) {
+    const char* host = (const char*)arg;
+    const char* gpsd_host = (host != NULL && host[0] != '\0') ? host : "127.0.0.1";
+
+    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(gpsd_host, "2947", &hints, &res);
+        if (s != 0) {
+            fprintf(stderr, "GPS线程: 解析 gpsd 地址失败 %s:2947: %s\n", gpsd_host, gai_strerror(s));
+            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 (connect(sockfd, rp->ai_addr, rp->ai_addrlen) != -1) {
+                break;
+            }
+            close(sockfd);
+            sockfd = -1;
+        }
+
+        freeaddrinfo(res);
+
+        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("GPS线程: 发送 WATCH 命令失败");
+                close(sockfd);
+                gps_sleep_before_retry();
+                continue;
+            }
+        }
+
+        // 3. 主循环：读取并解析数据流
+        // 注意：gpsd 数据是以 \n 结尾的，不能直接用固定长度 recv
+        while (g_running) {
+            ssize_t len = recv(sockfd, buffer + offset, sizeof(buffer) - 1 - offset, 0);
+
+            if (len <= 0) {
+                break;
+            }
+
+            offset += (size_t) len;
+            buffer[offset] = '\0'; // 确保字符串结束
+
+            // 查找换行符 \n，因为一条完整的 JSON 消息以 \n 结尾
+            char* start = buffer;
+            char* end;
+
+            while ((end = memchr(start, '\n', (buffer + offset) - start)) != NULL) {
+                *end = '\0'; // 临时截断，形成独立字符串
+
+                // --- 核心解析逻辑 ---
+                // 1. 检查是否为 TPV 数据包
+                if (is_tpv_class(start)) {
+                    double lat = 0.0;
+                    double lon = 0.0;
+                    int mode = 0;
+                    int has_fix = 0;
+
+                    // 2. 提取定位模式 (mode: 1=无定位, 2=2D, 3=3D)
+                    if (json_extract_int(start, "mode", &mode)) {
+                        has_fix = (mode >= 2);
+                    }
+
+                    // 3. 如果有定位，提取经纬度
+                    if (has_fix) {
+                        int got_lat = json_extract_double(start, "lat", &lat);
+                        int got_lon = json_extract_double(start, "lon", &lon);
+
+                        if (got_lat && got_lon) {
+                            gps_video_sample_t sample;
+
+                            // 4. 更新全局共享变量，使用 double 直接携带经纬度。
+                            if (normalize_gps(lat, lon, &sample) == 0) {
+                                store_gps(sample.latitude, sample.longitude);
+                            }
+                            // 调试：取消注释可查看实时经纬度
+                            // printf("更新GPS: lat=%.6f, lon=%.6f\n", lat, lon);
+                        }
+                    }
+                    // 如果无定位，这里不操作，保持上一次的有效值
+                }
+                // --- 解析结束 ---
+
+                // 移动指针到下一条消息
+                start = end + 1;
+            }
+
+            // 处理完所有完整消息后，将剩余未处理的数据移到缓冲区头部
+            if (start < buffer + offset) {
+                size_t remaining = (size_t) ((buffer + offset) - start);
+                memmove(buffer, start, remaining);
+                offset = remaining;
+            } else {
+                offset = 0; // 缓冲区已清空
+            }
+        }
+
+        close(sockfd);
+        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);
     }