# STM32F405 LPV-MPC 嵌入式项目说明

本文档面向第一次接手该工程的人，说明当前进展、目录结构、OSQP 求解器接入方式、实时性优化方法、验证流程以及常见问题处理。

## 1. 当前状态

当前 Keil 工程已经可以在 STM32F405RG 目标上完整编译通过。

工程入口：

```text
STM32_LPV_MPC/keil_project/MDK-ARM/LPV_MPC_STM32F405.uvprojx
```

最近一次命令行 Rebuild 结果：

```text
Program Size: Code=66260 RO-data=18196 RW-data=28512 ZI-data=107408
"Objects\LPV_MPC_STM32F405.axf" - 0 Error(s), 0 Warning(s).
```

当前为了适配 STM32F405RG 的 SRAM 容量和 20 ms 采样周期，MPC horizon 已从原 PC 参考规模降低为：

```c
MPC_NP = 10
MPC_NC = 5
MPC_NDU = 15
MPC_NYH = 70
MPC_NXH = 20
```

对应配置文件：

```text
keil_project/MPC/Inc/mpc_config.h
```

## 2. 目录结构

```text
STM32_LPV_MPC/
├─ keil_project/
│  ├─ Core/Inc, Core/Src              STM32CubeMX 生成的 HAL 工程入口
│  ├─ Drivers/                        STM32F4 HAL/CMSIS
│  ├─ MDK-ARM/                        Keil 工程、编译输出、map/log
│  ├─ MPC/Inc, MPC/Src                LPV-MPC 控制器主体
│  ├─ MPC/osqp_embedded               Keil 工程引用的 OSQP 嵌入式接口
│  └─ SEGGER_RTT/                     RTT 实时日志输出
├─ matlab_export/                     LPV 数据导出辅助脚本
├─ osqp_codegen/                      OSQP codegen 脚本与生成代码
├─ validation/                        RTT 日志、MATLAB/STM32 对比脚本和图
└─ STM32_LPV_MPC_项目说明.md
```

核心源码：

| 文件 | 作用 |
| --- | --- |
| `Core/Src/main.c` | 20 ms 周期调度、DWT 计时、RTT/UART 日志、deadline 统计 |
| `MPC/Src/mpc_controller.c` | 单步 MPC 主流程：LPV 插值、预测矩阵、QP 构造、OSQP 求解、状态更新 |
| `MPC/Src/lpv_interp.c` | 3D LPV 插值，调度量为 `XNLC, A_MSV, A8` |
| `MPC/Src/mpc_predict.c` | 构造 `Phi/Gamma/CG` 预测矩阵 |
| `MPC/Src/mpc_qp_build.c` | 构造 QP 代价矩阵 `H` 和线性项 `f` |
| `MPC/Src/uart_print.c` | 输出 STM32 对比日志 |
| `MPC/Inc/mpc_config.h` | horizon、权重、约束、初始点、目标点、实时性参数 |

## 3. 控制问题定义

状态：

```text
x = [NL, NH]
```

控制量：

```text
u = [WFM, A_MSV, A8]
```

输出：

```text
y = [NL, NH, T4, FN, SM_FAN, SM_IPC, SM_HPC]
```

当前约束：

```text
T4 <= MPC_Y_MAX_T4
SM_FAN >= MPC_Y_MIN_FAN
SM_IPC >= MPC_Y_MIN_IPC
SM_HPC >= MPC_Y_MIN_HPC
u_min <= u <= u_max
|du| <= du_max
```

模式切换/控制启动时间：

```text
MPC_T_SW_START  = 2.0 s
MPC_T_SW_END    = 5.0 s
MPC_SERIAL_FUEL = 2.0 s
MPC_TS          = 0.020 s
```

## 4. OSQP 求解器使用方式

OSQP 代码由 `osqp_codegen/generate_osqp.py` 生成，生成目录为：

```text
osqp_codegen/osqp_embedded/
```

Keil 工程中的：

```text
keil_project/MPC/osqp_embedded/workspace.c
keil_project/MPC/osqp_embedded/workspace.h
```

是轻量 wrapper，直接包含 `osqp_codegen/osqp_embedded` 下的生成结果。这样重新 codegen 后，Keil 不会继续使用旧 workspace 副本。

注意：

```text
keil_project/MPC/osqp_embedded/emosqp.c
```

