# Path_Following

**Repository Path**: xiaopengyouGU/Path_Following

## Basic Information

- **Project Name**: Path_Following
- **Description**: 这是一个简单的开源智能小车循迹项目，采取分层与模块化的设计思想实现。功能包括增量式PID、步进闭环定位、BootLoader（LTM）、IAP在线升级、循迹与差速控制等。本项目与LTM-Monitor上位机配套使用。
- **Primary Language**: Unknown
- **License**: MIT
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-10-10
- **Last Updated**: 2026-11-02

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# STM32G474 智能小车固件

面向 STM32G474RBT3 的智能小车固件，包含 12 路灰度循迹、双轮差速控制、两轴步进云台、BNO08x 航向采集、MaxiCam 视觉打卡、定点里程计、上位机通信，以及带 A/B 分区和失败回滚的 UART IAP。

## 1. 系统概览

| 项目 | 配置 |
| --- | --- |
| MCU | STM32G474RBT3，Cortex-M4F，128 KB Flash / 96 KB SRAM |
| 系统时钟 | 24 MHz HSE → PLL → 170 MHz |
| 实时控制 | TIM6，1 kHz，NVIC 优先级 0 |
| 任务计时 | SysTick，1 ms，NVIC 优先级 15 |
| 上位机链路 | USART1，115200 bps，LTM 帧协议 |
| 云台反馈 | 两路 AS5600，12 bit |
| 车体航向 | BNO08x UART-RVC |
| 固件升级 | USART1 → LTM → UDS PDU，BootLoader 支持 A/B 分区 |

应用主要完成以下工作：

- 采集 12 路灰度传感器，计算横向偏差并执行循迹控制；
- 通过编码器测速，对左右轮分别执行速度闭环；
- 使用 AS5600 反馈控制两轴步进云台；
- 接收上位机运动命令，周期上报运行状态和曲线数据；
- 通过独立 BootLoader 在 A/B 分区之间切换固件。

## 2. 软件架构

```mermaid
flowchart TB
    HOST["上位机 / IAP 客户端"]

    subgraph APP["Application"]
        MAIN["car/main<br/>初始化、命令分发、周期上报"]
        CAR["car<br/>业务入口、参数检查、单位换算"]
        LTM["components/protocol<br/>LTM 帧编解码"]
        CTRL["tasks/control_tasks<br/>控制循环、状态与故障处理"]
        SCHED["tasks/sched_task<br/>毫秒任务调度"]
        ALGO["components/algo<br/>PLL 测速、PID、轨迹规划"]
        ODOM["components/algo/odometry<br/>Q12 位姿、Q15 航向"]
        MISSION["tasks/mission<br/>打卡与返航状态机"]
        DEV["devices<br/>车轮、灰度、云台、IMU、通信"]
        VISION["devices/vision<br/>MaxiCam USART3 DMA"]
        BSP["drivers/BSP<br/>定时器、PWM、编码器、UART、DMA、GPIO、I2C"]
        HAL["STM32G474 HAL / CMSIS"]
    end

    HOST <-->|USART1 / LTM| MAIN
    MAIN --> CAR
    MAIN <--> LTM
    MAIN --> DEV
    CAR --> CTRL
    CAR --> SCHED
    CAR --> MISSION
    CTRL --> ALGO
    MISSION --> CTRL
    MISSION --> ODOM
    MISSION --> VISION
    CTRL --> DEV
    SCHED --> DEV
    DEV --> BSP
    BSP --> HAL

    TIM["TIM6 中断<br/>1 kHz"] --> CTRL
    TICK["SysTick 中断<br/>1 ms"] --> SCHED
```

各层职责如下：

