针对现在的“FreeRTOS 下位机 + Linux/ROS 2 上位机 + 后续轮速/IMU/2D雷达融合”，建议直接制定一套统一的时间戳与通信协议。

最核心的原则是：

[
\boxed{
\text{MCU原始单调时间戳永不修改}
\quad+\quad
\text{上位机维护时钟映射}
}
]

即：

[
T_{\rm host}=a,t_{\rm mcu}+b
]

## 1. 下位机统一时间基准

不要把 `xTaskGetTickCount()` 作为传感器主时间戳。

它适合：

* `vTaskDelayUntil()` 周期调度；
* 超时判断；
* 任务延时；
* 毫秒级状态机。

它不适合：

* IMU与编码器精确对齐；
* 200 Hz以上传感器采样；
* 分析通信延迟；
* 后续雷达、IMU、轮式里程计融合。

FreeRTOS Tick 的分辨率由 `configTICK_RATE_HZ` 决定；ISR 中若确实要读 Tick，应使用 `xTaskGetTickCountFromISR()`。[FreeRTOS Task Utilities](https://www.freertos.org/Documentation/02-Kernel/04-API-references/03-Task-utilities/00-Task-utilities)

### 推荐时间源

使用一个32位硬件定时器：

```text
计数频率：1 MHz
计数单位：1 us
自动重装值：0xFFFFFFFF
软件扩展：64 bit
时间含义：MCU自本次上电以来经过的微秒数
```

例如 STM32 可以使用 TIM2 或 TIM5：

[
t_{\rm mcu}\in uint64_t,\qquad 单位=\mu s
]

32位、1 MHz计数器约每71.58分钟回绕一次，所以用更新中断增加高32位。

```c
static volatile uint32_t g_time_high = 0;

void TIM2_IRQHandler(void)
{
    if (__HAL_TIM_GET_FLAG(&htim2, TIM_FLAG_UPDATE) != RESET) {
        __HAL_TIM_CLEAR_IT(&htim2, TIM_IT_UPDATE);
        g_time_high++;
    }
}

uint64_t mcu_time_us(void)
{
    uint32_t high1;
    uint32_t high2;
    uint32_t low;
    uint32_t status;

    do {
        high1 = g_time_high;
        __DMB();

        low = TIM2->CNT;
        status = TIM2->SR;

        __DMB();
        high2 = g_time_high;
    } while (high1 != high2);

    /*
     * 计数器已经回绕，但更新中断可能还没有得到执行。
     * low 小于半量程，说明读取发生在回绕之后。
     */
    if ((status & TIM_SR_UIF) && low < 0x80000000U) {
        high1++;
    }

    return ((uint64_t)high1 << 32) | low;
}
```

注意：

* 定时器时钟必须确认是否受 APB 分频后的“定时器倍频”影响；
* 不要直接读取一个由中断写入的普通 `uint64_t`，32位MCU上未必原子；
* 如果启用了 STOP 模式或 Tickless Idle，要确认这个定时器休眠时是否继续工作；
* 时间戳只能递增，不能因为校时而向前或向后跳变。

## 2. 时间戳应该在哪里打

时间戳必须尽可能靠近数据真正产生的位置。

| 数据       | 推荐时间戳位置       |
| -------- | ------------- |
| IMU      | DRDY中断发生时     |
| SPI读取IMU | 不要等SPI读取完成才打点 |
| 编码器      | 控制定时器锁存计数器时   |
| 电机控制量    | PWM寄存器更新时     |
| 控制周期     | 控制周期入口        |
| 上位机接收    | 完整帧接收完成时，单独记录 |

例如：

```c
typedef struct {
    uint64_t sample_time_us;
    int16_t gyro_raw[3];
    int16_t accel_raw[3];
} ImuSample;
```

IMU中断中只做：

1. 读取 `mcu_time_us()`；
2. 保存时间戳；
3. 通知采集任务；
4. 立即退出中断。

不要在中断中完成协议编码、CRC计算和串口发送。

如果传感器内部启用了低通滤波，DRDY时间仍然可能比真实物理测量晚一个固定群延迟。后续可以增加：

```c
corrected_time_us = drdy_time_us - imu_filter_delay_us;
```

但必须在确定传感器滤波器延迟后再补偿。

## 3. FreeRTOS任务结构

建议使用：

```mermaid
flowchart TD
    A["IMU DRDY / 控制定时器 ISR"] --> B["采样环形缓冲区"]
    B --> C["传感器处理任务"]
    C --> D["协议发送队列"]
    D --> E["唯一 UART TX 任务"]
    F["UART DMA / IDLE ISR"] --> G["接收环形缓冲区"]
    G --> H["协议解析任务"]
```

关键规则：

* UART发送只能有一个所有者任务，避免多个任务发送的数据交叉；
* UART接收使用DMA、IDLE中断和环形缓冲区；
* ISR只负责搬运数据和通知任务；
* 二进制协议和 `printf()` 调试日志不要共用同一个UART；
* 如果ISR调用 FreeRTOS 的 `...FromISR()` 接口，中断优先级必须符合 `configMAX_SYSCALL_INTERRUPT_PRIORITY` 约束；
* 周期任务使用 `vTaskDelayUntil()`，它能避免普通相对延时产生的累计漂移，但实际执行时刻仍应调用硬件微秒时钟记录。[FreeRTOS `vTaskDelayUntil()`](https://freertos.org/xtaskdelayuntiltask-control.html)

例如200 Hz低精度周期任务：

```c
void SensorTask(void *argument)
{
    TickType_t last_wake = xTaskGetTickCount();
    const TickType_t period = pdMS_TO_TICKS(5);

    for (;;) {
        vTaskDelayUntil(&last_wake, period);

        uint64_t actual_time_us = mcu_time_us();

        /* 采集和处理 */
    }
}
```

如果要求更稳定的200 Hz采样，应由硬件定时器产生中断，再通知任务，而不是完全依赖 FreeRTOS Tick。

## 4. 推荐二进制帧规范 v1

假设使用 UART 或 USB CDC，建议采用：

```text
COBS编码（协议头 + 负载 + CRC32） + 0x00帧分隔符
```

COBS的好处是任意解析错误后，都可以在下一个 `0x00` 重新找到帧边界。

### 统一协议头：24字节

之前已有的基础上额外增加
| session_id     | uint32 |  4 | 本次MCU启动会话ID  |
| sample_time_us | uint64 |  8 | MCU采样时间      |


但不要直接执行：

```c
uart_send((uint8_t *)&header, sizeof(header)); // 不推荐
```

因为不同编译器可能存在：

* 结构体填充；
* 对齐差异；
* 大小端差异；
* 浮点格式差异。

             |


## 7. 四时间戳同步协议

每次同步记录：

* (T_1)：上位机发送请求；
* (t_2)：MCU收到并解析请求；
* (t_3)：MCU准备发送响应；
* (T_4)：上位机收到完整响应。

定义MCU时间减上位机时间为：

[
\theta=
\frac{(t_2-T_1)+(t_3-T_4)}{2}
]

往返延迟：

[
d=(T_4-T_1)-(t_3-t_2)
]

初始转换：

[
T_{\rm host}\approx t_{\rm mcu}-\theta
]

运行中收集多组：

[
x_i=\frac{t_{2,i}+t_{3,i}}{2}
]

[
y_i=\frac{T_{1,i}+T_{4,i}}{2}
]

拟合：

[
\boxed{T_{\rm host}=a,t_{\rm mcu}+b}
]

推荐参数：

* 启动时连续同步20～50次；
* 初始阶段先令 (a=1)，从最小RTT样本估计 (b)；
* 运行中每1秒同步一次；
* 保存最近60～120组；
* 丢弃RTT明显偏大的样本；
* 使用RTT最小的20%～30%拟合 (a,b)；
* MCU时间戳本身永远不被“校准”或重写。

上位机使用 `CLOCK_MONOTONIC_RAW` 或 `std::chrono::steady_clock`，不要使用会被NTP校时改变的墙上时间参与控制。

## 8. ROS 2时间戳转换

MCU的“上电微秒数”不能直接填进 ROS 消息的 `header.stamp`，否则无法和激光雷达时间对齐。

上位机先得到：

[
T_{\rm steady}=a,t_{\rm mcu}+b
]

再在ROS桥接节点中保存一对锚点：

[
(T_{\rm steady,0},T_{\rm ros,0})
]

转换为：

[
T_{\rm ros,sample}
==================

T_{\rm ros,0}
+
(T_{\rm steady,sample}-T_{\rm steady,0})
]

然后将这个时间填入：

* `sensor_msgs/Imu.header.stamp`；
* `nav_msgs/Odometry.header.stamp`；
* 编码器或轮速自定义消息时间戳。

这样轮速、IMU和激光雷达才能在同一ROS时间轴上融合。

## 9. 上位机命令不要依赖绝对时间

普通运动命令建议使用“接收后立即执行＋超时”：

```c
typedef struct {
    uint32_t command_sequence;
    uint32_t valid_for_us;
    float velocity_ref;
    float yaw_rate_ref;
} MotionCommand;
```

MCU收到时记录：

```c
last_command_rx_us = mcu_time_us();
```

安全判断：

```c
if (mcu_time_us() - last_command_rx_us > command.valid_for_us) {
    enter_safe_deceleration();
}
```

只有真正需要未来定时执行时，才让上位机通过逆映射计算：

[
t_{\rm mcu,execute}
===================

\frac{T_{\rm host,execute}-b}{a}
]