Loongson_2k0300_SmartCar/docs/ARCHITECTURE.md

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）
• line_tracking_width / height = newWidth / calc_scale, newHeight / calc_scale
• newWidth / newHeight 由 CameraInit 读取 /dev/fb0 屏幕分辨率后动态计算

⚠ 回退模式： 当 CONVERT_RGB=0 未生效时，raw_frame = 640×480，模型框坐标也在 640×480 空间。 此时 LCD 渲染的硬编码缩放 newWidth/160.0f 会出错，但正常运行时不会进入此模式。

---

主循环 (main.cpp)

main()
 ├── cfg_load_all()                    # 从工作目录文件读取全部配置
 ├── 补读 cone_avoid_gain/range/hold_frames
 ├── CameraInit(0)                     # 打开摄像头, LCD, 模型, I2C, 计算巡线尺寸
 ├── ControlInit()                     # 初始化双电机 GPIO + PWM + 启动编码器线程
 │
 └── while(running):
      ├── CameraHandler()              # ★ 逐帧执行
      └── target_speed = g_cfg.speed   # (debug 模式下每 7 帧重载)

CameraInit 现在只接受 camera_id 一个参数（之前有 dest_fps, width, height 三个遗留参数已移除）。 巡线分辨率由屏幕自适应算法决定：line_tracking_w = newWidth/2, line_tracking_h = newHeight/2。

---

CameraHandler 11 步流水线

Step 1  capture_frame()          640×480 MJPEG → IMREAD_REDUCED_COLOR_4
  取帧+解码                       → raw_frame = 160×120 BGR
                                 (回退: raw_mat 三通道 → 直用 640×480)

Step 2  save_image_if_requested()  debug=1 && saveImg 文件=1 → 存 ./image/XXXXX.jpg
  保存图像 (条件)

Step 3  image_main()              raw_frame → resize(lt_w×lt_h) → HSV 双通道Otsu
  视觉巡线                        → floodFill(种子 (lt_w/2, lt_h-10), 半径5)
                                  → 逐行最长连续段搜索 → 丢线补全(row 10~59)
                                  输出: left_line[], right_line[], mid_line[]

Step 4  run_model_inference()     每 2 帧推理一次
  模型推理                        raw_frame (160×120) → model_v10_detect()
                                  4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线
                                  g_thresh = {0.90, 0.75, 0.80, 0.90}
                                  → g_boxes[16], g_box_count

Step 5a zebra_process()           去抖计数 + ZNORMAL→ZSTOP(4s)→ZCOOLDOWN(5s)
  斑马线状态机                    触发: 连续5帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee)
                                  刹停时 I2C 语音播报

Step 5b traffic_light_process()   TLNORMAL→TLSTOP→TLWAIT_GREEN
  红绿灯状态机                    红灯≥3帧 → 停车; 红灯消失 → 等绿灯; 绿灯≥3帧 → 通行

Step 5c cone_detect_and_deform()  去抖确认 + 中线变形 + 消失后保持衰减
  锥桶检测 & 中线变形              cone_speed 减速 + cone_hold_frames 保持

Step 6  steering_update()         foresee/2 → check_row → mid_line[row]×2 - newWidth/2
  舵机控制                        deadband 过滤 → servo duty: 1500000 ± offset ns

Step 7  motor_update()            开环PWM + 编码器比例刹车 + 弯道减速 + 锥桶减速
  电机控制

Step 8  lcd_render()              track→BGR→ROI + 边界线(红/绿/蓝) + 检测框 + 状态指示
  LCD 渲染                        RGB565 → /dev/fb0 mmap

Step 9  fps_log()                 每 15 帧输出总帧率 + 分步耗时 ms
  FPS 统计

---

视觉巡线深度展开 (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)
  │     → 种子点处画实心圆(半径5, 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-2 → 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 (偏左)
        底行兜底: 丢线时用 lt_w/2

边界线区间有效性： row 1059 有数据；row 09 的 mid_line 保持初始值 -1（不可靠）。

---

舵机控制数学

foresee = g_cfg.foresee              ← 前瞻行索引（显示空间像素），默认 40
check_row = (int)foresee / calc_scale ← 转为巡线空间行号（calc_scale=2, int 整除）
mid_val = mid_line[check_row]        ← 该行中线列坐标 (0~79)

如果 mid_val == -1 → 跳过（丢线行无效）
否则：
  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)