| 目录 | 职责 |
| --- | --- |
| `src/app/car/` | 应用入口和业务门面。向上位机暴露 `car_set_speed()`、`car_set_angle()`、`car_start()` 等接口，并完成物理单位与内部控制量之间的转换。 |
| `src/app/tasks/` | 实时控制、任务调度和状态机。`control_tasks.c` 是底盘与云台控制的核心。 |
| `src/app/components/algo/` | 可复用算法，包括 PLL 测速、定点 PID 和步进轨迹规划。控制热路径主要使用 Q15/Q24 定点运算。 |
| `src/app/components/algo/odometry/` | 定点轮式里程计。位置使用 Q12，航向使用标准 Q15，并在 IMU 无效时使用轮速差降级。 |
| `src/app/devices/` | 设备语义层。对任务层提供轮速、灰度偏差、云台位置和 IMU 状态，不直接暴露 HAL 寄存器。 |
| `src/app/devices/vision/` | MaxiCam 串口帧解析和测量快照，不包含运动决策。 |
| `src/app/drivers/` | 板级资源与中断处理，包括 BSP 统一初始化、A/B 槽提交、TIM1 PWM、TIM2/TIM3 编码器、TIM4/TIM17 步进脉冲、USART、DMA、GPIO 和软件 I2C。 |
| `src/app/common/` | 公共类型、控制参数和固定点换算宏。 |
| `src/app/tasks/mission/` | 打卡与返航任务状态机，50 Hz 运行，通过现有控制接口驱动底盘。 |
| `src/boot/` | 独立 BootLoader 工程，包括 UDS 服务、LTM 传输、Flash 端口、主机升级工具和主机侧测试。 |
| `third_party/` | STM32G474 HAL、CMSIS 和启动文件。 |

依赖方向保持单向：

```text
main / car
    -> tasks / protocol
        -> components / devices
            -> BSP
                -> STM32 HAL
```

控制策略集中在 `tasks` 和 `components/algo`，硬件差异收敛在 `drivers/BSP`。移植到同类平台时，原则上只需要替换 BSP、启动文件和链接脚本。

## 3. 运行时结构

系统没有引入 RTOS，实时性和低速设备访问分开处理。

| 执行上下文 | 周期 | 主要工作 |
| --- | ---: | --- |
| TIM6 中断，优先级 0 | 1 kHz | 执行 `control_tasks_run()` |
| 循迹外环 | 200 Hz | 灰度采样、偏差解算、PD、左右轮目标更新 |
| 轮速内环 | 1 kHz | 编码器快照、PLL 测速、PI、PWM 占空比更新 |
| 云台前馈 | 1 kHz | 轨迹规划器推进、速度前馈叠加位置 P 修正 |
| 状态监控 | 1 kHz | 停机判定、角度快照、步进故障检查 |
| SysTick 中断，优先级 15 | 1 ms | 任务表倒计时 |
| 主循环任务 | 100 Hz | `dev_step_poll()` 读取 AS5600，`control_tasks_axis_poll()` 更新位置 P |
| 视觉解析 | 50 Hz | `dev_vision_poll()` 消费 USART3 DMA 数据 |
| 任务规划 | 50 Hz | `car_poll()` 节拍调用 `mission_task()`，并向应用层返回 `car_event_t` |
| 主循环 | 条件触发 | LTM 收包、命令执行、状态文本和 10 路曲线数据上报 |

USART 接收中断优先级为 4。BNO08x 使用 USART2 DMA 接收 19 字节定长帧，并通过 IDLE 中断收尾；超过 200 ms 没有有效帧时，IMU 数据标记为失效。

两路 AS5600 通过软件 I2C 读取，因此放在主循环的 100 Hz 任务中执行。1 kHz 中断只使用最近一次已完成的位置快照，避免软件 I2C 阻塞实时控制。

## 4. 控制链路

### 4.1 循迹与底盘

```text
12 路灰度掩码
    -> 检查命中位连续性
    -> 计算横向偏差
    -> 中心死区
    -> atan 几何角度
    -> PD 差速量
    -> 左右轮目标转速
    -> 200 Hz 加速度限幅
    -> 1 kHz 轮速 PI
    -> 8 kHz PWM
```

灰度模块只接受连续点亮段。全亮、全灭和断点数据会被判为无效。丢线后，系统保持最后一次有效偏差，并根据丢线前的转向方向执行原地找线。

左右轮使用 TIM2/TIM3 正交编码器。编码器计数经 PLL 得到 `count/s`，再转换为 Q24 圈/秒；两侧速度 PI 输出 Q15 占空比，由 TIM1 的两路 8 kHz PWM 驱动。

`car_set_mode(mode, arg)` 是底盘统一命令入口：

