fix: 对接GPS数据
This commit is contained in:
nnbcccscdscdsc
280
src/gps_buffer.c
Normal file
280
src/gps_buffer.c
Normal file
@@ -0,0 +1,280 @@
|
||||
#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 volatile uint64_t g_current_gps_data = 0;
|
||||
static volatile int g_running = 0;
|
||||
static pthread_t g_gps_thread;
|
||||
|
||||
// 将经纬度打包进 uint64_t
|
||||
static uint64_t pack_gps(double latitude, double longitude) {
|
||||
if (!isfinite(latitude) || !isfinite(longitude)) {
|
||||
return 0;
|
||||
}
|
||||
// 过滤掉 0,0 这种无效坐标
|
||||
if (fabs(latitude) < 1e-6 && fabs(longitude) < 1e-6) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 转换为整数微度 (micro-degrees)
|
||||
int32_t lat_i = (int32_t)(latitude * 1000000.0);
|
||||
int32_t lon_i = (int32_t)(longitude * 1000000.0);
|
||||
|
||||
return ((uint64_t)(uint32_t)lat_i << 32) | (uint64_t)(uint32_t)lon_i;
|
||||
}
|
||||
|
||||
// =================================================================
|
||||
// 以下是借鉴 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++;
|
||||
}
|
||||
|
||||
// 确保当前位置是数字
|
||||
if (!(*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;
|
||||
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) {
|
||||
// 4. 更新全局共享变量 (原子操作)
|
||||
g_current_gps_data = pack_gps(lat, lon);
|
||||
// 调试：取消注释可查看实时经纬度
|
||||
// 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;
|
||||
}
|
||||
|
||||
// =================================================================
|
||||
// 接口函数实现
|
||||
// =================================================================
|
||||
uint64_t get_latest_gps_for_video(void) {
|
||||
return g_current_gps_data;
|
||||
}
|
||||
|
||||
int gps_buffer_init(const char* host) {
|
||||
if (g_running) return 0;
|
||||
|
||||
g_running = 1;
|
||||
g_current_gps_data = 0;
|
||||
pthread_attr_t attr;
|
||||
pthread_attr_init(&attr);
|
||||
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
|
||||
// 创建后台线程
|
||||
if (pthread_create(&g_gps_thread, NULL, 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) 库
|
||||
@@ -621,24 +621,28 @@ static int video_sender_drain_pending_messages(video_sender_t *sender) {
|
||||
}
|
||||
}
|
||||
|
||||
static int video_sender_send_packet(video_sender_t *sender, const AVPacket *encoded_pkt, uint64_t timestamp) {
|
||||
static int video_sender_send_packet(
|
||||
video_sender_t *sender,
|
||||
const AVPacket *encoded_pkt,
|
||||
const video_pipeline_packet_metadata_t *metadata
|
||||
) {
|
||||
uint8_t *payload;
|
||||
size_t payload_len;
|
||||
int rc;
|
||||
|
||||
if (sender == NULL || sender->client == NULL || encoded_pkt == NULL) {
|
||||
if (sender == NULL || sender->client == NULL || encoded_pkt == NULL || metadata == NULL) {
|
||||
errno = EINVAL;
|
||||
return -1;
|
||||
}
|
||||
|
||||
payload_len = (size_t) encoded_pkt->size + sizeof(timestamp);
|
||||
payload_len = (size_t) encoded_pkt->size + sizeof(*metadata);
|
||||
if (video_sender_ensure_buffer_capacity(sender, payload_len) != 0) {
|
||||
return -1;
|
||||
}
|
||||
payload = sender->send_buffer;
|
||||
|
||||
memcpy(payload, encoded_pkt->data, (size_t) encoded_pkt->size);
|
||||
memcpy(payload + encoded_pkt->size, ×tamp, sizeof(timestamp));
|
||||
memcpy(payload + encoded_pkt->size, metadata, sizeof(*metadata));
|
||||
rc = kcp_client_send_binary(sender->client, sender->target_peer, payload, payload_len);
|
||||
if (rc != 0) {
|
||||
return rc;
|
||||
@@ -837,9 +841,11 @@ int video_pipeline_run(const video_pipeline_config_t *config, video_pipeline_sta
|
||||
double encode_end_ms = 0.0;
|
||||
double send_start_ms = 0.0;
|
||||
double send_end_ms = 0.0;
|
||||
video_pipeline_packet_metadata_t packet_metadata;
|
||||
int frame_number = frame_index + 1;
|
||||
|
||||
memset(&transport_stats, 0, sizeof(transport_stats));
|
||||
memset(&packet_metadata, 0, sizeof(packet_metadata));
|
||||
|
||||
if (config->max_frames > 0 && frame_index >= config->max_frames) {
|
||||
break;
|
||||
@@ -924,6 +930,9 @@ int video_pipeline_run(const video_pipeline_config_t *config, video_pipeline_sta
|
||||
send_start_ms = encode_end_ms;
|
||||
}
|
||||
|
||||
packet_metadata.timestamp_ms = (uint64_t) get_realtime_ms();
|
||||
packet_metadata.gps_data = get_latest_gps_for_video();
|
||||
|
||||
kcp_client_runtime_stats_snapshot(sender.client, &transport_stats);
|
||||
if (video_sender_hard_backpressure_active(config, &transport_stats)) {
|
||||
uint32_t now_ms = omni_now_millis32();
|
||||
@@ -1002,7 +1011,7 @@ int video_pipeline_run(const video_pipeline_config_t *config, video_pipeline_sta
|
||||
continue;
|
||||
}
|
||||
|
||||
if (video_sender_send_packet(&sender, encoded_pkt, (uint64_t) get_realtime_ms()) != 0) {
|
||||
if (video_sender_send_packet(&sender, encoded_pkt, &packet_metadata) != 0) {
|
||||
pthread_mutex_lock(&stats->mutex);
|
||||
stats->send_errors += 1;
|
||||
pthread_mutex_unlock(&stats->mutex);
|
||||
|
||||
Reference in New Issue
Block a user