不能直接包含 codegen 生成的 PC 示例 `emosqp.c`，因为该文件会定义自己的 `main()`，与 `Core/Src/main.c` 冲突。当前 Keil 侧 `emosqp.c` 只包含 `workspace.h`。

单步求解流程在 `mpc_controller.c`：

1. `lpv_interp()` 根据当前 `NL, A_MSV, A8` 插值得到 `A/B/C/D` 和 offset。
2. `mpc_build_prediction()` 构造预测矩阵。
3. `mpc_build_qp()` 构造 `H_qp` 和 `f_qp`。
4. 按 codegen 固定稀疏结构填充 `P_vals` 和 `A_vals`。
5. 调用：

```c
osqp_update_data_mat(&solver, P_vals, OSQP_NULL, MPC_OSQP_P_NNZ,
                     A_vals, OSQP_NULL, MPC_OSQP_A_NNZ);
osqp_update_data_vec(&solver, f_osqp, l_osqp, u_osqp);
osqp_warm_start(&solver, ws_osqp, OSQP_NULL);
osqp_solve(&solver);
```

OSQP 的矩阵结构必须固定，只允许更新数值。若修改 `Np/Nc/NX/NU/NY/输出约束个数`，必须重新生成 OSQP workspace。

## 5. 已采用的实时性和内存优化

当前 F405RG 上采用了以下工程化加速/瘦身手段：

1. 降低 horizon：`Np=10, Nc=5`，显著降低 OSQP workspace、`Gamma/CG/H` 等静态数组占用。
2. 固定稀疏结构：每步只更新 OSQP `P/A/q/l/u` 的数值，不动态分配稀疏矩阵。
3. 全部大数组静态分配：避免栈溢出和运行时 malloc。
4. OSQP warm start：上一拍最优 `DU` 左移一格作为下一拍初值。
5. 关闭 OSQP scaling：`solver.settings->scaling = 0`，减少运行时额外开销。
6. 限制最大迭代数：`MPC_OSQP_MAX_ITER = 100`，避免过渡段单拍求解时间失控。
7. 2 s 启动处权重斜坡：`MPC_QY_RAMP_TIME = 0.20 s`，将 `Qy/Qu/Pu` 从 0 平滑过渡到 1，避免在 2 s 处代价函数硬切换造成 OSQP 迭代尖峰。
8. DWT cycle counter 计时：`s->solve_us` 是控制器真实执行时间，不包含 UART 阻塞发送。
9. UART 使用 DMA，RTT 用于高可靠日志输出。

## 6. 为什么 2 s 附近曾出现求解时间尖峰

旧逻辑中：

```c
int qy_active = (t_now >= MPC_T_SW_START) ? 1 : 0;
```

在 2.0 s 时，输出跟踪权重 `Qy`、输入跟踪 `Qu/Pu` 和 `FN` 参考同时从“基本不参与”跳到“完全参与”。OSQP 前面 2 s 解的是一个非常弱跟踪的问题，2 s 后突然变成强跟踪问题，`P/q` 大幅变化，warm start 质量很差，因此过渡段容易触发大量迭代，日志中曾出现约 50 ms 的单拍耗时。

当前处理方式：

```c
MPC_QY_RAMP_TIME = 0.20f
MPC_OSQP_MAX_ITER = 100
```

即 2.0 s 到 2.2 s 之间平滑激活跟踪权重，同时限制最坏迭代次数。这样可以降低过渡段单拍超时风险。代价是目标跟踪会有一个很短的软启动过程，这是在 F405RG 实时性约束下的工程折中。

如果实测仍有 deadline miss，优先按以下顺序处理：

1. 将 `MPC_OSQP_MAX_ITER` 从 100 降到 80。
2. 将 `MPC_QY_RAMP_TIME` 增大到 0.30 或 0.50 s。
3. 进一步降低 `Np/Nc`，例如 `Np=8, Nc=4`，并重新 codegen。
4. 放宽 OSQP 收敛容差，或接受 `OSQP_MAX_ITER_REACHED` 时的可行近似解。

## 7. STM32 日志和 MATLAB 对比

当前固件输出 12 列 CSV：

```text
t,NL_pct,NH_pct,AMSV_in2,A8,WFM_lbs,T4_K,FN_N,SM_FAN,SM_IPC,SM_HPC,solve_us
```

RTT Viewer 保存到：