ControlUpdate(speed, zebra_or_tl_block):

  if !g_cfg.start:
      motor[0].updateduty(0)   → 两电机停转
      motor[1].updateduty(0)
      mortorEN.setValue(0)      → GPIO73 拉低
      return

  if zebra_or_tl_block:                          ← 斑马线或红灯刹停
      读取 g_enc_speed (编码器线程, 脉冲/秒)
      读取 g_enc_dir (编码器方向)
      if cur_speed > 0.5:
          brake_ns = cur_speed × g_cfg.brake_scale   ← 比例刹车
          brake_ns = clamp(brake_ns, 0, g_cfg.brake_max)
          brake_pct = brake_ns / 500                 ← ns → % (period=50000ns)
          dir = cur_dir ? 1 : 0
          motor[i]->updateduty(dir ? -brake_pct : brake_pct)  ← 反转刹车
      else:
          motor[i]->updateduty(0)                     ← 已静止，不刹车
      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 拉高使能

编码器刹车（encoder brake）： 当斑马线或红灯触发停车时，根据实时编码器速度计算反向刹车占空比， 刹车力度与当前车速成正比（brake_scale），有上限（brake_max ns）。 速度低于 0.5 pps 时不再施加刹车。

motor_update 额外逻辑： motor_update() 在调用 ControlUpdate 之前，若锥桶已触发（cone_is_slow()）， 会将 speed 乘以 g_cfg.cone_speed（默认 0.5），实现锥桶路段减速。

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 类存在但从未被实例化或调用。

编码器线程 (ControlInit → encoder_thread)：独立线程高频轮询 GPIO67（LSB脉冲）+ GPIO72（方向）， 每 100ms 计算一次速度 → g_enc_speed（脉冲/秒），含 500µs sleep 防止 CPU 饿死。

---

斑马线状态机

检测到 cls=3 → 取第一个斑马线框（之后 break）→ 去抖计数+追踪最远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 传给 motor_update → 编码器刹车
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，忽略同帧其他斑马线框

---

红绿灯状态机

检测到 cls=1 (红灯) / cls=2 (绿灯) → 去抖计数 → 状态转移

          ┌──────┐  红灯≥3帧    ┌──────┐  红灯消失   ┌───────────┐
          │NORMAL│ ───────────► │ STOP │ ──────────► │WAIT_GREEN │
          │      │ ◄─────────── │      │             │           │
          └──────┘  绿灯≥3帧    └──────┘             └─────┬─────┘
               ▲                                           │
               └───────────────────────────────────────────┘

TL_NORMAL:      允许通行，检测红绿灯
TL_STOP:        红灯→停车（编码器刹车），等红灯消失
TL_WAIT_GREEN:  红灯已消失，等待绿灯出现（≥3帧）→ 恢复通行

返回值： traffic_light_process() 返回 true 当 g_tl_state != TL_NORMAL， 传递给 motor_update → ControlUpdate 触发编码器刹车。

---

锥桶检测 & 中线变形

cone_detect_and_deform():
  (仅在 Z_NORMAL && TL_NORMAL 时运行)

  ┌─ 寻找最近锥桶 ─┐
  │ 遍历 g_boxes, cls=0, conf >= cone_thresh
  │ 模型空间坐标 → 巡线空间坐标
  │ 过滤: rl < 10 或 无边界线的行 → 跳过
  │ cone_margin==0 → 中心点; ==1 → 框边缘 (左右均在边界线内)
  │ 取 cy 最大(最近)的锥桶
  │
  ├─ 去抖确认 ──────────────────────────────
  │ 位置容忍: row_tol = max(2, lt_h/12), col_tol = max(3, lt_w/8)
  │ 连续出现且位置接近 → g_cone_frames++
  │ 丢失 → g_cone_frames = max(0, g_cone_frames - 1)
  │ g_cone_confirmed = (g_cone_frames >= cone_min_frames)
  │
  ├─ 保持/衰减 ────────────────────────────
  │ 确认时: g_cone_hold_ctr=0, 记录锥桶位置
  │ 未确认但有历史: g_cone_hold_ctr++, 衰减推离量
  │ 超过 cone_hold_frames → 停止影响
  │
  └─ 中线变形 ─────────────────────────────
    dir = (锥桶偏左) ? +1.0 : -1.0     ← 向远离锥桶方向推
    decay = (确认) 1.0 : (1 - hold_ctr/hold_frames)
    for row = 10 → src_row:
        t = clamp((row-10) / cone_avoid_range, 0, 1)   ← 斜坡上升
        push = t × cone_avoid_gain × half_w × dir × decay
        mid_line[row] = clamp(mid_line[row] + push,
                               left_line[row]+2, right_line[row]-2)

