T536_DTU/app_public/fuxi_public/fuxi_bsp/source/pps_sync.c

/**
 * @file pps_sync.c
 * @author zhl
 * @brief PPS
 * @version 0.1
 * @date 2025-11-25
 * 
 * @copyright Copyright (c) 2025
 * 
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sched.h>
#include <pthread.h>
#include <time.h>
#include <string.h>
#include <errno.h>
#include <sys/time.h>
#include <sys/timex.h>
#include <stdint.h>
#include <sys/prctl.h>

#include <gps.h>

// ==================== 配置宏定义 ====================
#define DEBUG_PRINT 0
#define PPS_DETAIL_PRINT 0
#define SYNC_DETAIL_PRINT 0
#define STATS_PRINT 0

// 超时和重试配置
#define PPS_READ_TIMEOUT_MS 1200 // PPS读取超时(毫秒)
#define MAX_CONSECUTIVE_ERRORS 5 // 最大连续错误次数
#define SIGNAL_LOST_THRESHOLD 10 // 信号丢失阈值(秒)

// 打印控制宏
#if DEBUG_PRINT
#define DBG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
#define DBG_PPS(fmt, ...) \
    if (PPS_DETAIL_PRINT) \
    printf(fmt, ##__VA_ARGS__)
#define DBG_SYNC(fmt, ...) \
    if (SYNC_DETAIL_PRINT) \
    printf(fmt, ##__VA_ARGS__)
#define DBG_STATS(fmt, ...) \
    if (STATS_PRINT)        \
    printf(fmt, ##__VA_ARGS__)
#define DBG_WARN(fmt, ...) printf("警告: " fmt, ##__VA_ARGS__)
#define DBG_ERROR(fmt, ...) printf("错误: " fmt, ##__VA_ARGS__)
#else
#define DBG_PRINT(fmt, ...) \
    do                      \
    {                       \
    } while (0)
#define DBG_PPS(fmt, ...) \
    do                    \
    {                     \
    } while (0)
#define DBG_SYNC(fmt, ...) \
    do                     \
    {                      \
    } while (0)
#define DBG_STATS(fmt, ...) \
    do                      \
    {                       \
    } while (0)
#define DBG_WARN(fmt, ...) \
    do                     \
    {                      \
    } while (0)
#define DBG_ERROR(fmt, ...) \
    do                      \
    {                       \
    } while (0)
#endif

static volatile sig_atomic_t running = 1;

// 系统状态枚举
typedef enum
{
    STATE_NORMAL = 0,  // 正常状态
    STATE_SIGNAL_WEAK, // 信号弱
    STATE_SIGNAL_LOST, // 信号丢失
    STATE_RECOVERING,  // 恢复中
    STATE_ERROR        // 错误状态
} system_state_t;

// 性能基准和状态管理
struct performance_benchmark
{

    // 计数
    int pps_count;

    uint64_t last_pps_time;
    uint64_t last_success_time;

    // 异常处理
    system_state_t system_state;
    int consecutive_errors;
    int signal_lost_count;
    uint64_t last_state_change;
};

volatile int64_t shared_gps_time = 0;
volatile int gps_time_valid = 0;

void signal_handler(int sig)
{
    running = 0;
    DBG_PRINT("收到停止信号，正在退出...\n");
}

// 高精度时间获取
static inline uint64_t get_time_ns(void)
{
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return (uint64_t)ts.tv_sec * 1000000000ULL + (uint64_t)ts.tv_nsec;
}

// 显示系统时间
void display_system_time(const char *context)
{
    struct timespec ts;
    struct tm tm_info;
    char time_str[64];

    clock_gettime(CLOCK_REALTIME, &ts);
    localtime_r(&ts.tv_sec, &tm_info);

    strftime(time_str, sizeof(time_str), "%Y-%m-%d %H:%M:%S", &tm_info);
    DBG_PRINT("%s: %s.%09ld\n", context, time_str, ts.tv_nsec);
}

// 状态管理函数
void change_system_state(struct performance_benchmark *bench, system_state_t new_state)
{
    if (bench->system_state != new_state)
    {
        const char *state_names[] = {"正常", "信号弱", "信号丢失", "恢复中", "错误"};
        DBG_WARN("系统状态变更: %s -> %s\n",
                 state_names[bench->system_state], state_names[new_state]);

        bench->system_state = new_state;
        bench->last_state_change = get_time_ns();
    }
}

// 检查信号状态
void check_signal_status(struct performance_benchmark *bench)
{
    uint64_t current_time = get_time_ns();
    uint64_t time_since_last_pps = (current_time - bench->last_pps_time) / 1000000000ULL; // 转换为秒

    if (time_since_last_pps > SIGNAL_LOST_THRESHOLD)
    {
        if (bench->system_state != STATE_SIGNAL_LOST)
        {
            change_system_state(bench, STATE_SIGNAL_LOST);
            bench->signal_lost_count++;
            DBG_WARN("GPS信号丢失! 最后PPS: %lld秒前\n", (long long)time_since_last_pps);
        }
    }
    else if (time_since_last_pps > 3)
    {
        if (bench->system_state == STATE_NORMAL)
        {
            change_system_state(bench, STATE_SIGNAL_WEAK);
            DBG_WARN("GPS信号弱，最后PPS: %lld秒前\n", (long long)time_since_last_pps);
        }
    }
    else
    {
        if (bench->system_state != STATE_NORMAL)
        {
            if (bench->system_state == STATE_SIGNAL_LOST)
            {
                DBG_PRINT("GPS信号恢复!\n");
            }
            change_system_state(bench, STATE_NORMAL);
            bench->consecutive_errors = 0;
        }
    }
}

// 带超时的PPS读取
ssize_t read_pps_with_timeout(int fd, void *buf, size_t count, int timeout_ms)
{
    fd_set readfds;
    struct timeval tv;
    int ret;

    FD_ZERO(&readfds);
    FD_SET(fd, &readfds);

    tv.tv_sec = timeout_ms / 1000;
    tv.tv_usec = (timeout_ms % 1000) * 1000;

    ret = select(fd + 1, &readfds, NULL, NULL, &tv);

    if (ret > 0)
    {
        // 有数据可读
        if (FD_ISSET(fd, &readfds))
        {
            return read(fd, buf, count);
        }
    }
    else if (ret == 0)
    {
        // 超时
        errno = ETIMEDOUT;
        return -1;
    }

    // 错误
    return -1;
}

// 考虑时钟偏差的亚秒级时间微调
int calibrated_subsecond_adjust(uint64_t pps_ns, struct performance_benchmark *bench)
{
    // 在信号弱或丢失状态下不进行时间调整
    if (bench->system_state != STATE_NORMAL)
    {
        DBG_WARN("信号状态不佳，跳过时间调整\n");
        return -1;
    }

    struct timespec current_ts, new_ts;
    int ret;
    bool bset = false;
    const int correction_value_ns = 500000; // 0.5ms

    if (gps_time_valid == 1)
    {
        new_ts.tv_sec = shared_gps_time + 1; // TODO，是否得加一秒
        new_ts.tv_nsec = 0;
        gps_time_valid = 0;
    }
    else
    {
        return -1;
    }

    // clock_gettime(CLOCK_REALTIME, &current_ts); //这个是改为获取GPS本身时间 zhl
    // DBG_SYNC("当前时间 clock_gettime: %ld.%09ld\n", current_ts.tv_sec, current_ts.tv_nsec);
    // ret = clock_settime(CLOCK_REALTIME, &new_ts);
    gps_get_time(&current_ts);
    if (current_ts.tv_sec == new_ts.tv_sec)
    {
        if (current_ts.tv_nsec >= correction_value_ns)
        {
            bset = true;
        }
    }
    else
    {
        if (abs(new_ts.tv_sec - current_ts.tv_sec) > 1)
        {
            bset = true;
        }
        else
        {
            if (new_ts.tv_sec > current_ts.tv_sec)
            {
                // 终端时间慢1ms以上
                if (NSEC_PER_SEC - current_ts.tv_nsec >= correction_value_ns)
                {
                    bset = true;
                }
            }
            else
            {
                // 终端时间快1ms以上
                if (current_ts.tv_nsec - correction_value_ns >= 0)
                {
                    bset = true;
                }
            }
        }
    }
    // DBG_SYNC("pps_adjust->gps_set_time:: %ld.%09ld\n", new_ts.tv_sec, new_ts.tv_nsec);
    if (bset)
    {
        gps_set_time(&new_ts); // 写入应用程序全局定时器
        rt_printf("gps set time!\r\n");
    }
    // memset(&new_ts, 0, sizeof(new_ts));
    // gps_get_time(&new_ts);
    // DBG_SYNC("pps_adjust->gps_get_time: %ld.%09ld\n", new_ts.tv_sec, new_ts.tv_nsec);

    return 0;
}

// 智能微调策略（考虑信号状态）
void smart_adjust_strategy(uint64_t pps_ns, uint64_t last_pps_ns,
                           struct performance_benchmark *bench)
{

    // 只在正常状态下进行时间调整
    if (bench->system_state != STATE_NORMAL)
    {
        return;
    }

    if (bench->pps_count == 2)
    {
        DBG_SYNC("首次PPS，进行时间微调...\n");
        calibrated_subsecond_adjust(pps_ns, bench);
    }
    else if (bench->pps_count % 1 == 0) // 可调整定期时间微调时间
    {
        // DBG_SYNC("定期时间微调...\n");
        calibrated_subsecond_adjust(pps_ns, bench);
    }
}

// PPS数据处理
void process_pps_data(uint64_t pps_ns, uint64_t last_pps_ns,
                      struct performance_benchmark *bench)
{
    bench->last_pps_time = get_time_ns();
    bench->last_success_time = bench->last_pps_time;
    bench->consecutive_errors = 0;
}

// 处理读取错误
void handle_read_error(struct performance_benchmark *bench)
{
    bench->consecutive_errors++;

    if (bench->consecutive_errors >= MAX_CONSECUTIVE_ERRORS)
    {
        DBG_ERROR("连续错误次数过多 (%d次)\n", bench->consecutive_errors);
        change_system_state(bench, STATE_ERROR);
    }

    // 短暂休眠避免忙等待
    usleep(100000); // 100ms
}

// 主捕获循环
void pps_capture_main(void *arg)
{
    int fd = (int)(long)arg;
    uint64_t pps_ns, last_pps_ns = 0;
    struct performance_benchmark bench = {0};

    // 初始化
    bench.last_pps_time = get_time_ns();
    bench.system_state = STATE_NORMAL;

    DBG_PRINT("开始PPS时钟校准和时间微调...\n");
    DBG_PRINT("异常处理已启用: 超时=%dms, 最大错误=%d次\n",
              PPS_READ_TIMEOUT_MS, MAX_CONSECUTIVE_ERRORS);

    display_system_time("当前系统时间");

    prctl(PR_SET_NAME, "pps_capture_thread");
    while (running)
    {
        // 检查信号状态
        check_signal_status(&bench);

        // 带超时的PPS读取
        ssize_t ret = read_pps_with_timeout(fd, &pps_ns, sizeof(pps_ns), PPS_READ_TIMEOUT_MS);

        if (ret != sizeof(pps_ns))
        {
            if (running)
            {
                if (errno == ETIMEDOUT)
                {
                    if (bench.system_state == STATE_NORMAL && bench.pps_count > 0)
                    {
                        DBG_WARN("PPS读取超时\n");
                    }
                }
                else if (errno != EINTR)
                {
                    DBG_ERROR("读取错误: code=%d\n", errno);
                    handle_read_error(&bench);
                }
            }
            continue;
        }

        // 读取到PPS数据
        bench.pps_count++;

        if (last_pps_ns != 0)
        {
            process_pps_data(pps_ns, last_pps_ns, &bench);
            smart_adjust_strategy(pps_ns, last_pps_ns, &bench);
        }
        else
        {
            DBG_PPS("PPS#%04d | 基准建立 | PPS时间戳: %.9fs\n",
                    bench.pps_count, pps_ns / 1e9);
        }

        last_pps_ns = pps_ns;

        if (bench.pps_count % 10 == 0)
        {
            fflush(stdout);
        }
    }
}

// 设置实时性
void set_realtime_environment(int fd)
{
    pthread_attr_t attr;
    struct sched_param param;
    pthread_t thread;

    if (pthread_attr_init(&attr) != 0)
    {
        perror("pthread_attr_init failed");
        return;
    }

    if (pthread_attr_setschedpolicy(&attr, SCHED_FIFO) != 0)
    {
        perror("pthread_attr_setschedpolicy failed");
        goto cleanup;
    }

    param.sched_priority = 20;
    if (pthread_attr_setschedparam(&attr, &param) != 0)
    {
        perror("pthread_attr_setschedparam failed");
        goto cleanup;
    }

    if (pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) != 0)
    {
        perror("pthread_attr_setinheritsched failed");
        goto cleanup;
    }

    if (pthread_create(&thread, &attr, pps_capture_main, (void *)(long)fd) != 0)
    {
        perror("pthread_create failed");
        goto cleanup;
    }

    // 分离线程
    pthread_detach(thread);

    DBG_PRINT("PPS线程已启动并在后台运行\n");

cleanup:
    pthread_attr_destroy(&attr);
}

int pps_init(void)
{
    int fd = 0;
    DBG_PRINT("初始化PPS设备...");
    fd = open("/dev/pps", O_RDONLY);
    if (fd < 0)
    {
        DBG_PRINT(" 初始化PPS失败: %s\n", strerror(errno));
        return -1;
    }
    DBG_PRINT(" 初始化PPS成功\n");

    // 设置为非阻塞模式，用于select
    int flags = fcntl(fd, F_GETFL, 0);
    fcntl(fd, F_SETFL, flags | O_NONBLOCK);

    set_realtime_environment(fd);

    return 0;
}