limingjie/OmniSocketGo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: GPS采集端修复

Browse Source
This commit is contained in:
nnbcccscdscdsc
2026-04-13 22:33:20 +08:00
parent 2f507a7546
commit 947ecb2a2b
6 changed files with 175 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

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

6
scripts/boot/modem_network_info.json
View File

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

1
scripts/dev/load-env.sh
View File

@@ -127,6 +127,7 @@ export OMNI_CAMERA_PROFILE="${OMNI_CAMERA_PROFILE:-night}"
export OMNI_CAMERA_BRIGHTNESS="${OMNI_CAMERA_BRIGHTNESS:-}" export OMNI_CAMERA_BRIGHTNESS="${OMNI_CAMERA_BRIGHTNESS:-}"
export OMNI_CAMERA_CUSTOM_CTRL="${OMNI_CAMERA_CUSTOM_CTRL:-}" export OMNI_CAMERA_CUSTOM_CTRL="${OMNI_CAMERA_CUSTOM_CTRL:-}"
export OMNI_CAMERA_VERIFY="${OMNI_CAMERA_VERIFY:-0}" 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_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_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:-}}" export OMNI_CONTROL_SERVER_ADDR="${OMNI_CONTROL_SERVER_ADDR:-${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR:-}}"

7
scripts/dev/robot-remote.env
View File

@@ -30,7 +30,7 @@ OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC="3"
FRONTEND_HOST="0.0.0.0" FRONTEND_HOST="0.0.0.0"
FRONTEND_PORT="5173" FRONTEND_PORT="5173"
ROS_DISTRO="jazzy" ROS_DISTRO="humble"
ROBOT_RECEIVER_TRANSPORT="unix_dgram" ROBOT_RECEIVER_TRANSPORT="unix_dgram"
ROBOT_RECEIVER_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}" ROBOT_RECEIVER_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}"
ROBOT_RECEIVER_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA}" 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_PEER_ID="peer-b-video"
OMNI_VIDEO_TARGET_PEER="peer-a-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_BRIGHTNESS=""
OMNI_CAMERA_CUSTOM_CTRL="" OMNI_CAMERA_CUSTOM_CTRL=""
OMNI_CAMERA_VERIFY="0" OMNI_CAMERA_VERIFY="0"

288
src/gps_buffer.c
View File

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

10
src/video_pipeline.c
View File

@@ -745,6 +745,8 @@ int video_pipeline_run(const video_pipeline_config_t *config, video_pipeline_sta
int sws_src_format = -1; int sws_src_format = -1;
uint32_t hard_backpressure_since_ms = 0; uint32_t hard_backpressure_since_ms = 0;
uint32_t last_soft_drop_log_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)); memset(&sender, 0, sizeof(sender));
if (stats == NULL) { 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"); video_pipeline_set_errno_error(stats, "failed to start video sender");
goto cleanup; 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); pthread_mutex_lock(&stats->mutex);
stats->connected = 1; stats->connected = 1;
@@ -1070,6 +1077,9 @@ cleanup:
pthread_mutex_lock(&stats->mutex); pthread_mutex_lock(&stats->mutex);
stats->connected = 0; stats->connected = 0;
pthread_mutex_unlock(&stats->mutex); pthread_mutex_unlock(&stats->mutex);
if (gps_buffer_started) {
gps_buffer_cleanup();
}
if (fd >= 0) { if (fd >= 0) {
(void) ioctl(fd, VIDIOC_STREAMOFF, &type); (void) ioctl(fd, VIDIOC_STREAMOFF, &type);
} }