spdis/Loongson_2k0300_SmartCar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
spdis
2026-06-13 13:05:18 +08:00
parent 457d608621
commit b9e32c5a40
66 changed files with 1698 additions and 5625 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/ARCHITECTURE.md
+302
View File
@@ -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 时出错
docs/CONE_DESIGN.md
+283
View File
@@ -0,0 +1,283 @@
# 锥桶避障功能 — 中线变形方案
## 1. 核心思路
**不改舵机代码，直接修改 `mid_line[]` 数组。** 在锥桶附近把中线"推开"一个凸起，后续巡线逻辑（deviation → deadband → servo）原样工作，车自然跟着变形后的中线绕开锥桶。
不使用任何推入计时器、归还逻辑、EMA 平滑——锥桶消失后，下一帧 `image_main()` 重新计算 `mid_line`，自动恢复。
---
## 2. 坐标与边界过滤
```
模型空间 (raw_frame):
正常模式: 160×120 (MJPEG 1/4解码)
回退模式: 640×480
巡线空间映射:
cone_col = box.cx * line_tracking_width / raw_frame.cols (典型 0~79)
cone_row = box.cy * line_tracking_height / raw_frame.rows (典型 0~59)
换算速查 (正常模式 160×120 → 典型 80×60):
cone_col = cx / 2
cone_row = cy / 2
```
边界过滤 (cls=0, conf ≥ cone_thresh):
```
row_lt < 10 → 丢弃 (row 0~9 无可靠巡线数据)
row_lt >= line_tracking_height → 丢弃
left_line[row_lt] == -1 → 丢弃 (该行丢线)
right_line[row_lt] == -1 → 丢弃
col_lt < left_line[row_lt] → 丢弃 (赛道外)
col_lt > right_line[row_lt] → 丢弃 (赛道外)
```
---
## 3. 中线变形
### 3.1 方向
```
mid_at_cone = mid_line[cone_row]
cone_col < mid_at_cone → 锥在左 → push_mid 向右 (+)
cone_col > mid_at_cone → 锥在右 → push_mid 向左 (-)
direction = (cone_col < mid_at_cone) ? +1 : -1
```
### 3.2 变形区域
从 `start_row = max(10, cone_row - range)` 到 `cone_row`，共 (cone_row - start_row) 行：
```
row 0 ───────────────────────────────── 远
│ (不变形)
row 10 ─────────────────────────────────
│ start_row ────────────── t=0
│ 线性斜坡
│ foresee 行在这里看到部分偏移
│
│ cone_row ────────────── t=1
row 59 ───────────────────────────────── 近 (车前方)
```
### 3.3 数学
```
for row in [start_row, cone_row]:
t = (row - start_row) / (cone_row - start_row) // [0, 1]
push = t * cone_avoid_gain * (line_tracking_width / 2) * direction
mid_line[row] = original_mid_line[row] + push
mid_line[row] = clamp(mid_line[row],
left_line[row] + 2,
right_line[row] - 2)
```
**参数：**
- `cone_avoid_gain` (double, 0.3)：归一化推离量。1.0 = 推到半幅赛道宽。默认 0.3 表示推到赛道 30% 宽度处。
- `cone_avoid_range` (int, 30)：斜坡覆盖行数。必须足够大使 foresee 行感受到偏移。
---
## 4. 数字算例
**场景：** 直道，锥桶在赛道左侧。`lt_w=80, lt_h=60, cone_avoid_gain=0.3, cone_avoid_range=30`
```
原始数据:
cone_col=25, cone_row=50, mid_at_cone=40 (锥在左侧15列)
direction = +1 (推中线向右)
start_row = max(10, 50-30) = 20
half_w = 40
变形计算:
push_max = 0.3 * 40 * 1 = 12 (行50处的最大位移, 巡线空间列)
row=20 (start, t=0): mid_line = 40 + 0 = 40 → deviation = 40*2-80 = 0 → 舵机中位
row=25 (t=5/30=0.17): mid_line = 40 + 2.04 = 42 → deviation = 42*2-80 = +4 → 略右
row=35 (t=15/30=0.5): mid_line = 40 + 6.0 = 46 → deviation = 46*2-80 = +12 → 明显右转
row=50 (t=1.0): mid_line = 40 + 12 = 52 → deviation = 52*2-80 = +24 → 大幅右转
foresee=40 (显示空间) → check_row=40/2=20 (巡线空间):
mid_line[20] 未被变形 (start_row=20, t=0)
→ 舵机看不到! 需要减小 range 或增大 foresee
```
**问题暴露：** `foresee=40` → `check_row=20`，但 `start_row=20` 时 t=0 无偏移。
**修正：** 确保变形起点在 `check_row` 以上。两种方式：
1. 增大 `cone_avoid_range`（如 40），让 start_row 更低
2. 让斜坡向上延伸超过 start_row（即从 row=10 就开始）
**采用方案 2：** 变形始终从 `row=10` 开始，`range` 参数控制斜坡的"陡峭度"而非范围。
---
## 5. 修正后的变形公式
```
range = cone_avoid_range // 陡峭度参数 (默认 30)
total_rows = cone_row - 10 // 实际跨度
for row in [10, cone_row]:
t_raw = (row - 10.0) / total_rows // 线性 [0, 1]
t = clamp(t_raw * (total_rows / range), // 用 range 缩放斜率
0.0, 1.0)
push = t * cone_avoid_gain * (line_tracking_width / 2) * direction
mid_line[row] = clamp(original_mid + push,
left_line[row] + 2,
right_line[row] - 2)
```
**效果（cone_avoid_range=30, cone_row=50, total_rows=40）：**
```
total_rows/range = 40/30 = 1.33 ← 斜率因子 >1, 斜坡更陡
row=10: t_raw=0, t=0*1.33=0 → push=0
row=20: t_raw=0.25, t=0.25*1.33=0.33 → push=0.33*12=4.0 → deviation=+8 (刚好过死区)
row=35: t_raw=0.625,t=0.83 → push=10 → deviation=+20
row=50: t_raw=1.0, t=1.0 → push=12 → deviation=+24
```
foresee 行 (row=20) 收到 33% 的偏移，产生 +8px 偏差 → 刚好越过 deadband(8px) → 舵机开始右转。
**调参规则：**
- 增大 `cone_avoid_range` → 斜坡更缓 → foresee 行偏移更小 → 转向更柔和
- 减小 `cone_avoid_range` → 斜坡更陡 → foresee 行偏移更大 → 转向更猛
- 增大 `cone_avoid_gain` → 整体偏移量增大 → 转向更猛
---
## 6. 去抖机制
与之前一致，不改变：
```
#define CONE_MIN_FRAMES 3
#define CONE_COUNT_DECAY 1
g_cone_frames: 连续检测计数
g_cone_last_row / g_cone_last_col: 位置跟踪 (容差 ±5行/±10列)
g_cone_confirmed = (g_cone_frames >= CONE_MIN_FRAMES)
```
**关键差异：** 去抖只决定"是否触发变形"，变形本身没有持续时间概念。
- 锥桶在 → 变形在（每帧重新计算 mid_line 变形）
- 锥桶消失 → 下帧 image_main() 重新计算 mid_line → 变形自动消失
- 不去抖时的抖动：锥桶在边界附近闪烁会使 mid_line 来回跳，用去抖平滑
---
## 7. 舵机与电机
**舵机：** 零改动。Step 6 读取 `mid_line[foresee/2]`，看到变形后的中线值，自然输出偏转方向。
**电机：** 推入期间减速：
```
if (g_cone_confirmed && g_zstate != Z_STOP) {
effective_speed = target_speed * cone_speed_factor;
} else {
effective_speed = target_speed;
}
ControlUpdate(effective_speed, g_zstate == Z_STOP);
```
---
## 8. 参数配置
### 8.1 启动时一次性读取（运行中不重新读文件）
| 文件 | 类型 | 默认 | 含义 |
|---|---|---|---|
| `./cone_avoid_gain` | double | 0.3 | 归一化推离量 (0=不推, 1=推到赛道边缘) |
| `./cone_avoid_range` | int | 30 | 斜坡陡峭度 (越小越陡，foresee 行感受越强) |
### 8.2 热更新参数
| 文件 | 类型 | 默认 | 含义 |
|---|---|---|---|
| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 |
| `./cone_min_frames` | int | 3 | 连续确认帧数 |
| `./cone_margin` | int | 0 | 0=中心点过滤, 1=框边缘过滤 |
| `./cone_thresh` | double | 0.80 | 锥桶置信度阈值 (业务层, 不改 NMS) |
### 8.3 读取实现
**main.cpp 启动时（一次性）：**
```cpp
double avoid_gain_val = readDoubleFromFile("./cone_avoid_gain");
int avoid_range_val = (int)readDoubleFromFile("./cone_avoid_range");
if (avoid_gain_val <= 0) avoid_gain_val = 0.3;
if (avoid_range_val <= 0) avoid_range_val = 30;
g_cfg.cone_avoid_gain = avoid_gain_val;
g_cfg.cone_avoid_range = avoid_range_val;
```
**global.cpp (cfg_load_all，热更新)：**
```cpp
g_cfg.cone_speed = readDoubleFromFile(cone_speed_file);
g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file);
g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file);
g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_file);
```
**ctl.sh：**
```sh
echo 0.3 > "$DIR/cone_avoid_gain" 2>/dev/null
echo 30 > "$DIR/cone_avoid_range" 2>/dev/null
echo 0.5 > "$DIR/cone_speed" 2>/dev/null
echo 3 > "$DIR/cone_min_frames" 2>/dev/null
echo 0 > "$DIR/cone_margin" 2>/dev/null
echo 0.80 > "$DIR/cone_thresh" 2>/dev/null
```
---
## 9. 代码修改清单
```
lib/global.h:
├── CfgCache 新增: cone_avoid_gain, cone_avoid_range,
│ cone_speed, cone_min_frames, cone_margin, cone_thresh
└── 新增文件名常量
src/global.cpp:
└── cfg_load_all() 追加热更新参数 (不含 avoid_gain/avoid_range)
main/main.cpp:
└── CameraInit 之前: 读取 cone_avoid_gain, cone_avoid_range 写入 g_cfg
src/camera.cpp:
├── #define CONE_CLASS 0
├── 新增静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed
├── Step 5 之后: 插入锥桶检测+边界过滤+去抖
├── Step 5.5: 中线变形 (修改 mid_line[row] for row in [10, cone_row])
├── Step 7 电机: g_cone_confirmed → effective_speed *= cone_speed_factor
└── Step 8 LCD: 锥桶框改橙色
ctl.sh:
└── init_pins() 追加 6 个新文件默认值
```
---
## 10. 方案对比
| | 旧方案（舵机推入） | 新方案（中线变形） |
|---|---|---|
| 舵机代码 | 需改动 | 零改动 |
| deadband | 需绕过 | 自 然生效 |
| steer_gain | 需单独 cone_gain | 共用 |
| 归还机制 | 需要计时器+衰减 | 自动 (image_main 重算) |
| 平滑性 | 需 EMA | 斜坡自带平滑 |
| 参数数量 | 7 | 6 |
| 锥桶消失恢复 | 需衰减逻辑 | 下一帧自动 |
docs/ENCODER_BRAKE.md
+95
View File
@@ -0,0 +1,95 @@
# 编码器刹车 — 设计与实现
## 1. 硬件
编码器2，引脚：
| 信号 | 引脚 | 模式 |
|---|---|---|
| LSB 脉冲 | GPIO 67 | 输入 |
| DIR 方向 | GPIO 72 | 输入 |
两引脚均属 gpiochip64（GPA64，base=64，16 pins）。
## 2. 触发条件
`ControlUpdate(speed, zebra_block)` 中 `zebra_block == true` 时进入刹车流程。`zebra_block` 由 `CameraHandler` Step 5 斑马线状态机传入——当 `g_zstate == Z_STOP` 时为 true。
## 3. 刹车状态机
```
┌──────────┐
正常行驶 ───→ │ IDLE │
└────┬─────┘
│ zebra_block==true
▼
┌──────────┐
│ BRAKING │ ← 施加 -30% 占空比
│ │ 每帧读 GPIO 67
└────┬─────┘
┌───────────┴───────────┐
│ 脉冲变化 │ 脉冲不变
▼ ▼
轮子在转 → 继续刹车 g_brake_still++
重置计数器 │
连续 5 帧无变化？
或超时 60 帧？
│
▼
┌──────────┐
│ HOLDING │ ← duty=0，保持静止
└──────────┘
│
zebra_block==false
│
▼
┌──────────┐
│ IDLE │
└──────────┘
```
## 4. 参数
定义在 `src/control.cpp`：
| 常量 | 值 | 含义 |
|---|---|---|
| `BRAKE_STILL_THRESH` | 5 | 连续无脉冲帧数阈值，超过即判停 |
| `BRAKE_TIMEOUT` | 60 | 最长刹车帧数（~2s @30fps），强制释放 |
| `BRAKE_DUTY` | -30 | 刹车占空比（-100~100，负值=反转） |
## 5. 读取方式
GPIO 67 通过 sysfs 文件描述符轮询（`GPIO::readValue()`），每帧 `lseek` + `read` 一次。不依赖中断。
**为什么轮询够用：** 30fps 帧率 = 33ms 间隔。编码器假设 100+ 脉冲/转，即使 1 转/秒的极慢速度也有 3+ 脉冲/帧间隔，不会漏判。
## 6. 刹车强度
```
motor[i]->updateduty(-30)
```
`MotorController::updateduty()` 内：
- 占空比：`period * |duty| / 100 = 50000 * 30 / 100 = 15000 ns`
- 方向：`duty < 0` → `directionGPIO.setValue(false)` → 反转
即 30% 占空比的反向驱动，足够刹停但不会让车向后猛冲。
## 7. 编码器生效范围
- **仅在斑马线 STOP 时**使用编码器
- 正常行驶时编码器处于"打开但未使用"状态（fd 保持 open，但不 read）
- 锥桶避障不使用编码器
- `g_cfg.start == 0` 时刹车状态机重置
## 8. 关键代码路径
```
main.cpp
└── while(running)
└── CameraHandler()
└── ControlUpdate(target_speed, g_zstate == Z_STOP)
└── if (zebra_block) → 刹车状态机
└── encoderLSB.readValue() ← GPIO 67
```
docs/TRAFFIC_LIGHT.md
+103
View File
@@ -0,0 +1,103 @@
# 红绿灯识别 — 设计与实现
## 1. 模型检测
NanoDet V10 模型 4 类输出中：
- cls=1: 红灯
- cls=2: 绿灯
模型置信度阈值由 `g_thresh[1]` 和 `g_thresh[2]` 控制（`src/camera.cpp:25`），当前分别为 0.80。
## 2. 状态机
```
┌──────────────────────────────────────────┐
│ │
▼ │
┌──────────┐ 红灯连续3帧 ┌──────────┐ │
正常行驶 │ TL_NORMAL │ ─────────────→ │ TL_STOP │ │
│ │ │ 刹车ON │ │
└──────────┘ └────┬─────┘ │
▲ │ 红灯消失 │
│ ▼ │
│ ┌──────────────┐ │
│ 绿灯连续3帧 │ TL_WAIT_GREEN │ │
└──────────────────│ 刹车ON │ │
└──────────────┘ │
```
| 状态 | 刹车 | 锥桶 | 触发条件 |
|---|---|---|---|
| TL_NORMAL | 无 | 正常 | — |
| TL_STOP | ON | 抑制 | 红灯连续 3 帧确认 |
| TL_WAIT_GREEN | ON | 抑制 | 红灯从视野消失 |
**去抖：** 红灯 3 帧确认触发，衰减 `-1`/帧；绿灯 3 帧确认触发，衰减 `-1`/帧。
**为什么红灯消失后还要等绿灯：** 防止红灯误检消失后误触发通行。确保绿灯确实出现在视野中才放行。
## 3. 刹车机制
和斑马线完全一样，复用编码器比例刹车：
```
ControlUpdate(spd, g_zstate == Z_STOP || tl_block)
刹车信号 = 斑马线STOP || 红灯STOP || 等待绿灯
```
刹车参数（热更新）：
| 文件 | 默认 | 含义 |
|---|---|---|
| `./brake_scale` | 10 | 速度(pps) → 刹车占空比(ns) |
| `./brake_max` | 10000 | 最大刹车占空比(ns) |
## 4. 与其他模块的交互
```
决策优先级:
Z_STOP (斑马线) > TL_STOP/WAIT_GREEN (红绿灯) > cone (锥桶) > 正常巡线
锥桶抑制: g_zstate != Z_STOP && g_tl_state == TL_NORMAL 时才运行
电机刹车: zebra_STOP || tl_block 任一为 true 即刹车
LCD 状态: "R"=红灯停车 "G"=等绿灯, 拼接斑马线状态
```
## 5. 关键代码路径
```
CameraHandler (src/camera.cpp)
Step 4: 模型推理 → g_boxes[], g_box_count
Step 5: 斑马线状态机
Step 5.5: 红绿灯状态机
├─ 遍历 g_boxes, 统计 red_seen/green_seen
├─ 去抖计数 (3帧确认, 衰减-1/帧)
└─ 状态跃迁
Step 5.6: 锥桶检测 (g_zstate!=Z_STOP && g_tl_state==TL_NORMAL 时才运行)
Step 7: 电机控制
ControlUpdate(spd, g_zstate==Z_STOP || tl_block)
control.cpp:
ControlUpdate(speed, zebra_block)
└─ zebra_block → 编码器线程读速度 → 比例刹车
```
## 6. 状态变量
定义在 `src/camera.cpp`：
```cpp
enum TLState { TL_NORMAL, TL_STOP, TL_WAIT_GREEN };
static TLState g_tl_state = TL_NORMAL;
static int g_tl_red_frames = 0; // 红灯连续计数
static int g_tl_green_frames = 0; // 绿灯连续计数
```
## 7. LCD 显示
状态字拼接：`[红绿灯][斑马线]`
- `RN` — 红灯停车 + 正常巡线
- `GN` — 等绿灯 + 正常巡线
- `RS` — 红灯 + 斑马线同时 (极少见)
- `NN` — 全部正常