H7_BSP

面向达妙 MC-02 开发板的 STM32H7 板级支持与机器人控制框架，基于 STM32H723VGT6 / Cortex-M7 / 480 MHz。工程围绕外设通信、设备驱动、控制与估计算法、系统服务组织代码，供机器人项目组合和复用。

底层使用 STM32CubeMX、HAL 与 FreeRTOS，任务接口采用 CMSIS-RTOS V2，构建使用 CMake + Ninja。用户层保持 C 风格运算、结构体与自由函数，设备和算法保留简洁的 Class_ 封装。

打开 H7_BSP.ioc 遇到版本迁移提示时，选择 Continue，不要选择 Migrate。 迁移并重新生成可能使 Middlewares/ 中的 FreeRTOS 与现有 SystemView 适配不兼容。请保持项目原有固件包，详见 CubeMX 与构建边界。

整体架构 · 通信与外设 · 设备层 · 算法层 · 小陀螺 · 接入方式 · 构建与调试 · 主机回归

整体架构

框架以模块职责划分边界：BSP 处理外设收发，Device 处理设备协议与状态，Algorithm 提供计算组件，System 提供共享服务；Task 和 Application 负责调度与业务组合。图中展示模块组织和主要使用关系。交互图 可下载后在本地浏览器打开。

层次	职责	入口
Application / Task	组织控制逻辑、任务周期与模块协作	Application、Task
Device	封装电机、板载器件与外接工具	Device
Algorithm	提供控制、观测、滤波、数学与调度辅助组件	Algorithm
System	统一初始化、回调、时间戳与调试服务	System
BSP	管理外设实例、缓冲区、收发与回调注册	BSP
HAL / RTOS / 工程配置	外设初始化、任务调度、内存布局与构建	Core、User_Config、CMakeLists.txt

工程目录

User_File/
├── Application/            应用控制与通信接入
├── Task/                   CMSIS-RTOS V2 任务入口
├── Device/Onboard/         板载设备
├── Device/Peripheral/      外接电机与调试工具
├── Middleware/Algorithm/   数学、控制、观测与滤波组件
├── Middleware/BSP/         外设管理与收发接口
└── System/                 初始化、回调、时间戳、参数与调试
Core/                       CubeMX 生成的启动、外设与 RTOS 配置
Drivers/                    HAL / CMSIS 驱动
Middlewares/                FreeRTOS、USB Device、CMSIS-DSP 等依赖
USB_DEVICE/                 USB CDC 设备配置
User_Config/                链接脚本、FreeRTOS 补丁与烧录配置
SystemView/                 SEGGER SystemView 与 RTT
Tests/                      独立主机算法与通信边界回归
sysid/                      系统辨识数据、脚本与报告

通信与外设 BSP

BSP 以外设管理对象和接口函数承接 HAL，设备层通过注册回调与收发接口使用总线资源。

模块	提供的能力	使用入口
CAN / FDCAN	按总线与 ID 分发接收；命令队列与最新周期帧两条发送通道	bsp_can.h
UART	DMA + IDLE 不定长接收、双缓冲、错误恢复；DMA 发送复制到专用缓冲并返回提交状态	bsp_uart.h
SPI	外设管理、片选、收发缓冲与完成回调	SPI
USB	CDC 收发封装，供调试通信组件使用	USB
OSPI	外部存储器收发与自动轮询接口	OSPI
ADC	校准、DMA 采样与采样缓冲管理	ADC

CAN 的两条发送通道适用于不同数据语义：

• CAN_Tx_Perform() 更新 (FDCAN, ID) 对应的周期槽，同一键保留最新数据，适合连续控制目标。
• CAN_Tx_Submit() 将命令复制到 FIFO 队列，适合使能、复位与模式设置等需要按顺序处理的操作。
• CanTxTask 调用 BSP_CAN_SendAsync() / BSP_CAN_SendPer() 处理发送。调用方需要检查提交结果；进入软件缓冲与总线发送完成是不同阶段。

CAN 接收回调在中断上下文执行。UART 的 DMA 接收须同时具备 CubeMX 的 RX DMA 配置和 BSP 管理入口，接入新端口时需同步核对。

UART_Transmit_Data() 在有 TX DMA 时，将数据复制到该端口位于 .dma_buffer 的专用发送缓冲；调用返回后，调用方可复用原始数据。UART 或 DMA 忙时返回 HAL_BUSY，不覆盖正在发送的内容；启动失败返回对应 HAL 状态，由调用方决定重试。无 TX DMA 的端口保留阻塞发送路径。