```text
validation/stm32_rtt.txt
```

运行对比脚本：

```matlab
run('\\PANG-206\Users\Public\VCE_CJA\STM32_LPV_MPC\validation\compare_results.m')
```

脚本会：

1. 读取 STM32 RTT/UART 日志。
2. 读取 MATLAB/PC 参考结果 `V3_plot.mat/res_concurrent`。
3. 对比 `NL, NH, WFM, A_MSV, A8, T4, FN, SM_FAN, SM_IPC, SM_HPC`。
4. 计算每个变量的 `RMSE, MAE, MaxAbsError, NRMSE, Correlation, Consistency`。
5. 生成 SCI 风格图片：

```text
validation/figures/response_comparison.png
validation/figures/response_comparison.pdf
validation/figures/solve_time.png
validation/figures/solve_time.pdf
validation/figures/consistency_metrics.csv
```

如果使用旧固件采集的 8 列日志，脚本仍可运行，但 `A8` 和三个喘振裕度指标会显示为 `NaN`。烧录当前新固件并重新采集日志后，这些变量会自动参与对比。

## 8. 修改 horizon 的正确流程

不能只改 `mpc_config.h`。只要修改以下任一参数：

```text
MPC_NP
MPC_NC
MPC_NX
MPC_NU
MPC_NY
MPC_OSQP_NCON_Y
```

就必须同步：

1. 修改 `keil_project/MPC/Inc/mpc_config.h`。
2. 修改 `osqp_codegen/generate_osqp.py` 中的问题规模。
3. 重新运行 codegen，生成新的 `osqp_codegen/osqp_embedded/workspace.c/.h`。
4. 确认 `workspace.c` 中维度匹配，例如当前应看到：

```c
OSQPFloat sol_x[15];
OSQPFloat sol_y[70];
```

5. Rebuild Keil 工程。

若只改控制器维度但未重新生成 OSQP，常见现象是编译通过但运行异常，或链接时出现 workspace 数组尺寸不匹配。

## 9. 常见问题

### L6200E: Symbol main multiply defined

原因：Keil 编译了 OSQP codegen 的 PC 示例 `emosqp.c`，其中也定义了 `main()`。

处理：Keil 侧 `MPC/osqp_embedded/emosqp.c` 不要包含 codegen 示例入口，只保留嵌入式 wrapper。

### L6406E/L6407E: No space in execution regions

原因：OSQP workspace、MPC 预测矩阵和日志数组超过 F405RG SRAM/CCM 容量。

处理：

1. 降低 `Np/Nc` 并重新 codegen。
2. 缩小 `g_mpc_log` 或减少日志字段。
3. 缩小 RTT/UART buffer。
4. 检查 Keil 是否仍在编译旧 `workspace.c`。

### 结果与 MATLAB 不一致

先检查：

1. MATLAB 参考是否与当前嵌入式 horizon 一致。当前固件是 `Np=10, Nc=5`，而部分历史 `V3_plot.mat` 可能是 `Np=20, Nc=15`。
2. OSQP workspace 是否重新生成。
3. `A_vals` 填充顺序是否仍与 `generate_osqp.py` 的稀疏结构一致。
4. RTT 日志是否是当前 12 列格式。

### 2 s 处仍有超时

优先调整：

```c
MPC_OSQP_MAX_ITER
MPC_QY_RAMP_TIME
```

然后重新烧录并查看：

```text
g_mpc_max_us
g_mpc_deadline_miss
validation/figures/solve_time.png
```

### UART 日志缺行

UART 是 DMA 异步发送，若串口带宽不足可能跳过部分 UART 行；RTT 日志更可靠。验证时优先保存 RTT Viewer 输出。

## 10. 推荐接手流程

1. 打开 Keil 工程并 Rebuild，确认 0 error。
2. 烧录固件，打开 RTT Viewer。
3. 运行 10 s 仿真，保存 RTT 输出为 `validation/stm32_rtt.txt`。
4. 运行 `validation/compare_results.m`。
5. 检查：

```text
g_mpc_deadline_miss == 0
max(solve_us) < 20000
response_comparison.png 中 STM32 与 MATLAB/PC 趋势一致
consistency_metrics.csv 中关键输出 RMSE 可接受
```

6. 若要提高控制性能，再逐步增大 horizon 或减小 ramp 时间；每次都必须重新评估 RAM 和实时性。