rustcv_core/
time.rs

1use std::collections::VecDeque;
2use std::sync::OnceLock;
3use std::time::{Duration, Instant};
4
5// 1. 定义一个全局静态变量来存储进程启动时间
6// OnceLock 保证它只会被初始化一次，且是线程安全的。
7static PROCESS_START: OnceLock<Instant> = OnceLock::new();
8static PROCESS_START_TIME: OnceLock<Instant> = OnceLock::new();
9
10/// 软件锁相环 (Software PLL) 与时间同步器
11///
12/// 解决两个问题：
13/// 1. 硬件时钟 (Hardware Timestamp) 通常与系统时钟 (System Time) 不同步。
14/// 2. 硬件时钟存在漂移 (Drift)，且 USB 传输导致到达时间 (Arrival Time) 有抖动 (Jitter)。
15///
16/// 算法：基于最小二乘法的线性回归 (Linear Regression on Sliding Window)
17#[derive(Debug)]
18pub struct ClockSynchronizer {
19    /// 滑动窗口大小 (例如最近 30 帧)
20    window_size: usize,
21    /// 历史数据点 (HW_Timestamp, System_Arrival_Time)
22    history: VecDeque<(u64, Instant)>,
23    /// 是否已初始化基准
24    #[allow(dead_code)]
25    baseline_established: bool,
26    /// 估算的斜率 (Drift Rate)
27    estimated_slope: f64,
28    /// 估算的截距 (Offset)
29    estimated_offset: f64,
30}
31
32impl ClockSynchronizer {
33    pub fn new(window_size: usize) -> Self {
34        Self {
35            window_size: window_size.max(2), // 至少两点决定一条直线
36            history: VecDeque::with_capacity(window_size),
37            baseline_established: false,
38            estimated_slope: 1.0,
39            estimated_offset: 0.0,
40        }
41    }
42
43    /// 输入一帧的原始硬件时间戳，返回矫正后的系统时间
44    ///
45    /// * `hw_ns`: 驱动提供的硬件时间戳 (纳秒)
46    /// * `arrival_time`: 帧到达用户态的系统时刻
47    pub fn correct(&mut self, hw_ns: u64, arrival_time: Instant) -> Duration {
48        // 1. 记录数据点
49        if self.history.len() >= self.window_size {
50            self.history.pop_front();
51        }
52        self.history.push_back((hw_ns, arrival_time));
53
54        // 2. 如果数据不够，直接返回到达时间作为降级方案
55        if self.history.len() < 5 {
56            // 在初始化阶段，假设无漂移，直接对齐到第一帧
57            if let Some(start_time) = self.history.front() {
58                // 简单偏移计算
59                let elapsed_hw = hw_ns.saturating_sub(start_time.0);
60                return start_time.1.elapsed() + Duration::from_nanos(elapsed_hw);
61                // 粗略估算
62            }
63            // 兜底：直接返回当前系统时间 (不推荐，但作为 fallback)
64            // 注意：这里需要计算相对于 System Boot 的 duration
65            return self.instant_to_duration(arrival_time);
66        }
67
68        // 3. 计算线性回归 (y = kx + b)
69        // x = hw_timestamp (relative to first point in window)
70        // y = system_time (relative to first point in window)
71        // 我们需要预测当前 x 对应的 y
72        self.recalculate_regression();
73
74        // 4. 应用矫正
75        let (base_hw, base_sys) = self.history.front().unwrap();
76        let dx = (hw_ns as f64) - (*base_hw as f64);
77        let predicted_dy_ns = self.estimated_slope * dx + self.estimated_offset;
78
79        let base_sys_dur = self.instant_to_duration(*base_sys);
80        base_sys_dur + Duration::from_nanos(predicted_dy_ns as u64)
81    }
82
83    /// 简单的最小二乘法实现
84    fn recalculate_regression(&mut self) {
85        let n = self.history.len() as f64;
86        let (base_hw, base_sys) = self.history.front().unwrap();
87        let base_sys_scalar = self.instant_to_scalar(*base_sys);
88
89        let mut sum_x = 0.0;
90        let mut sum_y = 0.0;
91        let mut sum_xy = 0.0;
92        let mut sum_xx = 0.0;
93
94        for (hw, sys) in &self.history {
95            let x = (*hw as f64) - (*base_hw as f64);
96            let y = self.instant_to_scalar(*sys) - base_sys_scalar;
97
98            sum_x += x;
99            sum_y += y;
100            sum_xy += x * y;
101            sum_xx += x * x;
102        }
103
104        let denominator = n * sum_xx - sum_x * sum_x;
105        if denominator.abs() < 1e-6 {
106            // 避免除零 (比如时间戳完全没变)
107            self.estimated_slope = 1.0;
108            self.estimated_offset = 0.0;
109        } else {
110            self.estimated_slope = (n * sum_xy - sum_x * sum_y) / denominator;
111            self.estimated_offset = (sum_y * sum_xx - sum_x * sum_xy) / denominator;
112        }
113    }
114
115    // 辅助：将 Instant 转为 f64 (秒), 仅用于计算差值
116    fn instant_to_scalar(&self, t: Instant) -> f64 {
117        // 这里实际上只需要相对值，不需要绝对 epoch
118        // 我们可以用 t.elapsed() 的反向值，或者既然在一个进程内，
119        // 我们统一参考 lazy_static 的启动时间点会更准。
120        // 为简化代码，这里假设 t 是单调的，转换成纳秒 float。
121        // 在实际生产代码中，应使用 Duration since Boot。
122        // t.elapsed().as_secs_f64() // 这是一个负相关的量，有点 tricky。
123        // 修正：应该保存一个 Process Start Time。
124        // 这里暂略，仅展示逻辑框架。
125        // 0.0
126
127        // 获取启动时间锚点（如果还没初始化，这里会初始化为当前时间）
128        let start_time = PROCESS_START.get_or_init(Instant::now);
129
130        if t >= *start_time {
131            // 正常情况：t 在启动时间之后
132            t.duration_since(*start_time).as_secs_f64()
133        } else {
134            // 边缘情况：t 在启动时间之前（极少见，但为了数学严谨性）
135            // 返回负数
136            -(start_time.duration_since(t).as_secs_f64())
137        }
138    }
139
140    fn instant_to_duration(&self, t: Instant) -> Duration {
141        // 将 Instant 转换为 "System Up Time" (CLOCK_MONOTONIC)
142        // 这是一个平台相关的操作。
143        // 在 Linux 上 Instant 已经是 monotonic。
144        // 我们简单返回 elapsed。实际上应该对齐到 UNIX EPOCH 或 Boot Time。
145        // 这是一个 Placeholder。
146        // Duration::from_nanos(0)
147
148        // 第一次调用时会执行 Instant::now()，后续调用直接返回该值
149        let anchor = PROCESS_START_TIME.get_or_init(Instant::now);
150
151        // 使用 saturating_duration_since 防止在极端边缘情况下 (t 早于 anchor) panic
152        t.saturating_duration_since(*anchor)
153    }
154}
rustcv_core/time.rs

rustcv_core/
time.rs
