Files

- 校正舵机中位、转向符号和阿克曼后轮差速模型\n- 增加航向角速度辅助及遥控通道调试开关\n- 将速度环提升至 200Hz，并按实际 dt 计算 PI 积分\n- 将 IMU 启动校准缩短为 2 秒\n- 为 USART3 增加 DMA 发送和 MCU 采样时间戳

2026-08-12 18:51:48 +08:00

7.8 KiB

Raw Blame History

针对现在的“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

2. 时间戳应该在哪里打

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

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

例如：

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

IMU中断中只做：

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

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

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

corrected_time_us = drdy_time_us - imu_filter_delay_us;

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

3. FreeRTOS任务结构

建议使用：

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()

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

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，建议采用：

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

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

统一协议头：24字节

但不要直接执行：

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. 上位机命令不要依赖绝对时间

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

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

MCU收到时记录：

last_command_rx_us = mcu_time_us();

安全判断：

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} ]

7.8 KiB Raw Blame History Unescape Escape