设备层

设备层维护协议编解码、状态、反馈与设备操作，复用 BSP 通信接口和系统时间服务。

电机组件

驱动	支持范围	接入说明
DJI	M2006/C610、M3508/C620、GM6020；反馈、控制环与分组发送	DJI 电机驱动
达妙	MIT、位置-速度、速度、力位混控接口；实际模式取决于型号与固件	达妙电机驱动
QDrive	QD4310 协议与控制接口	QDrive

电机型号、CAN ID、反馈源、方向、映射范围和控制参数由使用方配置；应用层负责控制周期、目标生成与输出边界。

达妙动作/模式请求及 QDrive 命令接口返回 bool，表示是否成功提交到软件发送通道。提交失败时保留相应状态，调用方可据此重试；达妙置零仅在提交成功后重置位置展开状态。返回成功不代表电机已经执行或确认命令。

板载设备与外接工具

组件	功能
BMI088	加速度计、陀螺仪、温控与姿态组件；当前采集采用 FIFO / SPI DMA，姿态使用 VQF
W25Q64JV	基于 OSPI 的外部 Flash 驱动
Power	板载电源输出控制与 ADC 电压采样
WS2812 / Buzzer / Key	灯效、蜂鸣器与按键处理
EricTool	USB / UART JustFloat 输出与 variable:value# 文本指令解析；UART 输出返回发送状态

板载组件位于 Onboard，外接组件位于 Peripheral。硬件资源绑定和设备初始化集中在 Init.cpp。

EricTool 的 USB/UART 解析均只读取回调传入的缓冲区及有效长度，通过字符串指针字典匹配变量名；非法帧返回索引 -1、值 0，不会沿用上一帧结果。当前保持首帧语义，第一个 # 后的字节忽略，不提供跨回调拼帧。UART 周期输出的返回值沿用 HAL 状态，应用需处理忙或失败。

算法层

算法按用途独立组织，设备和应用通过输入、参数与计算接口组合使用。现有源码在根 CMake 中显式注册；组件进入构建不代表已经接入具体控制闭环。

分类	组件	内容
控制	PID	PID 与前馈、积分分离/变速积分、微分先行及可选 D 支路一阶 IIR 低通
控制	SMC	单轴二阶对象滑模控制，线性滑模面与饱和边界层
轨迹	Trajectory	单轴三阶 S 曲线，位置/速度目标，限制速度、加速度和 jerk，支持运动中改目标
模糊推理	Fuzzy	双输入、多输出零阶 Sugeno 推理，完整规则表与分片双线性插值
观测	DOB	一阶名义模型、零阶保持离散与 Q 滤波扰动估计
状态估计	Kalman、EKF	线性与扩展卡尔曼滤波组件
姿态估计	VQF	姿态与陀螺仪零偏估计
信号滤波	Frequency、IIR	FIR 频率滤波与 IIR 低通、陷波等组件
自适应滤波	OneEuro	标量 One Euro 低通，根据变化速率调节截止频率
多项式滤波与微分	Polynomial	0～3 阶等权拟合，输出通用标量平滑值及一、二、三阶时间导数
数学	Basic、Complex、Matrix、Quaternion	基础运算、复数、定长矩阵与姿态表示转换
辅助	Slope、FSM、Queue、Pulse	斜坡、状态机、队列与周期分频

使用算法时需要明确量纲、采样周期、状态初始化和输出限幅。模型相关约定以模块头文件为准，例如 DOB 使用 y[k] 与上一周期实际输入 u[k-1]，SMC 由调用方提供同一时刻的状态及其导数。

控制与轨迹约定

• PID：死区作用于有效误差，不修改调用者目标；积分在本周期累加后限幅，支持负 Ki，Ki=0 时清空积分。积分限幅为零表示不限制积分，积分分离和变速积分的阈值约定见头文件。
• D 支路滤波：D_Filter_Cutoff 使用 Hz，默认 0 关闭；与 D_First 微分先行独立配置。先滤波差分速率，再乘 Kd；DJI 的 PID_InitTypeDef 配置已透传该字段。首次启用或切换微分来源时滤波状态从零开始，持续启用且来源不变时保留滤波值。PID 参数更新不自动清空全部历史状态，死区也不保证总输出为零。
• Trajectory：独立于原有 Slope，一个对象管理一个轴。位置目标以零速度、零加速度到达；速度目标到达后保持匀速，设置零速度可平滑停止。目标在下一周期从当前规划的 p/v/a 接续，重复目标不重新规划。模块不分配堆内存、不创建任务，不保证时间最优或多轴同步；制动距离内改目标允许必要的越过与返回。接口、错误处理和接入示例见 轨迹说明。

