93ca37e36c
- 校正舵机中位、转向符号和阿克曼后轮差速模型\n- 增加航向角速度辅助及遥控通道调试开关\n- 将速度环提升至 200Hz，并按实际 dt 计算 PI 积分\n- 将 IMU 启动校准缩短为 2 秒\n- 为 USART3 增加 DMA 发送和 MCU 采样时间戳
350 lines
7.8 KiB
Markdown
350 lines
7.8 KiB
Markdown
针对现在的“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}
|
||
]
|
||
|