- `Mode_Open_Loop`：直接设置左右轮占空比；
- `Mode_Speed`：设置左右轮目标转速；
- `Mode_Line_Following`：取消任务并进入普通循迹；
- `Mode_Mission`：启动单圈打卡，`arg` 为打卡数量，范围 1..255；
- `Mode_Mission_Fail`：中止打卡，转入失败返航；
- `Mode_Uturn`：按 BNO 航向闭环原地掉头；
- `Mode_Stop`：停机。

底层控制任务只接收开环、速度和循迹等基础模式；任务、返航和 U-turn 由 car 层分派。

### 4.2 状态机

```mermaid
stateDiagram-v2
    [*] --> Init
    Init --> Idle: Init_Done
    Idle --> Running: Start
    Running --> Stop: 两轮低于停机阈值
    Stop --> Running: Start
    Idle --> Error: Fault
    Running --> Error: Fault
    Stop --> Error: Fault
    Error --> Idle: Fault_Clear
```

停机不是立即把状态设为 `Stop`。发送 Stop 后，系统先关闭 PWM、清零控制和 PID 状态，等两轮速度都低于 1 RPM，再由监控任务把状态切换到 `Stop`。

步进驱动器进入错误状态后，App 会锁存首个错误并停止云台。当前主机命令中没有开放 `Fault_Clear`。

### 4.3 两轴云台

```text
目标角度
    -> 12 bit 编码器计数
    -> 单圈最短路径
    -> 在线轨迹规划
    -> 速度前馈
    -> 100 Hz 位置 P 修正
    -> 双向 pps
    -> TIM4 / TIM17 脉冲输出
```

云台位置以 AS5600 单圈计数为基准，跨圈位置通过相邻采样的最小差值展开。轨迹规划器按 1 ms 推进，当前配置为梯形加减速，最大速度 100 RPM，加减速时间 200 ms。

位置 P 使用 4 count 死区，避免编码器静止抖动导致持续修正。首次上电时，规划器必须等 AS5600 产生有效样本后才以真实位置启动，避免从 0 位置拉起云台。

### 4.4 打卡任务与定点里程计

规划层通过 CMake 选项 `APP_MISSION_ENABLED` 控制，默认开启；需要临时旁路时传入 `-DAPP_MISSION_ENABLED=OFF`。

打卡任务运行在独立的 `mission_task()` 中，不改变原循迹环和速度环：

```text
循迹 -> 目标达到距离阈值 -> 减速
     -> 车体按中心 X 修正，丢失后相对 ±90° 搜索
     -> 对准 -> 低速插入 -> 停车 2 s
     -> 后退 -> 重新捕获灰线
```

上位机通过 `F=<n>` 下发本次任务的打卡总数，同时将下发瞬间定义为任务坐标原点。打卡任务只执行一圈；全部打卡成功后自动返航，未完成指定数量也会在回到原点附近后停机。云台在任务中只保持使能，不参与搜索或对准。

`KFAIL` 会中止任务。失败流程先发送 `check failed!!!`，停车并转向返程方向，然后恢复正常循迹返航；如果短暂丢线，由循迹控制器处理。卡尔曼 X/Y 用于回到起点附近，距任务原点 60 mm 内停机并发送 `return home`。

里程计全部使用定点数：

- 位置和累计路程使用 Q12，单位为米；
- BNO08x 车体航向转换为 Q15 整圆角；
- 轮径 65 mm，轮距 105 mm，编码器 56000 count/rev；
- 航向统一使用 `-180°..180° ↔ -32768..32767`，所有相对角运算后都做整圆归一化；
- 不再保存二维航迹表，任务层只使用实时 X/Y 与最后一次在线位置。

当前版本使用 BNO yaw 作为航向观测、编码器差速作为无 IMU 时的降级估计，没有引入完整 EKF。对于 1.5 m × 2 m 场地，这种方式运算量更低，也便于直接离线检查。后续如果需要输出位置置信度，可以在现有 `lt_odometry` 接口后增加二维 EKF，不影响任务层。

`mission_param_def.h` 中的视觉像素阈值、插入距离和后退距离需要在实车上标定。方向安装不一致时，优先调整 `MISSION_YAW_SIGN` 和 `VISION_X_SIGN`，不要在任务状态机里临时取反。