滤波、估计与模糊推理约定

• One Euro：固定周期标量输入，以首帧对齐初值；最低截止频率、速率系数 Beta 与导数截止频率可配置。周期或参数改变时重新初始化。
• Polynomial：默认二阶、支持 0～3 阶，窗口最多 33 点，在最新样本时刻求值。0 阶为移动平均；未收满窗口时原量直通、导数清零且 Ready=false，高于拟合阶数的导数恒为零。调用方负责等间隔新样本、量纲与角度展开，缺测后重置；接口与验证见 多项式滤波说明。
• Kalman：每周期先预测，缺测时跳过测量更新，状态与协方差仍连续推进；恢复有效测量后再执行更新。
• Sugeno：调用方提供有序节点和完整规则表，节点/规则在使用期间保持有效且只读；输入超范围时保持边界值。输入缩放、微分、规则设计及 PID 增益映射由应用负责，库中没有预设的电机或云台控制规则。使用方式与独立参考对照见 模糊推理说明。

小陀螺 SpinMode

SpinMode 提供可复用的云台与底盘协调计算接口，复用现有 FSM、Basic 和 Matrix 数学库。调用方选择数据来源、更新输入并读取输出；模块不绑定 IMU、电机、任务或硬件，也不包含 PID 或其他角度/速度控制器。

坐标约定为 X 向前、Y 向左、逆时针为正；角度使用 rad，角速度使用 rad/s，平移速度使用 m/s。theta - zero_point 表示校准后的“云台朝向减底盘朝向”，计算时归一化为最短相对角。

模式	行为	gimbal_yaw_target	forward	chassis_yaw_error
SpinMode_GIMBAL_FOLLOW 云台跟随底盘	保持云台与底盘的相对夹角	进入模式后首次计算锁存的校准相对角	0	0
SpinMode_GIMBAL_LOCK 云台锁定	云台保持调用方设置的世界 Yaw 目标，底盘可独立旋转	Set_WorldTarget() 设置的连续角度	-vw	0
SpinMode_CHASSIS_FOLLOW 底盘跟随云台	输出底盘转向云台当前朝向的最短角误差，供调用方控制底盘	Set_WorldTarget() 设置的连续角度	-vw	校准后的最短相对角，正值表示底盘应逆时针转

小陀螺使用 云台锁定模式 + 非零 vw，无需单独的第四种模式。

• Init(&config) 配置零位偏移与初始模式；Init() 默认零偏移、云台跟随底盘。配置非法时返回 false 并保留原状态。
• Set_theta(theta) 更新云台与底盘的相对夹角；进入云台跟随模式后，应在首次计算前更新该值。重复设置同一模式不会重新锁存目标。
• Set_WorldTarget(world_angle) 保存上层期望保持的云台世界 Yaw，可由上层按 IMU 姿态生成；保留输入的连续角度，切换模式不会自动抓取当前 IMU 值。
• Set_MoveTarget(vx, vy, vw) 设置云台坐标系的平移指令和底盘角速度指令。三个模式均将平移指令旋转为底盘坐标系的 x/y，并原样输出 w = vw。
• Set_Spin_Mode(mode) 选择上述三种模式。调用方在同一上下文提供有限输入，然后调用 TIM_Calculate_PeriodElapsedCallback()，通过 Get_Output() 取得当次计算结果。

forward 依据传入的底盘角速度 vw 计算，叠加到云台相对底盘的速度目标。chassis_yaw_error 是角度误差，由调用方的控制器生成后续 vw；本模块不会自行将角度误差换算为角速度，也不会直接驱动底盘。

系统服务

服务	作用	入口
初始化	绑定 BSP、配置设备与建立系统服务	System/Init
回调分发	将 HAL 回调转交外设与设备处理逻辑	System/callback
时间戳	提供统一微秒时间，供周期测量与超时判断使用	System/Timestamp
参数配置	集中维护当前 IMU 采样、姿态与零偏估计参数	System/IMU
调试数据	导出便于 Watch、绘图与遥测读取的状态	System/debug
健康状态	20 Hz 汇总 IMU 与已注册 DJI 电机，发布全局 SYS_Health 快照	健康系统与验证
周期与任务	CMSIS-RTOS V2 任务入口、线程标志和周期回调	Task

