替换 Mild v12 模型 + 同事编码器刹车红绿灯状态机合并

2026-06-13 13:05:18 +08:00
parent 457d608621
commit b9e32c5a40
66 changed files with 1698 additions and 5625 deletions
@@ -0,0 +1,302 @@
+# SmartCar 决策总架构：从视频帧到执行器
+
+## 坐标系统一览
+
+```
+               Camera 输出 640×480 MJPEG
+                      │
+          CONVERT_RGB=0 + IMREAD_REDUCED_COLOR_4
+          → 1/4 MJPEG 解码 → raw_frame = 160×120
+                      │
+          ┌───────────┤
+          ▼           ▼        (raw_frame.cols/rows 决定分母)
+   巡线空间            显示空间                模型空间
+   (line_tracking    (newWidth × newHeight)  160×120  (正常)
+   _width ×          calc_scale = 2          640×480  (回退模式)
+    line_tracking    newWidth  = lt_w × 2
+    _height)         newHeight = lt_h × 2
+   典型值 80×60
+```
+
+**关键换算关系（正常模式 raw_frame = 160×120）：**
+- 模型空间 → 巡线空间：`col_lt = cx * line_tracking_width / 160`，`row_lt = cy * line_tracking_height / 120`
+- 巡线空间 → 显示空间：`pixel = value * calc_scale`（calc_scale = 2）
+- newWidth / newHeight 取决于屏幕物理分辨率，由 `CameraInit` 动态计算
+- line_tracking_width / height = newWidth/calc_scale, newHeight/calc_scale
+
+**⚠ 回退模式：** 当 CONVERT_RGB=0 未生效时，raw_frame = 640×480，模型框坐标也在 640×480 空间。
+此时 LCD 渲染仍硬编码除以 160/120（会出错），但正常运行时不会进入此模式。
+
+---
+
+## 主循环 (main.cpp → CameraHandler)
+
+```
+main()
+ ├── cfg_load_all()                   # 从工作目录文件读取全部配置
+ ├── CameraInit(0, dest_fps, 320, 240) # 打开摄像头, LCD, 模型, I2C
+ ├── ControlInit()                    # 初始化双电机 GPIO + PWM
+ │
+ └── while(running):
+      └── CameraHandler()             # ★ 以下逐帧执行
+```
+
+---
+
+## CameraHandler 9 步流水线
+
+```
+                     ┌─────────────────────┐
+Step 1               │ cap.read(raw_mat)    │  640×480 MJPEG 原始字节流
+  取帧+解码          │ raw_mat 单通道(字节) │  → IMREAD_REDUCED_COLOR_4
+                     │ → cv::imdecode      │  → raw_frame = 160×120 BGR
+                     │ raw_frame = decoded  │  (回退: raw_mat 三通道→直用 640×480)
+                     └────────┬────────────┘
+                              │
+Step 2               ┌────────▼────────────┐
+  保存图像 (条件)     │ g_cfg.debug==1      │  写入 ./image/image_XXXXX.jpg
+                     │ && saveImg==1 →     │  (双重条件，缺一不可)
+                     │ saveCameraImage()   │
+                     └────────┬────────────┘
+                              │
+Step 3               ┌────────▼────────────┐
+  视觉巡线            │ image_main()        │  raw_frame → resize(lt_w×lt_h)
+  (每帧都跑)           │                    │  → HSV双通道Otsu → floodFill
+                     │ 输出: left_line[]   │  → 逐行最长连续段搜索
+                     │       right_line[]  │  → 丢线补全(仅 row 10~59)
+                     │       mid_line[]    │  → track (赛道蒙版 128/0)
+                     └────────┬────────────┘
+                              │
+Step 4               ┌────────▼────────────┐
+  模型推理            │ model_v10_detect()  │  每 2 帧推理一次
+  (每 2 帧)            │ raw_frame (160×120) │  4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线
+                     │ → g_boxes[16]       │  g_thresh = [0.80,0.80,0.80,0.75]
+                     │ → g_box_count       │  框坐标 ∈ raw_frame 空间
+                     └────────┬────────────┘
+                              │
+Step 5               ┌────────▼────────────┐
+  斑马线状态机         │ 遍历 g_boxes        │  仅处理 cls=3，取第一个斑马线框
+                     │ 去抖计数+远近判断    │  NORMAL→STOP(4s)→COOLDOWN(5s)
+                     │                    │  I2C 语音播报 @ 0x34
+                     └────────┬────────────┘
+                              │
+Step 6               ┌────────▼────────────┐
+  舵机控制            │ if g_cfg.start:     │
+                     │   foresee→check_row │  偏差 = mid_line[row]×2 - newWidth/2
+                     │   mid_line[check_row]│  → g_steer_deviation ∈ [-1, 1]
+                     │   → deviation       │  → servo duty: 1500000 ± offset
+                     │   → deadband 过滤   │     clamp [1.2M, 1.8M] ns
+                     │   → servo.setDuty()│   mid_val==255 → 跳过(无效行)
+                     └────────┬────────────┘
+                              │
+Step 7               ┌────────▼────────────┐
+  电机控制            │ ControlUpdate(      │
+  (开环)              │   target_speed,     │  if zebra_STOP: duty=0 (刹停)+GPIO73=0
+                     │   g_zstate==Z_STOP) │  else: speed × curve
+                     │                    │    curve = 1 - |deviation|×0.4
+                     │                    │    curve ∈ [0.6, 1.0]
+                     │                    │  左右电机同速(无差速)
+                     └────────┬────────────┘
+                              │
+Step 8               ┌────────▼────────────┐
+  LCD 渲染            │ if g_lcd_on:        │  g_lcd_on = g_cfg.showImg 的缓存
+                     │   track→BGR→ROI    │  (每10帧刷新一次缓存)
+                     │   + 边界线(红)      │  检测框坐标 bx=newWidth/160 缩放
+                     │   + 中线(蓝)        │  状态指示 N(绿)/S(红)/C(黄)
+                     │   + 检测框标注      │  → RGB565 → /dev/fb0 mmap
+                     │   (L1条状:跳过红灯) │
+                     └────────┬────────────┘
+                              │
+Step 9               ┌────────▼────────────┐
+  FPS 统计            │ 每 15 帧输出        │  stdout: "fps=XX.X|rd=X vi=X md=X ct=X ms"
+                     │ 分步计时+总帧率      │  15帧平均
+                     └─────────────────────┘
+```
+
+---
+
+## 视觉巡线深度展开 (image_main)
+
+```
+raw_frame (160×120 或 640×480 BGR)
+  │
+  ├── cv::resize → (line_tracking_w × line_tracking_h)  典型 80×60
+  │
+  ├── image_binerize():
+  │     BGR→HSV → H通道Otsu(THRESH_BINARY_INV) → S通道Otsu → bitwise_or
+  │     赛道=255(白), 背景=0(黑)
+  │     原理：蓝底赛道H偏聚集(100~130)、S高；灰路面S低
+  │     bitwise_or 取并集，H或S任一判为赛道即保留
+  │
+  ├── find_road():
+  │     MORPH_OPEN(2×2 CROSS) → 种子点(lt_w/2, lt_h-10)
+  │     → 种子点处画实心圆(半径10, 255) 防种子落在黑色区域
+  │     → floodFill(loDiff=20, upDiff=20, 8邻域, newVal=128)
+  │     → mask 提取 ROI → track (赛道内部=128, 外部=0)
+  │
+  ├── 逐行最长连续段搜索:
+  │     uchar(*IMG)[lt_w] = track.data
+  │     每行扫描，找最长连续非零段
+  │     有多个白段时取最后一个(≥取等，偏右)
+  │     → left_line[row], right_line[row]
+  │     → 全零行: left=right=-1
+  │
+  └── 中线 + 丢线补全 (row = lt_h-1 → 10):
+        有边界: mid = (left+right)/2, int 整除
+        丢线:   mid[row] = mid[row+1] (继承下行)
+                若 mid > lt_w/2 → left=mid, right=lt_w-1 (偏右)
+                若 mid ≤ lt_w/2 → left=0, right=mid (偏左)
+```
+
+**边界线区间有效性：** row 10~59 有可靠数据；row 0~9 的 mid_line 保持初始值 -1（不可靠，不要写入 box 过滤逻辑）。
+
+---
+
+## 舵机控制数学
+
+```
+foresee = g_cfg.foresee         ← 前瞻行索引（像素，显示空间），默认 40
+check_row = foresee / calc_scale ← 转为巡线空间行号（calc_scale=2, int 整除）
+mid_val = mid_line[check_row]   ← 该行中线列坐标 (0~79)
+
+如果 mid_val == 255 → 跳过（丢线补全也写不到 255，仅作防御）
+否则：
+  deviation = mid_val × 2 - newWidth / 2       ← 转为显示空间偏差(px)
+  deviation -= g_cfg.center_bias               ← 中心偏置修正
+  g_steer_deviation = deviation / (newWidth/2)  ← 归一化到 [-1, 1]
+
+  if |deviation| < deadband:
+      servo = 1500000 ns (直行)
+  else:
+      norm = deviation / (newWidth/2)
+      offset = norm × steer_gain × 300000
+      duty = clamp(1500000 + offset, 1200000, 1800000)
+      servo.setDutyCycle(duty)
+```
+
+**舵机量程：** 1,200,000 ~ 1,800,000 ns，中位 1,500,000 ns，±300,000 ns 对应满偏。
+**deadband 单位：** 显示空间像素（deviation 的单位）。
+
+---
+
+## 电机控制数学
+
+```
+ControlUpdate(speed, zebra_block):
+
+  if zebra_block || !g_cfg.start:
+      motor[0].updateduty(0)   → 两电机停转
+      motor[1].updateduty(0)
+      if !g_cfg.start: mortorEN.setValue(0)  → GPIO73 拉低
+      return
+
+  curve = 1.0 - |g_steer_deviation| × 0.4
+  curve = max(curve, 0.6)        ← 最低不低于 60% 速度
+  spd = speed × curve
+
+  motor[0].updateduty(spd)      → 左电机 (pwmchip8/pwm2, gpio12 方向)
+  motor[1].updateduty(spd)      → 右电机 (pwmchip8/pwm1, gpio13 方向)
+  mortorEN.setValue(1)           → GPIO73 拉高使能
+```
+
+**motor[0] vs motor[1]：** 左/右区别仅在于 gpioNum（12 vs 13）和 pwm通道（2 vs 1），函数调用完全相同。
+
+**updateduty(duty) 内部：**
+- `pwm_period * |duty| / 100` → 转占空比 ns 写入 sysfs
+- duty > 0 → GPIO 方向 = 1（正转），duty ≤ 0 → GPIO 方向 = 0（反转）
+
+**速度控制是开环的：** `MotorController` 仅包含 `updateduty()` 方法，无编码器反馈。
+`PIDController` 类存在但从未被实例化或调用。
+MotorController.h 中也无 `updateSpeed()` 方法（之前的文档中误记了此函数）。
+
+---
+
+## 斑马线状态机
+
+```
+检测到 cls=3 → 取第一个斑马线框 → 去抖计数+追踪最远cy
+
+ZNORMAL 期间:
+  zebra_seen: g_zc_frames++, g_zc_min_cy = min(g_zc_min_cy, zebra_cy)
+  未 seen:  g_zc_frames = max(0, g_zc_frames - 2)
+             g_zc_frames==0 → g_zc_min_cy=120  (重置)
+
+触发条件 (仅 ZNORMAL):
+  zebra_near (cy > g_cfg.zebrasee)
+  && g_zc_frames >= ZEBRA_MIN_FRAMES (5)
+  && g_zc_min_cy <= ZEBRA_FAR_CY (50)
+  → play_zebra_audio() → ZSTOP
+
+         ┌──────┐  条件: 连续5帧 + 起源于远(cy_min≤50) + 当前近(cy>zebrasee)
+         │NORMAL│ ─────────────────────────────────────► ┌──────┐
+         │      │ ◄───────────────────────────────────── │ STOP  │
+         └──────┘   冷却5秒结束                           │      │
+              ▲                                           └──┬───┘
+              │                    4秒后                      │
+              │           ┌──────────┐ ◄─────────────────────┘
+              └───────────│ COOLDOWN │
+                          └──────────┘
+
+NORMAL:   允许通行，检测斑马线
+STOP:     刹停 4 秒，g_zstate==Z_STOP 传给 ControlUpdate 第二个参数
+COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触发）
+          仅跳过检测，不影响其他功能（舵机/巡线正常）
+```
+
+**关键变量：**
+- `g_zc_min_cy`：NORMAL 期间追踪斑马线出现时的最小 cy（越远值越小）。未检测到斑马线时衰减减2/帧，归零后重置为 120。
+- `ZEBRA_FAR_CY = 50`：斑马线的 cy 必须曾在 ≤50 处出现过（即"从远处来"）
+- `zebrasee`（默认 60）：当前斑马线 cy > 此值视为"足够近"，触发停车
+- 去抖衰减速度：未检测到时每次 `-2`（比锥桶设计中的 `-1` 更快下降）
+- Box 遍历在找到第一个 cls=3 后 `break`，忽略同帧其他斑马线框
+
+---
+
+## 配置系统
+
+所有配置通过工作目录下的纯文本文件读写。文件不存在时 readDoubleFromFile 返回 0：
+
+| 文件 | 类型 | 代码默认 | ctl.sh 写入 | 含义 |
+|---|---|---|---|---|
+| `./speed` | double | 60 | 11 | 目标速度 (% 占空比) |
+| `./start` | int(0/1) | 0 | 0 | 使能开关，1=电机运行 |
+| `./debug` | int(0/1) | 0 | (不写入) | 每7帧重载配置+允许存图 |
+| `./showImg` | int(0/1) | 0 | (不写入) | LCD 显示（每10帧轮询→g_lcd_on） |
+| `./foresee` | double | 80 | 40 | 舵机前瞻行 (显示空间像素) |
+| `./deadband` | double | 5 | 8 | 舵机死区 (显示空间像素) |
+| `./steer_gain` | double | 1.0 | 1.5 | 舵机增益 |
+| `./center_bias` | double | 0 | (不写入) | 中线偏置修正 |
+| `./kp`,`./ki`,`./kd` | double | - | 3.5/0.3/2.0 | PID 参数 (当前未使用) |
+| `./mortor_kp/ki/kd` | double | - | 0.6/0.2/0 | 电机编码器 PID (当前未使用) |
+| `./zebrasee` | double | 60 | (不写入) | 斑马线近界阈值 (模型空间cy) |
+| `./destfps` | double | 30* | (不写入) | 目标帧率 (*代码兜底值) |
+| `./saveImg` | int(0/1) | - | (不写入) | 保存帧图像 (需 debug=1) |
+
+**debug 模式：** `./debug` = 1 时，主循环每7帧调用一次 `cfg_load_all()` 重读全部配置，支持热更新调参。同时允许 `saveImg` 保存图像。
+
+**注意**：`global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。
+
+---
+
+## 代码中未使用的模块
+
+以下 `.cpp` 文件已编译进 `common_lib` 但主循环中从未调用：
+
+| 文件 | 功能 | 状态 |
+|---|---|---|
+| `PIDController.cpp` | 位置式/增量式 PID | 类存在但无实例化，无调用 |
+| `serial.cpp` | VOFA 串口可视化 (vofa_justfloat/vofa_image) | 已实现但无调用 |
+| `Timer.cpp` | 定时器线程 | 已实现但无调用（Video 依赖它但 Video 也未用） |
+| `video.cpp` | 视频文件流读取 | 已实现但无调用 |
+
+---
+
+## 当前已知缺陷 / 未利用能力
+
+1. **cls=0 锥桶** — 模型已检测但被忽略（CONE_DESIGN.md 设计了方案）
+2. **cls=1 红灯 / cls=2 绿灯** — 检测框跳过不画（LCD 渲染中 `if g_boxes[i].cls == 1 || g_boxes[i].cls == 2: continue`），无决策
+3. **电机开环** — 无编码器反馈，PID 类已实现但无调用点，`MotorController` 无 `updateSpeed()` 方法
+4. **舵机死区** — 用 `abs(deviation) < deadband` 比较像素值，deadband 单位是显示空间像素
+5. **中线 row 范围** — 丢线补全仅覆盖 row 10~59，row 0~9 的 mid_line 保持 -1 不可靠
+6. **mid_val == 255 防御** — 代码中写死但 255 永远不会出现在 mid_line 中（当前实现下）
+7. **LCD 坐标缩放硬编码** — 检测框绘制用 `newWidth/160.0f`，假设 raw_frame 宽=160。回退模式下 raw_frame=640 时出错