打卡任务中的 yaw 云台只使能驱动器、停止脉冲和位置规划器，不主动摆动。目标搜索由车体完成；如果实际接线中 yaw/pitch 轴编号互换，通过 `STEP_AXIS_YAW` 统一修正。

## 5. 上位机通信

LTM 数据帧格式如下：

| 字段 | 长度 | 说明 |
| --- | ---: | --- |
| Header | 2 B | `0xA5B9`，小端发送为 `B9 A5` |
| Data type | 1 B | 数据类型 |
| Length | 1 B | 负载长度，最大 128 B |
| Payload | N B | 业务数据 |
| CRC | 2 B | Modbus CRC16 |

文本命令：

| 命令 | 作用 |
| --- | --- |
| `V=<left>,<right>` | 进入速度模式并设置左右轮目标，单位 RPM |
| `O=<left>,<right>` | 进入开环模式并设置左右轮占空比，单位 % |
| `Y=<deg>` | 设置云台 yaw 目标 |
| `P=<deg>` | 设置云台 pitch 目标 |
| `F` | 取消任务规划，进入持续普通循迹 |
| `F=<n>` | 配置一圈 n 点打卡任务（n=1..255），位置作为本次任务原点 |
| `KFAIL` | 中止打卡并返航 |
| `S` | 请求停机 |
| `I` | 输出 IMU 诊断信息 |
| `H` 或 `?` | 输出命令帮助 |

二进制定义包括 Start、Stop 和 Reset。`V`、`O`、`F` 只修改模式和目标值，不代替 Start。模式切换仅在 `Idle` 或 `Stop` 状态有效，轮速目标仅在 `Running` 或 `Stop` 状态有效；运行中切换模式需要先停机。

周期曲线帧使用 `Data_Channel_ALL`，负载为 10 个单精度浮点数：

```text
[Kalman X(m), Kalman Y(m), 左轮转速, 右轮转速, 循迹偏差,
 MaixCAM 中心 X(px), MaixCAM 中心 Y(px), yaw, pitch, 车体 yaw]
```

运行状态会以文本周期输出，包括状态、模式、错误码和系统 tick，便于在不接入上位机绘图工具时直接排查问题。

MaxiCam 使用 `B9 A5 | 08 | 12 | 18-byte payload | CRC16 low/high` 二进制帧。载荷依次为：

```text
sub_id, seq, status, flags, center_x, center_y, major,
distance_mm, quality, station_id, aligned_frames
```

多字节字段均为小端，`center_x/center_y` 单位为 0.1 px。CRC 使用 Modbus CRC16，覆盖帧头和载荷。解析结果保留最近一帧，超过 150 ms 没有新帧即判为失效。`C` 命令可查看帧数、解析错误、CRC 错误和 DMA 环形缓冲溢出情况。

打卡成功、全部完成、错误和返回原点由 `car_poll()` 翻译成 `car_event_t`。应用层只根据 car 事件输出文本，不占用 `Data_User_Defined` 或 IAP 自定义通道：

```text
checkpoint success: <done>/<total>
check failed!!!
return home
return home, checked all!!!
```

## 6. BootLoader 与 A/B 升级

### 6.1 Flash 分区

| 区域 | 起始地址 | 大小 |
| --- | --- | ---: |
| BootLoader | `0x08000000` | 20 KB |
| Boot 元数据 | `0x08005000` | 2 KB |
| App Slot A | `0x08005800` | 42 KB |
| App Slot B | `0x08010000` | 42 KB |

App 和 BootLoader 共用同一份分区定义。SRAM 顶部 4 字节 `0x20017FFC` 用作软跳转标记，初始栈顶固定在 `0x20017FF8`，满足 8 字节栈对齐要求。

### 6.2 升级流程

当前固件启用 UART IAP，CAN-FD/ISO-TP 代码已保留，但 `BL_ENABLE_UDS` 为 0。UART 通道将 UDS PDU 封装在 LTM 的 `Data_User_Defined` 帧中。

```text
Reset -> BootLoader
    -> 6 秒升级窗口
    -> 0x10 进入编程会话
    -> 0x27 安全访问
    -> 0x34 请求下载
    -> 0x36 分块传输
    -> 0x37 回读 CRC 并标记 PENDING
    -> 0x11 复位
    -> 启动新固件
    -> bsp_hal_init() 内部提交当前槽为 VALID
```