速度联动： cone_is_slow() 在锥桶确认或保持期间返回 true， motor_update 将当前速度乘以 cone_speed（默认 0.5）。

---

LCD 渲染

lcd_render():
  g_lcd_on 每 10 帧从 g_cfg.showImg 刷新

  track (128/0 灰度) → resize(newWidth×newHeight) → GRAY→BGR
  → 居中拷贝到 lcd_fbImage (screenHeight × screenWidth) 的 ROI 区域

  边界线绘制: 红=左边界, 绿=右边界, 蓝=中线
  (跳过 mid_line[row]==-1 的行)

  检测框绘制: 遍历 g_boxes, bx=newWidth/raw_frame.cols
  跳过 cls=1(红灯) 和 cls=2(绿灯) 的框
  cls=0 锥桶 → 橙色框, cls=3 斑马线 → 紫色框
  标注框类别+置信度

  状态指示 (左下角): N=正常, S=斑马线停车, C=斑马线冷却,
                     R=红灯停车, G=等绿灯
  → convertMatToRGB565 → /dev/fb0 mmap

---

配置系统

所有配置通过工作目录下的纯文本文件读写。文件不存在时 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)
./cone_avoid_gain	double	0.3	0.3	锥桶中线变形推离量 (归一化)
./cone_avoid_range	int	30	30	变形斜坡陡峭度
./cone_speed	double	0.5	0.5	锥桶触发时速度倍率
./cone_min_frames	int	3	3	锥桶连续确认帧数
./cone_margin	int	0	0	0=中心点, 1=框边缘检查
./cone_thresh	double	0.80	0.80	锥桶置信度阈值
./cone_hold_frames	int	30	30	锥桶消失后保持变形帧数
./brake_scale	double	10	10	刹车速度→占空比系数
./brake_max	int	10000	10000	最大刹车占空比 (ns)

debug 模式： ./debug = 1 时，主循环每7帧调用一次 cfg_load_all() 重读全部配置，支持热更新调参。同时允许 saveImg 保存图像。

注意： global.h 中的 CfgCache 默认值和 ctl.sh 写入的值不一致（如 speed: 60 vs 11）。ctl.sh 写入的值覆盖代码默认值，是实际运行参数。

---

代码中未使用的模块

以下 .cpp 文件存在于 src/ 中但已被 CMakeLists.txt 的 list(FILTER ... EXCLUDE) 排除编译：

文件	功能	排除原因
vl53l0x.cpp	激光测距模块	硬件未连接
zebra_detect.cpp	经典斑马线检测	已被 Mild 模型替代
PIDController.cpp	位置式/增量式 PID	电机开环直驱，无调用点
serial.cpp	VOFA 串口可视化	未使用

注意： PIDController.h 仍在 lib/ 中，MotorController 仅提供 updateduty()（开环占空比）， 不提供 updateSpeed() 方法。

---

模型：Mild v12

• 推理引擎：src/model_v10.{hpp,cpp}（名称为兼容保留，内部为 Mild v3）
• 输入：160×120 BGR
• 输出：4 类 + 背景 → 9 通道：0=锥桶, 1=红灯, 2=绿灯, 3=斑马线
• 架构：3→9→9→13→13→19→19→19→44→44→64→64 → head(64→64,dw) → out(64→9)
• 阈值：{0.90, 0.75, 0.80, 0.90}（锥桶/红灯/绿灯/斑马线）
• 权重文件：mild_v12.bin（105KB 自定义二进制格式）
• 推理频率：每 2 帧一次（跳帧节省 CPU）

---

已知局限

1. 电机开环 — 正常行驶无编码器反馈，仅制动时使用编码器速度做比例刹车。PID 类已实现但无调用点。
2. 舵机死区 — 用 abs(deviation) < deadband 比较像素值，deadband 单位是显示空间像素。
3. 中线 row 范围 — 丢线补全仅覆盖 row 1059，row 09 的 mid_line 保持 -1 不可靠。
4. LCD 坐标缩放硬编码 — 检测框绘制用 newWidth/160.0f，假设 raw_frame 宽=160。回退模式（raw_frame=640）下会出错。
5. 锥桶变形可能跳过 row 0~9 — 变形循环从 row=10 开始，row 0~9 不会改变。