From 947ecb2a2b2901963146ffdee4c399fae4b8aa1a Mon Sep 17 00:00:00 2001 From: nnbcccscdscdsc <2709767634@qq.com> Date: Mon, 13 Apr 2026 22:33:20 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20GPS=E9=87=87=E9=9B=86=E7=AB=AF=E4=BF=AE?= =?UTF-8?q?=E5=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 1 + scripts/boot/modem_network_info.json | 6 +- scripts/dev/load-env.sh | 1 + scripts/dev/robot-remote.env | 7 +- src/gps_buffer.c | 288 +++++++++++++++------------ src/video_pipeline.c | 10 + 6 files changed, 175 insertions(+), 138 deletions(-) diff --git a/.gitignore b/.gitignore index 440386f..8169457 100644 --- a/.gitignore +++ b/.gitignore @@ -25,3 +25,4 @@ c/bin ros-control-py/install ros-control-py/log +scripts/boot/modem_network_info.json \ No newline at end of file diff --git a/scripts/boot/modem_network_info.json b/scripts/boot/modem_network_info.json index 1b4022e..09739b6 100644 --- a/scripts/boot/modem_network_info.json +++ b/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" ] } \ No newline at end of file diff --git a/scripts/dev/load-env.sh b/scripts/dev/load-env.sh index 2381dd7..1c531ff 100644 --- a/scripts/dev/load-env.sh +++ b/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:-}}" diff --git a/scripts/dev/robot-remote.env b/scripts/dev/robot-remote.env index 12a9c7b..8c40ebb 100644 --- a/scripts/dev/robot-remote.env +++ b/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" diff --git a/src/gps_buffer.c b/src/gps_buffer.c index af36d77..39d2078 100644 --- a/src/gps_buffer.c +++ b/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; +} // ================================================================= // 接口函数实现 diff --git a/src/video_pipeline.c b/src/video_pipeline.c index 1bd6bc2..9dafbea 100644 --- a/src/video_pipeline.c +++ b/src/video_pipeline.c @@ -745,6 +745,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) { @@ -792,6 +794,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; @@ -1070,6 +1077,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); }