# 激光雷达挡板避障设计

## 概述

VL53L0X 单点 ToF 激光测距传感器，返回沿光束方向的距离值（mm）。
当距离低于阈值时触发绕行：中线变形推向右侧，消失后保持衰减。

**当前实现：** 纯距离触发 + 预触发去抖，不检查视觉边界线。绕行方向固定朝右。

---

## VL53L0X 驱动集成

驱动已编译（`src/vl53l0x.cpp` + `lib/vl53l0x/` C API），在 `CameraInit()` 中初始化：

```
CameraInit():
  if g_cfg.lidar_enable:
      g_lidar_ok = g_lidar_sensor.init()      // open /dev/stmvl53l0x_ranging
      g_lidar_sensor.startMeasure()            // 启动首次测量
  else:
      g_lidar_ok = false                       // 禁用

cameraDeInit():
  if g_lidar_ok → g_lidar_sensor.stop()
```

硬件未连接时 `init()` 失败 → `g_lidar_ok = false` → `lidar_avoid_process()` 直接跳过，不影响正常行驶。

---

## 流水线位置

`lidar_avoid_process()` 位于 CameraHandler Step 4，在视觉巡线之后、模型推理之前：

```
Step 3  image_main()           → left_line[], right_line[], mid_line[]
Step 4  lidar_avoid_process()  → 修改 mid_line[] (绕行变形)  ★
Step 5  run_model_inference()
Step 6c cone_detect_and_deform() → 也修改 mid_line[] (叠加在 lidar 之上)
```

lidar 先于 cone 修改 mid_line，优先级更高。cone 的 clamp 确保不超出边界。

---

## 核心算法

### 1. 读取激光距离（每 2 帧一次）

```
lidar_avoid_process():
  if !g_lidar_ok || !g_cfg.lidar_enable → return
  if ++lidar_skip < 2 → return          // 每 2 帧一读 (~15Hz)
  lidar_skip = 0

  data = g_lidar_sensor.readResult()     // 非阻塞读取上次测量结果
  d_mm = data.RangeMilliMeter
  g_lidar_sensor.startMeasure()          // 立即启动下次测量 (后台 ~20ms)
```

### 2. 预触发去抖

```
  if d_mm == 0:                          // 读失败, 跳过
      skip

  if d_mm >= g_cfg.lidar_pre:            // 距离超出预触发窗口 (默认 800mm)
      g_lidar_frames = max(0, frames-1)  // 衰减但不归零
      skip

  // d_mm < lidar_pre → 进入触发判定
```

`lidar_pre`（默认 800mm）是预触发窗口：距离进入此范围就开始攒帧，但要 < `lidar_thresh`（默认 300mm）才真正确认。

### 3. 距离 → 图像行映射

```
  row = lt_h - 1 - (d_mm - lidar_near) × (lt_h - 11) / (lidar_far - lidar_near)
  row = clamp(row, 10, lt_h - 1)
```

| 参数 | 默认 | 含义 |
|------|------|------|
| `lidar_near` | 50 mm | row = lt_h-1（最近行）对应的距离 |
| `lidar_far` | 1200 mm | row = 10（最远有效行）对应的距离 |

### 4. 触发判定

```
  g_lidar_frames++
  g_lidar_obstacle_row = row

  g_lidar_confirmed = (d_mm < lidar_thresh) && (g_lidar_frames >= lidar_min_frames)
```

**注意：** 不检查视觉边界线（near_valid / far_lost），纯距离触发。
配置中的 `lidar_near_start`、`lidar_near_end`、`lidar_far_span` 参数被加载但**未使用**（为设计预留）。

### 5. 确认 → 记录状态

```
  if g_lidar_confirmed:
      g_lidar_hold_ctr     = 0
      g_lidar_hold_src_row = g_lidar_obstacle_row
      g_lidar_hold_dir     = 1.0          // 固定朝右绕行
      g_lidar_frames       = 0            // 重置计数
```

**方向固定为右（+1.0）**，不根据赛道左右空间动态判断。

---

## 中线变形

