cyy/origincar_controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

完善阿克曼控制与高速串口遥测

Browse Source 
- 校正舵机中位、转向符号和阿克曼后轮差速模型\n- 增加航向角速度辅助及遥控通道调试开关\n- 将速度环提升至 200Hz，并按实际 dt 计算 PI 积分\n- 将 IMU 启动校准缩短为 2 秒\n- 为 USART3 增加 DMA 发送和 MCU 采样时间戳
This commit is contained in:
cyy_mac
2026-08-12 18:51:48 +08:00
parent 576784bb4d
commit 93ca37e36c
17 changed files with 2151 additions and 932 deletions
Download Patch File Download Diff File Expand all files Collapse all files

374
doc/ackermann_steering_calibration.md Normal file
View File

@@ -0,0 +1,374 @@
# 阿克曼转向标定与拟合说明
本文记录阿克曼小车「(Vx, Vz) → 舵机 PWM + 左右后轮速度」控制律的标定数据、
拟合方法、单位约定，三种控制模式（标定运动学 / 直接映射 / 横摆闭环），以及
串口通信（200 Hz DMA 发送、921600 波特率）。对应代码：
- 控制律：`BALANCE/balance.c` 的 `Akm_Car` 分支，
`Akm_Curvature_To_Servo()`（拟合）与 `Akm_Norm_To_Servo()`（直接满行程映射）。
- 开关宏：`BALANCE/robot_select_init.h`；舵机直行点 `SERVO_INIT` 在 `HARDWARE/motor.h`。
- 串口：`HARDWARE/usartx.c`（`data_task` / `USART3_SEND` / `uart3_dma_tx_init`），
波特率在 `BALANCE/system.c` 的 `systemInit`。
## 1. 硬件与坐标约定
- 后驱阿克曼：`MOTOR_A` = 左后轮，`MOTOR_B` = 右后轮，前轮由舵机（TIM12 CCR2）转向。
- 输入语义（上位机下发）：
- `Vx` = 后轴中心线速度，单位 m/s。
- `Vz` = 绕转弯中心的旋转角速度 wz，单位 rad/s。
- 符号约定：
- 上位机 `Vz` 遵循 ROS 约定：**Vz > 0 = 逆时针 = 左转**。
- 标定表 / 舵机拟合使用相反符号：**曲率 κ > 0 = 右转**。
- 代码中 `kappa_fit = -Vz/Vx`，使 `Vz > 0` 得到 `kappa_fit < 0`（左转），
与拟合域一致。
## 2. 标定方法（手推法）
电机失能、舵机使能，手动推动小车走出稳定圆弧，对每个舵机 PWM 记录：
- `y 向半截距`（弧对应的纵向半截距，cm）
- `x 向半弓高`（弦的矢高 / 半弓高，cm）
- 由几何反推后轴中心转弯半径 `R`（cm）与 `1/R`
x 向半弓高的符号用于判定左右：PWM > ~1670 为右转（κ 取正），
低于该点为左转（κ 取负）。
### 三个"中位"值（易混，务必区分）
| 名称 | 值 | 含义 | 代码位置 |
|------|---:|------|----------|
| 遥控 CH1 中位 | 1500 | 遥控摇杆物理中点 | `AKM_REMOTER_CH1_MID`（balance.c） |
| 舵机机械中位 | 1600 | 舵机行程几何中点 (MIN+MAX)/2 | —（不再直接使用） |
| **直行点 SERVO_INIT** | **1670** | **实测 1/R≈0、车真正走直线的点** | `SERVO_INIT`（motor.h:52） |
关键：**所有控制律的舵机中位都对齐到 `SERVO_INIT = 1670`**（真正的直行点），
而非机械中位 1600。开机上电、Mode 1 / Mode 2 的零位、CH1 覆盖都以它为基准：
- CH1 覆盖时舵机 = `Remoter_Ch1 + (SERVO_INIT - AKM_REMOTER_CH1_MID)`，即整体
平移 `1670 - 1500 = +170`，使摇杆居中 = 车轮回正。
- 改 `SERVO_INIT` 一处，下游所有路径自动对齐。
### 原始标定数据
| servo(pwm) | y半截距/cm | x半弓高/cm | R/cm | 1/R (1/cm) |
|-----------:|----------:|----------:|-----:|-----------:|
| 2000 | 34 | 34 | 34.00 | 0.029412 |
| 1900 | 52.5 | 49.5 | 52.59 | 0.019015 |
| 1800 | 90 | 60 | 97.50 | 0.010256 |
| 1700 | 167.2 | 60 | 262.97 | 0.003803 |
| 1670 | 120 | -1 | 7200.5 | 0.000139 |
| 1600 | 120 | -23 | 324.54 | 0.003081 |
| 1500 | 89 | -60 | 96.01 | 0.010416 |
| 1400 | 61 | -60 | 61.01 | 0.016391 |
| 1300 | 43 | -44 | 43.01 | 0.023250 |
| 1200 | 35.5 | -34.3| 35.52 | 0.028152 |
| 1100 | 30 | -28.9| 30.02 | 0.033310 |
> 直行点约在 PWM 1670（1/R ≈ 0，x 半弓高 ≈ 0），并非舵机机械中位 1600。
## 3. 单位换算（关键，曾导致 100 倍错误）
标定表 `1/R` 列以 **1/cm** 为单位（R 用厘米）。而代码里的曲率来自
`kappa = wz / Vx`（均为 SI），单位是 **1/m**。两者相差 100 倍。
拟合前必须把 R 从 cm 换算为 m，再取 `kappa = 1/R_m`（并带符号）：
| servo | kappa (1/m, +=右) |
|------:|------------------:|
| 2000 | +2.9412 |
| 1900 | +1.9015 |
| 1800 | +1.0256 |
| 1700 | +0.3803 |
| 1670 | +0.0139 |
| 1600 | -0.3081 |
| 1500 | -1.0416 |
| 1400 | -1.6391 |
| 1300 | -2.3250 |
| 1200 | -2.8152 |
| 1100 | -3.3310 |
- 曲率量程：κ ∈ [−3.331, +2.941] 1/m。
- 最小转弯半径 R_min ≈ 0.30 m（左）/ 0.34 m（右）。
- 代码中 `AKM_KAPPA_MAX = 3.331f`。
## 4. 拟合方法与结果
以 `servo = f(kappa)`（kappa 为自变量，1/m）做多项式最小二乘。
比较一/二/三次，并用留一交叉验证（LOO）评估对未见点的泛化：
| 拟合 | 全量 max残差 | 全量 rms | LOO max误差 | LOO rms |
|------|-----------:|--------:|----------:|-------:|
| 一次 | 56.8 | 28.3 | — | — |
| 二次 | 12.3 | 7.6 | 15.1 | 10.2 |
| 三次 | 12.6 | 7.5 | — | — |
结论：**二次拟合最优**。三次不再改善（过拟合），一次残差过大。
### 最终系数（1/m 单位）
```
servo = 1656.373 + 140.548 * kappa - 7.654 * kappa^2
```
代码宏（`BALANCE/balance.c`）：
```c
#define AKM_SERVO_C0 1656.373f
#define AKM_SERVO_C1 140.548f
#define AKM_SERVO_C2 (-7.654f)
#define AKM_SERVO_MIN 1100
#define AKM_SERVO_MAX 2000
#define AKM_KAPPA_MAX 3.331f
```
### 为什么用拟合而非查表 + 线性插值
- 数据是手推测得，含测量噪声。查表被迫穿过每个噪声点，两点间直线段会
放大噪声；LOO 显示查表 max 误差 23 / rms 14，明显差于二次拟合（15 / 10）。
- 拟合在端点外推时形状正确（曲线两端明显弯），线性插值只能按最后一段
斜率外推，越推越偏。
- 拟合在节点处平滑，无斜率突变；运行成本仅两次乘加，比查找区间还省。
### 轴距 L 去哪了
标定表端到端直接测了 `servo → R`，已把「舵机 PWM → 前轮转角 →
`R = L/tan(δ)`」整条链路及传动比、轮胎侧偏等真实效应吸收进拟合系数。
因此无需再显式写 `R = Axle_spacing/tan(δ)`（那反而依赖不准的假设传动比）。
后轮差速只用轮距 track 与 κ，本就不含 L。阿克曼几何没有丢，而是以更贴合
实车的实测形式嵌入拟合。
## 5. 控制律（Mode 0：标定运动学，默认）
`AKM_DIRECT_MAP = 0` 时：
```
kappa_geom = Vz / Vx (Vx≈0 时置 0，阿克曼无前进不能转向)
kappa_fit = clamp(-kappa_geom, ±AKM_KAPPA_MAX)
Servo = f(kappa_fit) # 二次拟合
MOTOR_A(左) = Vx * (1 + 0.5*track*kappa_fit) # 后轮差速
MOTOR_B(右) = Vx * (1 - 0.5*track*kappa_fit)
```
物理特性：舵机角只决定转弯半径 R = 1/κ。
- 固定舵机角、改 Vx → wz 随之变（走同一圆，快慢不同）。
- 固定 wz、改 Vx → κ = wz/Vx 变 → 舵机角变（正确行为）。
### Vx–wz 可行域（κ_max ≈ 3.33 /m）
`|wz| <= Vx * κ_max`：
| Vx (m/s) | 可用 wz 范围 (rad/s) |
|---------:|:--------------------|
| 0.2 | [−0.67, 0.67] |
| 0.3 | [−1.00, 1.00] |
| 0.5 | [−1.67, 1.67] |
| 1.0 | [−3.33, 3.33] |
> 例：固定 wz=0.12，需 Vx ≥ 0.036 m/s 才不被夹紧。之前 `AKM_KAPPA_MAX`
> 误设为 0.0334（对应 R≈30 m）导致任何速度下都被夹死、改速度舵机不动，
> 已随单位修正解决。
## 6. 控制律（Mode 1：直接映射，调试用）
`AKM_DIRECT_MAP = 1` 时（脱离阿克曼物理，用于隔离调试舵机/电机）：
```
vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, ±1)
Servo = Akm_Norm_To_Servo(vz_norm) # 以 SERVO_INIT 为零位的分段满行程映射
MOTOR_A(左) = Vx # 无差速
MOTOR_B(右) = Vx
```
- `Vz` 线性铺满整个舵机行程 [AKM_SERVO_MIN, AKM_SERVO_MAX]，与速度无关。
- **零位对齐直行点**：`Akm_Norm_To_Servo` 以 `SERVO_INIT=1670`（而非机械中位
1600）为中心，左右两侧各自缩放到自己的端点，即使中位偏置也能用满全行程：
```
span = (norm>=0) ? (SERVO_INIT - AKM_SERVO_MIN) # 左侧行程 1670-1100=570
: (AKM_SERVO_MAX - SERVO_INIT) # 右侧行程 2000-1670=330
Servo = SERVO_INIT - norm * span
```
- `Vz > 0`（左）→ `norm>0` → 靠近 AKM_SERVO_MIN（左端），符号与 ROS 一致。
- `Vx` 原样给左右电机，不做曲率/差速运算。
- 满量程输入由 `AKM_DIRECT_VZ_FULL`（默认 **1.0 rad/s**）设定。
> **`AKM_DIRECT_VZ_FULL` 的物理含义** = 上位机会发的最大 angular.z，Vz 到该值时
> 舵机打满。它只决定"Vz→行程"的比例，不改变峰值转角（峰值由舵机端点 1100/2000
> 决定，恒为 ~28°左 / 25°右）。太小→小 Vz 就饱和、失去比例控制；太大→常用区间
> 只用到一小段行程、转角偏小。当前定为 1.0（Vz=±1.0 打满），把行程摊到 ±1 rad/s
> 全区间，规划分辨率比 0.5 时翻倍。Vz 在 RX 中断里以 rad/s 到达
> （usartx.c `XYZ_Target_Speed_transition`: raw/1000）。
## 7. 控制律（Mode 2：横摆角速度闭环 / 简化扭矩矢量）
`AKM_DIRECT_MAP = 0` 且 `AKM_YAW_ASSIST = 1` 时启用。
**动机（为什么要有 Mode 2）**：Mode 0 里 `κ = Vz/Vx`，同一个转向指令 `Vz` 在
高速时曲率被 `Vx` 除小，舵机自动回正 —— 这就是"高速转弯打不动"的根源。Mode 2
**主动解耦 w 与 v**：舵机由 `Vz` 直接决定（与 Mode 1 一样，不再除以 Vx），
所以大 `Vz` 在任何速度都给出大前轮角；阿克曼只作为后轮差速的**前馈参考**，
再叠加 IMU 横摆角速度 PI 闭环（简化扭矩矢量）。控制链路：
```
Vz ─(直接满行程映射, 与Vx无关)→ 舵机主转向 Akm_Norm_To_Servo ← 方向主控，解耦
Vx ───────────────────────────→ 左右轮基速 ← 驱动主控，解耦
(Vx, κ_cmd) ─(阿克曼几何)→ r_ref ─→ 后轮差速前馈 + IMU PI ← akm 仅做前馈
```
计算（fit 域 κ>0=右；ROS Vz>0=左=CCW；r>0=左转）：
```
vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, ±1) # 转向指令，不含 Vx —— 关键解耦点
Servo = Akm_Norm_To_Servo(vz_norm) # 与 Mode 1 完全相同的满行程直接映射
# 以 SERVO_INIT 为零位，不走拟合曲线
κ_cmd = -vz_norm * AKM_KAPPA_MAX # 仅用于构造下方前馈参考，不驱动舵机
r_ref = -κ_cmd * Vx # 阿克曼几何前馈的期望横摆角速度
r_meas = AKM_GYRO_Z_SIGN * gyro[2] / AKM_GYRO_Z_TO_RADPS
r_filt += AKM_YAW_IMU_LPF * (r_meas - r_filt) # 轻度一阶低通
e_r = r_ref - r_filt
dv_ff = AKM_YAW_FF_ALPHA * 0.5 * track * r_ref # 几何前馈
dv_fb = AKM_YAW_KP * e_r + AKM_YAW_KI * ∫e_r # PI 反馈（矩形积分, dt=1/200s）
dv = clamp(dv_ff + dv_fb, ±AKM_YAW_MAX_DIFF_RATIO*|Vx|) # 带抗积分饱和
MOTOR_A(左) = Vx - dv
MOTOR_B(右) = Vx + dv
```
要点：
- **解耦是核心**：`vz_norm` 只由 `Vz` 决定，不再 `Vz/Vx`，所以高速大转向不再被
几何"稀释"。`Vx` 独立设定驱动基速。
- **舵机走直接满行程映射，不走拟合**：Mode 2 的舵机与 Mode 1 / CH1 调试一致，
用 `Akm_Norm_To_Servo(vz_norm)` 把转向指令线性铺满行程（以 `SERVO_INIT` 为
零位），**不再调用标定拟合 `f(κ)`**。这样"高速转弯打不动"从根上消失——舵机角
只看指令、与速度无关。（早期版本这里错用了 `f(κ_cmd)`，仍隐含耦合，已改正。）
- **阿克曼降级为纯前馈**：`κ_cmd = -vz_norm*AKM_KAPPA_MAX` 只用来构造 `r_ref`
——"若车按该曲率走出的理想横摆角速度"，仅喂给后轮差速前馈；实际横摆由 IMU PI
收敛，可克服前轮几何/打滑带来的偏差。`κ_cmd` 不参与舵机计算。
- **退化关系**：`AKM_YAW_FF_ALPHA = 1` 且 `AKM_YAW_KP = AKM_YAW_KI = 0` 时，
`dv = 0.5*track*r_ref = -0.5*track*κ_cmd*Vx`，即纯几何差速。α 是平滑旋钮：
0 = 纯 IMU 反馈，1 = 纯几何前馈。
- **低速冻结**：`|Vx| < AKM_YAW_MIN_SPEED` 时清零积分并停用差速（低速横摆角
速度信噪比太差）。
- 用原始角速度（`gyro[2]` 去零偏后的 LSB），不做航向角积分。
### 上位机规划用：发布 Vz → 大致前轮转角 δ
Mode 2 里舵机只由 `Vz` 决定（与 `Vx` 无关），所以上位机可直接按下表估转角。
换算链路（非线性，因为 PWM→R 那段是标定曲线）：
```
vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, ±1) # 默认 VZ_FULL=1.0 → Vz=±1.0 打满
Servo = SERVO_INIT - vz_norm * span # 左 span=570, 右 span=330
R = 1 / |1/R(Servo)| # 1/R 由 §2 标定表(1/cm)插值再×100
δ = atan(L / R) # L = Akm_axlespacing = 0.160 m
```
用当前宏值（`SERVO_INIT=1670`, `MIN=1100`, `MAX=2000`, `VZ_FULL=1.0`）、按 §2 标定表
`1/R` 列插值算出的对照表（δ 为后轴等效前轮转角，正数只表大小，方向见末列）：
| Vz (rad/s) | vz_norm | Servo(PWM) | R(m) | δ(°) | 方向 |
|-----------:|--------:|-----------:|-----:|-----:|:----:|
| +1.0 | +1.0 | 1100 | 0.30 | 28.1 | 左 |
| +0.8 | +0.8 | 1214 | 0.36 | 23.7 | 左 |
| +0.6 | +0.6 | 1328 | 0.47 | 18.8 | 左 |
| +0.5 | +0.5 | 1385 | 0.57 | 15.6 | 左 |
| +0.4 | +0.4 | 1442 | 0.72 | 12.5 | 左 |
| +0.3 | +0.3 | 1499 | 0.95 | 9.5 | 左 |
| +0.2 | +0.2 | 1556 | 1.59 | 5.8 | 左 |
| +0.1 | +0.1 | 1613 | 3.95 | 2.3 | 左 |
| 0.0 | 0.0 | 1670 | ∞ | 0.0 | 直 |
| −0.1 | −0.1 | 1703 | 2.50 | 3.7 | 右 |
| −0.2 | −0.2 | 1736 | 1.63 | 5.6 | 右 |
| −0.3 | −0.3 | 1769 | 1.21 | 7.5 | 右 |
| −0.4 | −0.4 | 1802 | 0.96 | 9.5 | 右 |
| −0.5 | −0.5 | 1835 | 0.75 | 12.0 | 右 |
| −0.6 | −0.6 | 1868 | 0.62 | 14.5 | 右 |
| −0.8 | −0.8 | 1934 | 0.44 | 19.8 | 右 |
| −1.0 | −1.0 | 2000 | 0.34 | 25.2 | 右 |
要点（上位机规划务必注意）：
- **左右不对称**：直行点 1670 偏向右端，左侧行程 570、右侧仅 330，所以同样
`|Vz|` 左转角比右转角略大（满量程 28° 左 vs 25° 右）。这是机械中位偏置导致的，
已被 `Akm_Norm_To_Servo` 的分段缩放吸收。
- **`|Vz| ≥ 1.0` 全部饱和**到端点（28°左 / 25°右）；再大的 Vz 不会有更大转角。
`VZ_FULL` 从 0.5 提到 1.0 后，满量程指令 = ±1.0 rad/s，同样的舵机行程摊到更宽的
Vz 区间，规划分辨率翻倍（峰值转角不变）。
- **非线性**：Vz→δ 不是直线（低 Vz 段每 0.1 约 +3°，高 Vz 段趋缓），规划时按表
插值而非线性外推。
- R 是**后轴中心转弯半径**，δ = atan(L/R) 是等效前轮转角；实车受轮胎侧偏/打滑
影响，表值为标定静推的近似，动态下 IMU 横摆环会再做修正。
调参顺序（用户建议）：
1. 先标定陀螺零偏（开机静止采样，代码已做）。
2. **实测确认 `gyro[2]` 符号**：命令左转，确认 `gyro[2] > 0`；若相反把
`AKM_GYRO_Z_SIGN` 改为 `-1.0f`。
3. 小 `Kp`（0.05~0.10）起步，逐步加大到临界前。
4. 有稳态误差再加一点 `Ki`。
5. 最后引入前馈 `α = 0.2~0.5` 减轻 PI 负担。
## 8. 开关一览（`BALANCE/robot_select_init.h`）
| 宏 | 默认 | 含义 |
|----|-----:|------|
| `AKM_SERVO_DEBUG_REMOTE_CH1` | 1 | 1 = 舵机直接由遥控 CH1 驱动，覆盖所有控制律（板级调试） |
| `AKM_DIRECT_MAP` | 1 | 1 = 直接映射调试模式；0 = 交给 Mode 0 / Mode 2 |
| `AKM_DIRECT_VZ_FULL` | 1.0f | Mode 1 & Mode 2 共用：Vz 满行程量程（rad/s）= 上位机会发的最大 angular.z |
| `AKM_YAW_ASSIST` | 0 | 1 = 横摆闭环（Mode 2）；仅在 `AKM_DIRECT_MAP=0` 时生效 |
| `AKM_YAW_KP` / `AKM_YAW_KI` | 0.10 / 0.00 | 横摆角速度误差 PI 增益（m/s per rad/s） |
| `AKM_YAW_FF_ALPHA` | 0.00f | 几何前馈混合系数（0=纯反馈，1=纯几何差速=Mode 0） |
| `AKM_YAW_MIN_SPEED` | 0.10f | 低于此速度冻结横摆环（m/s） |
| `AKM_YAW_MAX_DIFF_RATIO` | 0.35f | 差速幅度上限（占 |Vx| 的比例） |
| `AKM_GYRO_Z_TO_RADPS` | 3754.9f | gyro[2] LSB→rad/s（FS ±500dps） |
| `AKM_GYRO_Z_SIGN` | +1.0f | IMU +z 与 ROS（+=左）符号对齐，实测确认 |
| `AKM_YAW_IMU_LPF` | 0.30f | 横摆角速度一阶低通系数（0=无滤波） |
模式互斥与优先级：
- `AKM_SERVO_DEBUG_REMOTE_CH1 = 1` 时，**舵机总是由 CH1 覆盖**，优先于任何控制
律计算出的舵机值（电机差速仍按所选模式运行）。
- `AKM_DIRECT_MAP` 与 `AKM_YAW_ASSIST` 都作用于整条控制律：`AKM_DIRECT_MAP = 1`
优先（直接映射）；`= 0` 时再看 `AKM_YAW_ASSIST`（1 = Mode 2 横摆闭环，
0 = Mode 0 标定运动学）。
## 9. 串口通信（ROS ↔ STM32）
### 频率与方向
| 方向 | 频率 | 机制 | 代码位置 |
|------|-----:|------|----------|
| 控制环 | 200 Hz | `Balance_task`，`RATE_200_HZ` | balance.c:350 |
| 发送（TX，STM32→ROS） | 200 Hz | **DMA 非阻塞** | usartx.c `data_task` |
| 接收（RX，ROS→STM32） | 中断驱动 | `USART3_IRQHandler`（RXNE） | usartx.c |
- 只保留 **USART3（ROS）**。原先 `data_task` 20 Hz 阻塞式群发 USART1/3/5+CAN，
现已删掉 USART1/USART5/CAN 发送，只留 USART3。
- 帧长 24 字节（`SEND_DATA_SIZE`），`Vz` 在 RX 中断里由
`XYZ_Target_Speed_transition` 解析：`raw/1000 + (raw%1000)*0.001`（rad/s）。
### 为什么改 DMA 非阻塞发送
旧的 `usart3_send` 是忙等：`USART3->DR = data; while((USART3->SR&0x40)==0);`。
`data_task` 与 `Balance_task` **同为优先级 4**（FreeRTOS 抢占 + 时间片，1ms tick）。
同优先级下忙等无法被控制环抢占，20→200 Hz 会放大抖动、白耗 CPU。改成 DMA 后：
- `USART3_SEND` 只触发一次 DMA 传输（DMA1 Stream3 / Channel4 = USART3_TX），
CPU 立即返回，shift-out 期间不占用控制环。
- 上一帧未发完则跳过本周期（`DMA_GetCmdStatus != DISABLE` 判定），不忙等。
- 需要把 `FWLIB/src/stm32f4xx_dma.c` 加入 Makefile（原先未编译该驱动）。
### 波特率 921600
USART3 波特率 **115200 → 921600**（system.c:129，`uart3_init(921600)`）。
- 一帧 24 字节 = 240 bit（8N1，10 bit/byte）的 shift-out 时间：
- 115200：240/115200 ≈ **2.08 ms**
- 921600：240/921600 ≈ **0.26 ms**（快 8 倍，远低于 5 ms 周期）
- 带宽占用：200 Hz × 24 B = 4800 B/s，两档都绰绰有余；提速主要是留余量。
- APB1=42 MHz，16 倍过采样分频后实际约 913 k，偏差 ~0.9%（UART 容忍 <2.5%）。
- **ROS 端必须同步改 921600**，否则乱码——这是最易漏的一步。
## 10. 复现拟合
标定原始脚本与中间产物在 `calibration/` 目录。核心步骤：
1. R(cm) → R(m)，`kappa = sign / R_m`（sign 由 x 半弓高定，+ 为右）。
2. 对 `(kappa, servo)` 做二次最小二乘，得 C0/C1/C2。
3. 用 LOO 交叉验证确认二次优于查表与三次。
4. `AKM_KAPPA_MAX = max(|kappa|)`。