main.c 完成 MPU、HAL 和外设初始化后调用 System_Init()，随后初始化 RTOS、创建任务并启动调度。CAN 发送资源在内核初始化后建立，周期服务与设备计算按职责由任务调度。

当前应用接入仍需按项目配置：Control_Task.cpp 中的 Gimbal / Balance 初始化与循环调用尚未启用。驱动和算法可作为框架组件使用，具体应用需要补齐资源绑定与调度。

接入方式

添加设备或通信协议

• 在 Device/Onboard 或 Device/Peripheral 中放置设备实现，参照同层组件组织配置、状态和接口。
• 复用现有 BSP 管理对象与回调注册；设备层负责协议解析，任务或应用层负责业务处理和控制目标。
• 在显式 Init() 中绑定资源，按依赖顺序接入系统初始化；需要 RTOS 对象的部分放在内核初始化之后。
• 按需求接入周期回调或任务，明确 ISR 与任务边界、缓冲区所有权和数据有效期。
• 在根 CMakeLists.txt 注册源码与 include 路径，并检查收发结果、超时和实际硬件行为。

组合控制与算法

• 在应用层组织目标、反馈和状态，调用设备接口及算法组件；已有数学与时间服务优先复用。
• 按模块约定初始化模型与参数，在确定的采样周期内更新输入、计算输出并提交给设备。
• 控制计算、通信发送和调试输出按实时性分配，接入后观察任务栈水位、周期与执行时间。

代码风格

沿用同层模块的 Struct_、Enum_、Class_ 命名和文件组织，内部优先使用直接的 C 风格运算。保留已有必要模板与类接口，新模块避免无必要的抽象层、动态分配和重复实现。

硬件操作放在 Init() 中，构造函数不访问硬件。C 调用入口通过 extern "C" 暴露，C 可见头文件保持兼容。任务与同步接口使用 CMSIS-RTOS V2，中断内调用 RTOS API 前核对优先级和接口限制。

工程基础

DMA 与内存布局

项目使用自有 h7_memory.ld，由 h7_linker.cmake 选择：

区域	地址	大小	用途
DTCMRAM	0x20000000	128 KiB	普通数据、栈、部分 FreeRTOS heap；DMA1/DMA2 不可直接访问
RAM_DMA	0x24000000	64 KiB	.dma_buffer；MPU 配置为共享、不可缓存
RAM_D1	0x24010000	256 KiB	.ram_d1_data 与部分 FreeRTOS heap
RAM_D2 / RAM_D3	0x30000000 / 0x38000000	32 / 16 KiB	其余 SRAM，使用时核对对应 DMA 的可达性

DMA1/DMA2 缓冲区应放入 .dma_buffer，并核对对齐、生命周期和传输长度；BDMA 等控制器需要单独确认内存可达性。RAM_DMA 的地址与大小必须与 main.c / .ioc 中的 MPU 配置一致，链接脚本包含一致性断言。

UART 的专用 TX 缓冲随管理对象放在该不可缓存区域，CPU 复制完成后通过内存屏障再启动 DMA；该路径无需额外清理 D-Cache。此约定依赖当前链接布局与 MPU 配置，修改内存属性时须一并复核。

FreeRTOS 使用 heap_5，默认总量 64 KiB，分为 48 KiB DTCMRAM + 16 KiB RAM_D1。DTCM 分区由 H7_FREERTOS_DTCM_HEAP_SIZE 配置，heap 与 port 补丁位于 FreeRTOS_Patch。

CubeMX 与构建边界

• 打开 IOC 时选择 Continue，禁止直接使用 Migrate 升级本工程。 当前 H7_BSP.ioc 记录 STM32CubeMX 6.15.0、STM32Cube FW_H7 V1.12.1，仓库内 FreeRTOS 为 V10.3.1。缺少原固件包时先安装对应版本，再继续打开。
• Migrate 会迁移项目使用的数据库与固件版本；重新生成时可能替换 Middlewares/ 中的 FreeRTOS，使其与现有 SystemView 跟踪宏、RTOS 适配和 port 补丁 不兼容。需要升级时应配套调整并验证这些组件，不能仅靠迁移后编译通过判断跟踪功能正常。迁移选项含义见 ST CubeMX 官方说明。
• 外设配置入口为 H7_BSP.ioc，生成代码的用户修改放在 USER CODE BEGIN/END 区域。
• 用户源码与 include 路径由根 CMakeLists.txt 显式维护；链接布局、RTOS 补丁维护在 User_Config/。
• CubeMX 重生成后重新 configure/build，检查用户集成、链接脚本选择和补丁接入。
• 工程链接 CMSIS-DSP Cortex-M7 浮点库，C++ 工具链禁用异常和 RTTI。