当 `lidar_is_active()`（确认中或保持衰减中）时，对 row 10 ~ 障碍行施加变形：

```
  decay = (确认中) 1.0 : (1.0 - hold_ctr / hold_frames)   // 保持期线性衰减

  for row = 10  to  g_lidar_hold_src_row:
      t    = clamp((row - 10) / lidar_avoid_range, 0.0, 1.0)
      push = t × lidar_avoid_gain × half_w × dir × decay
      mid_line[row] = clamp(mid_line[row] + push,
                             left_line[row] + 2.0,
                             right_line[row] - 2.0)
```

---

## 状态机

```
                   ┌────────────────────────┐
                   │                        │
                   ▼                        │
 ┌──────┐  确认   ┌──────┐  消失   ┌──────────┐  保持结束
 │NORMAL│ ─────► │AVOID │ ─────► │HOLD_DECAY│ ────────► NORMAL
 │      │        │      │        │ (衰减)   │
 └──────┘        └──────┘        └──────────┘
    ▲                                │
    └────────────────────────────────┘ 测距恢复正常

NORMAL:      正常行驶，激光测距 > lidar_pre
AVOID:       挡板确认 → 中线变形绕行 (固定朝右)
HOLD_DECAY:  挡板消失 → 变形量线性衰减 (lidar_hold_frames 帧内归零)
             衰减期间若再次检测到挡板 → 立即切回 AVOID
```

---

## 速度联动

`motor_update()` 中包含挡板减速代码（`lidar_is_active()` → `speed × lidar_speed`），
但当前**已注释**。绕行仅靠中线变形，不减速。如需启用，取消注释即可。

---

## 配置参数

| 文件 | 类型 | 默认 | 含义 |
|------|------|------|------|
| `./lidar_enable` | int(0/1) | 1 | 激光避障总开关（0=禁用，init 也不调用） |
| `./lidar_thresh` | int | 300 | 障碍判定距离阈值 (mm)，低于此值触发确认 |
| `./lidar_pre` | int | 800 | 预触发窗口 (mm)，低于此值开始攒帧 |
| `./lidar_near` | int | 50 | row=lt_h-1 对应的物理距离 (mm) |
| `./lidar_far` | int | 1200 | row=10 对应的物理距离 (mm) |
| `./lidar_min_frames` | int | 3 | 连续确认帧数 |
| `./lidar_avoid_gain` | double | 0.4 | 绕行中线推离力度 (归一化) |
| `./lidar_avoid_range` | int | 30 | 绕行斜坡陡峭度 (行数) |
| `./lidar_speed` | double | 0.4 | 绕行时速度倍率 (当前未生效) |
| `./lidar_hold_frames` | int | 30 | 挡板消失后变形保持帧数 |
| `./lidar_near_start` | int | 1 | (已加载但未使用，预留) |
| `./lidar_near_end` | int | 4 | (已加载但未使用，预留) |
| `./lidar_far_span` | int | 6 | (已加载但未使用，预留) |

---

## 边界情况 & 注意事项

1. **激光硬件未连接** — `init()` 失败时 `g_lidar_ok = false`，后续跳过，不阻塞。
2. **弯道边墙误判** — 急弯处激光可能打到边墙产生近距读数。依赖 `lidar_thresh` + `lidar_min_frames` 去抖抑制。
3. **方向固定右绕** — 当前不判断挡板在赛道左侧还是右侧，一律朝右推中线。对大多数场景够用，但若挡板在右侧则绕行方向不理想。
4. **标定建议** — 在平坦路段用 debug 模式打印 `d_mm` 和 `row`，确认 `lidar_near` / `lidar_far` 映射准确。
5. **VL53L0X 读取模式** — 非阻塞：先 `startMeasure()` 启动后台测量 (~20ms)，下次调用时 `readResult()` 取结果。每 2 帧一读不阻塞主循环。
6. **多传感器优先级** — lidar 先于 cone 修改 mid_line。zebra/tl 的 block 刹车优先级最高（红灯/斑马线停车不绕行）。