111
doc/uart_protocol_v2_timestamp.md Normal file
View File

@@ -0,0 +1,111 @@
# 下位机 UART 发送协议 v2（带采样时间戳）
本文件描述 STM32 下位机通过 **USART3（ROS 口，波特率 921600）** 周期发送给上位机的
传感器数据帧格式。v2 在原 24 字节帧的基础上，于**帧尾追加**了会话 ID 与两个采样
时间戳字段，用于上位机做 IMU / 轮速 / 雷达的时间对齐。
- 对应源码：[HARDWARE/usartx.c](../HARDWARE/usartx.c)、[HARDWARE/usartx.h](../HARDWARE/usartx.h)
- 时间源实现：[HARDWARE/timer.c](../HARDWARE/timer.c) 的 `TIM7_Init()` / `mcu_time_us()`
- 设计背景与总体原则见 [时间戳.md](时间戳.md)
## 1. 帧总览
| 项目 | 值 |
| ---- | ---- |
| 帧头 `FRAME_HEADER` | `0x7B` |
| 帧尾 `FRAME_TAIL` | `0x7D` |
| 帧长 `SEND_DATA_SIZE` | **44 字节**（v1 为 24 字节） |
| 发送周期 | 200 Hz（`data_task`，USART3 非阻塞 DMA 发送） |
| 字节序 | 多字节字段一律 **大端**（高字节在前） |
| 校验 | 单字节 XOR，覆盖 byte 0..41 |
设计约束：byte 0..21 与 v1 完全一致，新增字段全部放在校验位之前的尾部，
使只读取旧字段的解析器仍然兼容。
## 2. 字节布局
| 偏移 | 长度 | 字段 | 类型 | 说明 |
| ---- | ---- | ---- | ---- | ---- |
| 0 | 1 | Frame_Header | u8 | 帧头 `0x7B` |
| 1 | 1 | Flag_Stop | u8 | 软件失能标志（1=停车） |
| 2 | 2 | X_speed | int16 | X 轴线速度，单位 mm/s，大端 |
| 4 | 2 | Y_speed | int16 | Y 轴线速度，单位 mm/s，大端 |
| 6 | 2 | Z_speed | int16 | Z 轴角速度，单位 mm/s（等效），大端 |
| 8 | 2 | Accel_X | int16 | 加速度计 X（ROS 坐标），大端 |
| 10 | 2 | Accel_Y | int16 | 加速度计 Y（ROS 坐标），大端 |
| 12 | 2 | Accel_Z | int16 | 加速度计 Z（ROS 坐标），大端 |
| 14 | 2 | Gyro_X | int16 | 陀螺仪 X（ROS 坐标），大端 |
| 16 | 2 | Gyro_Y | int16 | 陀螺仪 Y（ROS 坐标），大端 |
| 18 | 2 | Gyro_Z | int16 | 陀螺仪 Z（ROS 坐标），大端 |
| 20 | 2 | Power_Voltage | int16 | 电池电压×1000，大端 |
| **22** | **4** | **session_id** | **uint32** | 本次 MCU 启动会话 ID，大端 |
| **26** | **8** | **speed_sample_time_us** | **uint64** | 编码器采样时刻（μs），大端 |
| **34** | **8** | **imu_sample_time_us** | **uint64** | IMU 采样时刻（μs），大端 |
| 42 | 1 | Checksum | u8 | byte 0..41 的 XOR |
| 43 | 1 | Frame_Tail | u8 | 帧尾 `0x7D` |
加粗行为 v2 相对 v1 的新增字段。
## 3. 时间源
时间戳由自由运行的 **TIM7** 提供，见 [HARDWARE/timer.c](../HARDWARE/timer.c)：
- TIM7 挂在 APB1，定时器时钟 84 MHz，预分频 84 → **1 MHz 计数（1 tick = 1 μs）**。
- 16 位计数器每 65.536 ms 回绕一次，更新中断把高 32 位软件计数 `g_time_high` 加一。
- `mcu_time_us()` 返回 `(g_time_high << 16) | CNT`，即**自上电以来的单调递增微秒数**，
可用位宽 48 位（约 8.9 年不回绕）。
- TIM2~TIM5 被四路编码器占用、TIM8 被航模遥控输入捕获占用，故选空闲的 TIM7。
- TIM7 更新中断优先级设为 5（数值大于 `configMAX_SYSCALL_INTERRUPT_PRIORITY`
对应的抢占门限），ISR 内只做自增，不调用任何 FreeRTOS API。
> 注意：`mcu_time_us()` 只增不减，绝不因校时而跳变。上位机负责维护
> `T_host = a·t_mcu + b` 的时钟映射（见 [时间戳.md](时间戳.md) 第 7、8 节）。
## 4. 两个时间戳分别打在哪里
v1 只有"发送时刻"隐含语义，无法表达 IMU（100 Hz）与轮速（200 Hz）是**异步采集**的。
v2 在各自真实采样点打点，写入全局变量，`data_transition()` 打包时快照：
| 时间戳 | 打点位置 | 采样率 |
| ---- | ---- | ---- |
| `speed_sample_time_us` | [balance.c](../BALANCE/balance.c) `Get_Velocity_Form_Encoder()` 读编码器锁存计数器的瞬间 | 200 Hz |
| `imu_sample_time_us` | [MPU6050.c](../HARDWARE/MPU6050/MPU6050.c) `MPU6050_task` I2C 读取前 | 100 Hz |
因此一帧内同时带两个时间戳：上位机应把加速度/角速度对齐到 `imu_sample_time_us`，
把 X/Y/Z 速度对齐到 `speed_sample_time_us`，而不是共用一个发送时刻。
> IMU 打点在 I2C 突发读取之前，是在没有 DRDY（数据就绪）中断条件下能取到的、
> 最接近物理采样的时刻。若日后接入 MPU 的 DRDY 引脚中断，可把打点移到中断里进一步减小抖动。
## 5. session_id
- 定义在 [usartx.c](../HARDWARE/usartx.c)：`g_session_id`。
- 当前实现为**固定值 `0xA5A5A5A5`**。
- 用途：上位机据此判断下位机是否重启过（重启后 `mcu_time_us()` 从 0 重新计时，
时钟映射 `a, b` 必须重新拟合）。
- 待办：如需真正区分每次上电，应在启动时用熵源（如未初始化 RAM、启动 tick 等）
生成一次性随机值。当前固定值只能表明"协议版本一致"，不能区分不同启动会话。
## 6. 校验与解析建议
- 校验位 `buffer[42] = XOR(buffer[0..41])`，与 v1 的计算方式相同，只是范围从 22 扩到 42。
- 上位机解析步骤：
1. 同步帧头 `0x7B`；
2. 读满 44 字节；
3. 校验 `buffer[43] == 0x7D` 且 `XOR(buffer[0..41]) == buffer[42]`；
4. 按上表大端解出各字段。
- 多字节字段务必按大端手动组装，不要直接 `memcpy` 结构体（编译器填充/对齐/端序差异）。
## 7. 其他发送通道
| 通道 | 现状 |
| ---- | ---- |
| USART3（ROS 主口，DMA） | 已用 `SEND_DATA_SIZE`，自动发送完整 44 字节 |
| USART1 / USART5 | 已改用 `SEND_DATA_SIZE`，发送完整 44 字节 |
| CAN | 当前未被调用；`CAN_SEND()` 仍按 8×3=24 字节分包，**不含时间戳字段**。如需经 CAN 传时间戳需另行扩展分包 |
## 8. 上位机适配提醒
USART3 收到的帧已从 24 字节变为 **44 字节**。上位机（ROS 端）解析代码必须同步更新：
帧长、校验范围、以及尾部三个新字段的读取。否则会因帧长不符导致解析错位。

349
doc/时间戳.md Normal file
View File

@@ -0,0 +1,349 @@
针对现在的“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}
]