下载目标始终是当前活动分区的另一槽，已确认的 App 不被覆盖。Boot 元数据记录 `active`、`boot`、两槽状态、镜像长度和 CRC。

新镜像以 `PENDING` 状态启动。App 运行到 `bsp_hal_init()` 后提交当前槽为 `VALID`。如果新镜像连续 3 次未能提交，BootLoader 会清除该槽的待验证状态并回到原活动分区。

App 收到 Reset 命令后重新进入 BootLoader。若 6 秒内没有升级命令且已有有效 App，BootLoader 会自动跳转。

主机升级工具位于 `src/boot/host/upgrade_client.py`，BootLoader 的主机侧功能测试和压力测试位于 `src/boot/host/test/`。

## 7. 构建与烧录

构建依赖：

- CMake 3.20 或更高版本；
- `arm-none-eabi-gcc` 工具链；
- MinGW Makefiles 生成器；
- Python 3；
- J-Link（仅烧录时需要）。

常用命令：

```bash
python script.py -b
python script.py -b --boot
python script.py -R --app
python script.py -R --boot
python script.py -j --boot
python script.py -c
```

CMake 缓存统一放在根目录 `build/`：

```text
build/
├── app/       # 主应用
└── boot/      # BootLoader
```

构建产物：

```text
bin/path_following_A.hex
bin/path_following_B.hex
build/app/path_following_g474_A.elf
build/app/path_following_g474_B.elf
build/boot/bin/bootloader.hex
```

首次烧录应先写 BootLoader，再写 App A。`python script.py -R --app` 会清除 A/B 元数据，避免旧 CRC 记录阻止 J-Link 直接烧录的开发镜像。

J-Link 路径可通过 `JLINK_PATH` 环境变量指定，也可以传给脚本的 `--jlink` 参数。

## 8. 硬件资源

| 功能 | 引脚 / 外设 |
| --- | --- |
| 左轮编码器 | PA0/PA1，TIM2_CH1/CH2 |
| 右轮编码器 | PA6/PA7，TIM3_CH1/CH2 |
| 左轮 PWM / 方向 | PA8 TIM1_CH1；PB12/PB13 |
| 右轮 PWM / 方向 | PA11 TIM1_CH4；PB14/PB15 |
| 云台轴 1 脉冲 / DIR / EN | PB6 TIM4_CH1；PC0 / PC1 |
| 云台轴 2 脉冲 / DIR / EN | PB9 TIM17_CH1；PC2 / PC3 |
| AS5600 A | PC4/PC5，软件 I2C |
| AS5600 B | PC6/PC7，软件 I2C |
| 上位机 / IAP | USART1，PA9/PA10，115200 bps |
| BNO08x | USART2，PB3/PB4 |
| MaxiCam | USART3，PB10/PB11，115200 bps |
| 运行 / 错误 LED | PC11 / PC12 |

12 路灰度的完整引脚表见 [`docs/pins_g474.md`](docs/pins_g474.md)。

软件中 `STEP_AXIS_YAW` 对应下层步进轴 0（TIM4/PB6），`STEP_AXIS_PITCH` 对应上层步进轴 1（TIM17/PB9）；遥测和 `Y=`/`P=` 指令使用同一组轴常量。

实车线束中步进与 AS5600 的编号是交叉的：步进轴 0 对应 AS5600 通道 1，步进轴 1 对应 AS5600 通道 0。`dev_step.c` 的轴配置已按实物接线映射，不能用两个相同的数组下标直接对应。

## 9. 调试与标定

`I` 命令可查看 IMU 有效标志、yaw、接收长度、帧头匹配和校验统计。

以下参数与具体车体强相关，换板或调整机械结构后需要重新标定：

- 左右编码器方向；
- 左右电机方向；
- 两路 AS5600 安装方向；
- 云台机械限位；
- 灰度传感器间距和中心位置；
- 轮速 PI、循迹 PD 和云台位置 P 参数。

当前云台限位仍是占位配置 `0..4095`，正式使用前必须根据实际机械行程修改。