334 lines
10 KiB
C
334 lines
10 KiB
C
#include "gps_buffer.h"
|
||
#include <pthread.h>
|
||
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include <unistd.h>
|
||
#include <sys/socket.h>
|
||
#include <netdb.h>
|
||
#include <math.h>
|
||
#include <errno.h> // 确保包含 errno
|
||
|
||
// 全局共享变量
|
||
static gps_video_sample_t g_current_gps_data = {0.0, 0.0};
|
||
static volatile int g_running = 0;
|
||
static pthread_t g_gps_thread;
|
||
static pthread_mutex_t g_gps_mutex = PTHREAD_MUTEX_INITIALIZER;
|
||
|
||
static double normalize_coordinate(double coordinate) {
|
||
return round(coordinate * 1000000.0) / 1000000.0;
|
||
}
|
||
|
||
static void store_gps(double latitude, double longitude) {
|
||
pthread_mutex_lock(&g_gps_mutex);
|
||
g_current_gps_data.latitude = normalize_coordinate(latitude);
|
||
g_current_gps_data.longitude = normalize_coordinate(longitude);
|
||
pthread_mutex_unlock(&g_gps_mutex);
|
||
}
|
||
|
||
static void clear_gps(void) {
|
||
pthread_mutex_lock(&g_gps_mutex);
|
||
g_current_gps_data.latitude = 0.0;
|
||
g_current_gps_data.longitude = 0.0;
|
||
pthread_mutex_unlock(&g_gps_mutex);
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
if (sample == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
sample->latitude = normalize_coordinate(latitude);
|
||
sample->longitude = normalize_coordinate(longitude);
|
||
return 0;
|
||
}
|
||
|
||
// =================================================================
|
||
// 以下是借鉴 gps_parse.c 实现的底层解析函数
|
||
// =================================================================
|
||
|
||
// 1. 辅助函数：在 JSON 字符串中查找键对应的值的起始位置
|
||
static const char* find_json_value(const char* json, const char* key) {
|
||
char pattern[64];
|
||
int written;
|
||
const char* position;
|
||
|
||
if (json == NULL || key == NULL) return NULL;
|
||
|
||
// 构建搜索模式: "key":
|
||
written = snprintf(pattern, sizeof(pattern), "\"%s\":", key);
|
||
if (written < 0 || (size_t)written >= sizeof(pattern)) {
|
||
return NULL;
|
||
}
|
||
|
||
position = strstr(json, pattern);
|
||
if (position == NULL) {
|
||
return NULL;
|
||
}
|
||
|
||
// 跳过 "key":
|
||
position += written;
|
||
|
||
// 跳过可能存在的空格
|
||
while (*position == ' ' || *position == '\t') {
|
||
position++;
|
||
}
|
||
|
||
return position;
|
||
}
|
||
|
||
// 2. 解析函数：从 JSON 字符串中提取 Double 类型的值
|
||
static int json_extract_double(const char* json, const char* key, double* value) {
|
||
const char* position;
|
||
char* endptr = NULL;
|
||
double parsed;
|
||
|
||
position = find_json_value(json, key);
|
||
if (position == NULL) {
|
||
return 0; // 键不存在
|
||
}
|
||
|
||
// 确保当前位置是数字或负号
|
||
if (*position != '-' && !(*position >= '0' && *position <= '9')) {
|
||
return 0;
|
||
}
|
||
|
||
// 重置 errno 以检测错误
|
||
errno = 0;
|
||
parsed = strtod(position, &endptr);
|
||
|
||
// 检查转换是否成功
|
||
if (errno != 0 || endptr == position || !isfinite(parsed)) {
|
||
return 0;
|
||
}
|
||
|
||
*value = parsed;
|
||
return 1;
|
||
}
|
||
|
||
// 3. 解析函数：从 JSON 字符串中提取 Int 类型的值
|
||
static int json_extract_int(const char* json, const char* key, int* value) {
|
||
double dval;
|
||
if (json_extract_double(json, key, &dval)) {
|
||
*value = (int)dval;
|
||
return 1;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
// 4. 检查是否为 TPV (定位数据) 包
|
||
static int is_tpv_class(const char* json) {
|
||
char class_buf[32] = {0};
|
||
const char* pos = find_json_value(json, "class");
|
||
if (pos == NULL || *pos != '"') return 0;
|
||
|
||
// 简单提取 class 的值 (TPV/SKY/DEVICES)
|
||
sscanf(pos, "\"%31[^\"]\"", class_buf);
|
||
return (strcmp(class_buf, "TPV") == 0);
|
||
}
|
||
|
||
// =================================================================
|
||
// 后台线程函数：负责连接 gpsd 并更新全局变量
|
||
// =================================================================
|
||
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;
|
||
}
|
||
|
||
// =================================================================
|
||
// 接口函数实现
|
||
// =================================================================
|
||
gps_video_sample_t get_latest_gps_for_video(void) {
|
||
return load_gps();
|
||
}
|
||
|
||
int gps_buffer_init(const char* host) {
|
||
if (g_running) return 0;
|
||
|
||
g_running = 1;
|
||
clear_gps();
|
||
pthread_attr_t attr;
|
||
pthread_attr_init(&attr);
|
||
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
|
||
// 创建后台线程
|
||
if (pthread_create(&g_gps_thread, &attr, gps_update_thread, (void*)host) != 0) {
|
||
g_running = 0;
|
||
pthread_attr_destroy(&attr); // 清理属性
|
||
perror("无法创建 GPS 线程");
|
||
return -1;
|
||
}
|
||
pthread_attr_destroy(&attr); // 清理属性
|
||
return 0;
|
||
}
|
||
|
||
void gps_buffer_cleanup(void) {
|
||
g_running = 0;
|
||
// 等待线程结束
|
||
|
||
usleep(10000); // 等待 100ms 让后台线程有机会处理退出标志
|
||
}
|
||
|
||
|
||
//gcc main.c video_pipeline_run.c gps_buffer.c -lpthread -lm -o my_app 请确保在编译命令中链接 pthread 和 m (math) 库
|