构建与调试

环境与构建

准备 CMake 3.22 或更高版本、Ninja 和 GNU Arm 工具链，确保 arm-none-eabi-gcc / arm-none-eabi-g++ 等命令可用。在仓库根目录执行：

cmake --preset Debug
cmake --build --preset Debug

产物为 build/Debug/H7_BSP.elf，链接映射为同目录下的 H7_BSP.map。Release 使用对应 preset：

cmake --preset Release
cmake --build --preset Release

CMakePresets.json 管理构建配置。Debug 使用 -Og -g3，Release 使用 -Os -g0。

主机回归

仓库内提供独立 CMake 测试工程，使用主机 C++ 编译器直接编译生产源码，按需以桩函数替换硬件接口。不要给这些工程加载固件 ARM 工具链。

工程	覆盖范围
Fuzzy	独立 Sugeno 参考模型、9 万个随机输入、多输出、非均匀节点及配置/输入边界
Boundary	PID 积分/死区/D 低通、KF 连续缺测、电机命令失败返回、EricTool 有界解析、UART DMA 发送寿命及忙/失败路径，共 5 组
Trajectory	输入契约、6 万组随机初态、1657 组边界初态、10 万次逐周期改目标、连续信号跟随及分段连续性，共 5 组
FilterPolynomial	0～3 阶独立系数、流式卷积、解析导数、生命周期及配置失败状态保留，共 5 组
SpinMode	坐标转换、三模式目标与前馈、最短角误差、模式切换及生命周期，共 4 组
Health	快照一致性、IMU 增量告警、DJI 只读超时检测、24 电机容量及任务调度，共 5 组

在仓库根目录运行下列 PowerShell 命令；将 g++ 替换为本机主机编译器路径：

foreach ($suite in @("Fuzzy", "Boundary", "Trajectory", "FilterPolynomial", "Health", "SpinMode")) {
    cmake -S "Tests/$suite" -B "build/Tests_$suite" -G Ninja -DCMAKE_CXX_COMPILER=g++ -DCMAKE_BUILD_TYPE=Release
    if ($LASTEXITCODE -ne 0) { throw "$suite 配置失败" }
    cmake --build "build/Tests_$suite"
    if ($LASTEXITCODE -ne 0) { throw "$suite 构建失败" }
    ctest --test-dir "build/Tests_$suite" --output-on-failure
    if ($LASTEXITCODE -ne 0) { throw "$suite 测试失败" }
}

2026-09-19 的功能与边界修复已进行主机验证及 MC02 Debug 编译链接。主机测试不代替实际 DMA/CAN 通信、电机闭环和实时性验证；新增算法仍需由应用接入，Trajectory 尚未测量板上的最坏重规划耗时。

烧录与观察

VS Code 任务 提供 DAPLink、ST-Link、J-Link 选择与烧录入口；调试配置 和 Ozone 工程 提供源码调试入口。使用前按本机安装位置检查工具路径、探针和目标芯片配置。

• Ozone / GDB：观察设备反馈、算法状态、系统调试数据与任务栈水位。
• SystemView / RTT：观察任务调度、中断和运行时信息。
• EricTool：通过 USB / UART 输出数据；TransportTask 中保留了 USB 周期输出的使用示例。
• sysid：系统辨识数据、采集分析脚本与实验报告。

文档与参考

• DJI 电机驱动 · 达妙电机驱动 · 更新记录。
• FreeRTOS heap memory management。
• ST AN4891：STM32H7 系统架构与性能。
• ST AN4839：STM32F7/H7 一级缓存。

致谢

感谢 Kylin-6 在 PR #4 中贡献达妙电机驱动及初版使用说明，并在 PR #5 中贡献 DJI 电机原始驱动。

部分驱动参考 达妙 MC02 BSP；UART 实现参考 SCUT-Robotlab / 达妙 drv_uart 的组织方式。感谢相关开源项目与原作者。

维护架构图

架构图由 Archify 生成。编辑 H7_BSP.architecture.json，使用 版本记录 对应的 Archify 技能目录执行：

.\Tools\Architecture\Build.ps1 -ArchifyRoot "<Archify 技能目录>"

Build.ps1 更新 Assets/Architecture/ 下的 SVG 与交互 HTML。