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

288
src/gps_buffer.c
View File

@@ -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;
}
// =================================================================
// 接口函数实现