feat: 斑马线停车LCD图片提示 + 冷却期bang-bang转向

- 斑马线Z_STOP期间LCD显示pedestrian_stop.png图片，不受showImg控制
- 进入/离开Z_STOP时清屏，仅状态切换时操作LCD，无每帧开销
- Z_COOLDOWN 3.3~5s内：一次方向锁定打满机会（偏差出现->打满->方向翻转/回正即退出）
- Z_COOLDOWN 3.3~5s降速上限13，foresee缩短至24
- 程序退出前自动清屏
- CMake: OpenCV_DIR改为本地交叉编译4.13.0路径
- 主程序改名为smartcar_test

AGENTS.md

@@ -3,7 +3,7 @@
 ## Project overview
 
 龙芯 2K0300 嵌入式自动驾驶智能车，OpenCV + Mild v12 魔改 YOLO 模型。
-板端运行，x86 Linux 交叉编译。
+板端运行，x86 Linux 交叉编译。No unit tests, no CI, no linting — tested on-device only.
 
 ## Build
 
@@ -17,27 +17,32 @@ make -j$(nproc)
 - **Cross-compilation target**: LoongArch64 (`-march=loongarch64 -mtune=loongarch64`)
 - **Toolchain**: `/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6`
 - **C++ standard**: 17
-- **OpenCV path** (device-side): `D:\PPPProgram\smartcar\opencv_device\` (mounted as `/mnt/d/...` in WSL)
+- **OpenCV path** (WSL): `/home/ilikara/loongson/opencv-4.13.0/loongson` (v4.13.0, 无 SIMD)
+- **OpenCV 设备库**: `opencv_libs/` 目录下 `.so.413` 文件，部署到设备 `/home/root/opencv/lib/` 并更新 `.so` 符号链接
 - `cross.cmake` is included **before** `cmake_minimum_required` — intentional, to set compiler before project().
+- Set `CROSS_COMPILE` to `0` in `cross.cmake` for native x86 builds.
 
 ## Project structure
 
 | Directory | Purpose |
 |-----------|---------|
-| `src/` | Source → compiled into `common_lib` (static lib) |
+| `src/` | Source (.cpp + .hpp) → compiled into `common_lib` (static lib) |
-| `lib/` | Public headers only (no .cpp) |
+| `lib/` | Public headers (.h only) + `vl53l0x/` C API sources |
-| `main/` | Entry point → executable `smartcar_demo1` |
+| `main/` | Entry points → `smartcar_demo1` (main) + `lidar_test` (standalone test) |
-| `docs/` | Architecture docs (`ARCHITECTURE.md` is the design reference) |
+| `docs/` | Design docs (ARCHITECTURE, CONE_DESIGN, ENCODER_BRAKE, LIDAR_AVOID, TRAFFIC_LIGHT) |
 | `build/` | CMake build output (gitignored) |
 
-**Executable name**: `smartcar_demo1` (CMake project name `smartcar_demo2` ≠ executable name).
+**Executable name**: `smartcar_demo1` (CMake project name is `smartcar_demo2` — mismatch is intentional).
 
 ## Excluded modules
 
-These source files exist but are **excluded from build**:
+These source files exist in `src/` but are **excluded from build** via CMakeLists.txt `FILTER EXCLUDE`:
 
-- `vl53l0x.cpp` — laser ranging module, hardware not connected
 - `zebra_detect.cpp` — classic zebra-crossing detection (replaced by Mild model)
+- `PIDController.cpp` — unused (motor is open-loop direct drive)
+- `serial.cpp` — Vofa/serial image streaming, unused
+
+Note: `vl53l0x.cpp` IS compiled (lidar ranging is active).
 
 ## Device-side operation
 
@@ -46,101 +51,69 @@ Controlled via `ctl.sh` (writes to text files, no CLI args):
 ```sh
 # On device:
 sh ctl.sh init          # initialize GPIO/PWM pins + write default config files
-sh ctl.sh start         # start the demo in background
+sh ctl.sh start         # init_pins + start the demo in background
 sh ctl.sh stop          # stop and zero PWM
 ```
 
-`ctl.sh` sets default config values by writing to files like `./speed`, `./kp`, `./steer_gain`, etc. The running binary reads these files at startup and optionally re-reads them every 7 frames (when `./debug` = 1).
+`ctl.sh` writes default config values to text files (`./speed`, `./steer_gain`, etc.). The binary reads them at startup via `cfg_load_all()` and re-reads every 30 frames when `./debug` = 1.
 
-`start.sh` is an alternative launcher using `LD_PRELOAD=./gpio_fix_final.so` for the GPIO 74 workaround.
+**Bug**: `start.sh` references binary as `smartcar_demo` but actual executable is `smartcar_demo1` — use `ctl.sh` instead.
 
 ## Configuration system
 
-All config is via single-value text files in the working directory:
+All config is via single-value text files in the working directory. The full list of parameters and defaults is in `lib/global.h` (struct `CfgCache`). Config is loaded by `cfg_load_all()` in `src/global.cpp`.
 
-### Steering
+Key groups:
-- `./speed` (double) — target speed (duty cycle %)
+- **Steering**: `speed`, `deadband`, `steer_gain`, `center_bias`, `foresee`
-- `./deadband` — steering deadband (pixels)
+- **Debug**: `start` (0/1), `showImg` (0/1), `debug` (0/1), `destfps`, `saveImg`
-- `./steer_gain` — steering gain multiplier
+- **Zebra**: `zebrasee`
-- `./center_bias` — midline offset correction
+- **Cone avoidance**: `cone_avoid_gain`, `cone_avoid_range`, `cone_speed`, `cone_min_frames`, `cone_margin`, `cone_thresh`, `cone_hold_frames`, `cone_return_gain`, `cone_return_frames`
-- `./foresee` — look-ahead row for steering
+- **Curve slowdown**: `curve_slope` (default 0.4), `curve_min` (default 0.8)
-
+- **Encoder brake**: `brake_scale`, `brake_max`
-### Debug / Display
+- **Lidar avoidance**: `lidar_enable`, `lidar_thresh`, `lidar_pre`, `lidar_near`, `lidar_far`, `lidar_near_start`, `lidar_near_end`, `lidar_far_span`, `lidar_min_frames`, `lidar_avoid_gain`, `lidar_avoid_range`, `lidar_speed`, `lidar_hold_frames`
-- `./start` (0/1) — motor enable switch
-- `./showImg` (0/1) — LCD display toggle
-- `./debug` (0/1) — when 1, reloads all config files every 7 frames
-- `./destfps` — target framerate
-- `./saveImg` — when written as 1 (in debug mode), saves current frame
-
-### Zebra crossing
-- `./zebrasee` — zebra crossing near-threshold (cy > this = near)
-
-### Cone avoidance
-- `./cone_avoid_gain` — midline deformation push amount (normalized)
-- `./cone_avoid_range` — deformation ramp steepness
-- `./cone_speed` — speed multiplier when cone triggered
-- `./cone_min_frames` — consecutive confirmation frames
-- `./cone_margin` — 0=center point, 1=box edge
-- `./cone_thresh` — confidence threshold
-- `./cone_hold_frames` — hold deformation frames after cone disappears
-
-### Encoder brake
-- `./brake_scale` — speed (pps) → brake duty cycle (ns) scaling factor
-- `./brake_max` — maximum brake duty cycle (ns)
-
-### PID gains (reserved, motor is open-loop)
-- `./kp`, `./ki`, `./kd` — steering PID (currently not used)
-- `./mortor_kp`, `./mortor_ki`, `./mortor_kd` — motor encoder PID (note: spelling `mortor` is intentional)
 
 ## Architecture
 
-### Per-frame pipeline in `CameraHandler()` ([`src/camera.cpp`](file:///D:\PPPProgram\smartcar\smartcar2\src\camera.cpp))
+### Per-frame pipeline in `CameraHandler()` (`src/camera.cpp:755`)
 
-1. **Capture frame** — 640×480 MJPEG → `cv::imdecode(IMREAD_REDUCED_COLOR_4)` → 1/4 decode to 320×240 BGR
+```
-2. **Save image** (debug mode only, when `./saveImg` = 1)
+CameraHandler()
-3. **Vision line tracking** — resize to 80×60 → HSV dual-channel Otsu → floodFill → longest-run search → `left_line[60]`, `right_line[60]`, `mid_line[60]`
+├── capture_frame()           // 1. MJPG 取帧 → 1/4解码 320×240 BGR
-4. **Model inference** — every 2nd frame, Mild v12 model at 160×120, 4 classes: cone(0) / red light(1) / green light(2) / zebra(3)
+├── save_image_if_requested() // 2. 保存帧 (debug)
-5. **Zebra state machine** — NORMAL → STOP(4s) → COOLDOWN(5s) → NORMAL, with debounce (5 frames min, far-enough cy check)
+├── image_main()              // 3. 视觉巡线 (80×60 HSV-Otsu-FloodFill)
-6. **Traffic light state machine** — TL_NORMAL → TL_STOP (red≥3 frames) → TL_WAIT_GREEN (red gone) → TL_NORMAL (green≥3 frames)
+├── lidar_avoid_process()     // 4. VL53L0X 激光避障 (每2帧) + 中线变形
-7. **Cone avoidance** — confirmed cone detection deforms midline to steer away, with hold-frames decay after cone disappears
+├── run_model_inference()     // 5. Mild v12 每2帧推理 (160×120, 4类)
-8. **Steering servo** — deadband-filtered proportional control via PWM
+├── zebra_process()           // 6a. 斑马线去抖 + Z_STOP/Z_COOLDOWN
-9. **Motor control** — open-loop duty cycle with curve slowdown in turns; encoder brake on zebra/red-light stop
+├── traffic_light_process()   // 6b. 红绿灯 TL_NORMAL→TL_STOP→TL_WAIT_GREEN
-10. **LCD rendering** — `/dev/fb0` mmap, RGB565 conversion, bounding boxes + state indicator overlay
+├── cone_detect_and_deform()  // 6c. 锥桶检测 + 中线变形 + 保持衰减
-11. **FPS logging** — every 15 frames, prints per-stage timing
+├── steering_update()         // 7. 舵机比例控制 (deadband过滤)
+├── motor_update()            // 8. 电机开环PWM + 编码器刹车 + 弯道减速
+├── lcd_render()              // 9. LCD RGB565渲染 (边界线+检测框+状态)
+└── fps_log()                 // 10. 分步耗时日志
+```
 
-### Motor control details
+Also: `malloc_trim(0)` every 150 frames to return freed memory to OS (embedded optimization).
 
-- **Open-loop**: `MotorController::updateduty(spd)` — direct PWM duty cycle, no encoder feedback in normal driving.
+### Motor control
-- **Encoder brake**: When `zebra_block=true` (red light or zebra stop), reads `g_enc_speed` from the encoder thread and applies reverse braking proportional to current speed.
+
-- **Encoder thread** ([`control.cpp:32`](file:///D:\PPPProgram\smartcar\smartcar2\src\control.cpp#L32)): polls GPIO67 (LSB pulse) + GPIO72 (direction) at ~2kHz, 100ms window speed calculation → `g_enc_speed` (pulses/sec). Now includes 500µs sleep to prevent CPU starvation.
+- **Open-loop**: `MotorController::updateduty(spd)` — direct PWM, no encoder feedback in normal driving.
-- **Curve slowdown**: `speed *= (1.0 - |deviation| × 0.4)`, clamped to ≥ 60%.
+- **Encoder brake**: When `zebra_block || tl_block`, reads `g_enc_speed` from encoder thread and applies reverse braking.
+- **Encoder thread** (`control.cpp`): polls GPIO67 (pulse) + GPIO72 (direction), 100ms window → `g_enc_speed` (pulses/sec). 500µs sleep to prevent CPU starvation.
+- **Curve slowdown**: `speed *= (1.0 - |deviation| × curve_slope)`, clamped to ≥ `curve_min`.
+- **Cone/lidar speed reduction**: Code exists but is currently **commented out** in `motor_update()`.
 
 ### Model: Mild v12
 
-- **Architecture**: 3→9→9→13→13→19→19→19→44→44→64→64 → head(64→64,dw) → out(64→9)
+- **Input**: 160×120 BGR, **Output**: 4 classes + background
-- **Input**: 160×120 BGR
+- **Class indices**: 0=cone, 1=red light, 2=green light, 3=zebra
-- **Output**: 4 classes (cone / red light / green light / zebra) + background
+- **Per-class thresholds**: `{0.65, 0.75, 0.80, 0.82}` (cone/red/green/zebra)
-- **Classes used in decision logic**:
-  - cls=0 (cone) → midline deformation avoidance
-  - cls=1 (red light) → traffic light state machine stop
-  - cls=2 (green light) → traffic light state machine resume
-  - cls=3 (zebra) → zebra crossing state machine stop
-- **Per-class thresholds**: `{0.90, 0.75, 0.80, 0.90}` (cone/red/green/zebra)
 - **Weight file**: `mild_v12.bin` (105KB custom binary format)
-- **Inference engine**: `src/model_v10.{hpp,cpp}` (names kept as `model_v10_*` for compatibility, internally Mild v3)
+- **Inference engine**: `src/model_v10.{hpp,cpp}` (file names say v10, internally Mild v3 — historical naming)
-
-### Vision pipeline details
-
-- Processing resolution: 80×60 (optimal balance for 2K0300 CPU)
-- HSV dual-channel Otsu: H channel (blue hue) + S channel (saturation) → bitwise AND → track region
-- FloodFill: seed at bottom center (40, 50), 8-connected, extracts connected track
-- Per-row longest-run search on floodFill result → left/right boundaries
-- Missing-line recovery: propagate downward midline + virtual boundaries
 
 ## Style and conventions
 
-- C++ files have **no copyright headers** — comments are ascii-box-style block comments when present
+- No copyright headers — comments use ascii-box-style block dividers
-- Header guards use `#ifndef FILENAME_H_` / `#define FILENAME_H_` format (with some exceptions: `#pragma once` in model files)
+- Header guards: `#ifndef FILENAME_H_` / `#define FILENAME_H_` (some model files use `#pragma once`)
-- Global state uses `extern` globals (e.g., `g_cfg`, `g_steer_deviation`, `g_boxes`)
+- Global state via `extern` globals (e.g., `g_cfg`, `g_steer_deviation`, `g_boxes`)
-- `lib/` contains .h files only; `src/` contains .cpp files only
+- `lib/` has `.h` headers; `src/` has `.cpp` implementations + `.hpp` model headers
-- No unit tests, no CI, no linting — this is embedded code tested on-device
+- No unit tests, no CI, no linting — do not add comments to code unless asked

CMakeLists.txt

@@ -7,13 +7,14 @@ cmake_minimum_required(VERSION 3.5.0)
 # 设置 C++ 标准
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -pthread -Wall -march=loongarch64 -mtune=loongarch64 -ffast-math -funroll-loops -fomit-frame-pointer")
+set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-rpath,/home/root/opencv/lib -Wl,--disable-new-dtags")
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall")
 
 # 定义项目名称和版本
 project(smartcar_demo2 VERSION 0.1.0 LANGUAGES C CXX)
 
 # OpenCV 设备端路径
-set(OpenCV_DIR /mnt/d/PPPProgram/smartcar/opencv_device/lib/cmake/opencv4)
+set(OpenCV_DIR /home/spdis/loongson/opencv-4.13.0/loongson/lib/cmake/opencv4)
 find_package(OpenCV REQUIRED)
 include_directories(${OpenCV_INCLUDE_DIRS})
 message(STATUS "OpenCV Include Directories: ${OpenCV_INCLUDE_DIRS}")
@@ -21,6 +22,7 @@ message(STATUS "OpenCV Include Directories: ${OpenCV_INCLUDE_DIRS}")
 # 项目头文件路径
 include_directories(src)
 include_directories(lib)
+include_directories(lib/vl53l0x)
 
 # 收集 src 下所有 .cpp (自动发现)
 aux_source_directory(src DIR_SRCS)
@@ -29,8 +31,17 @@ aux_source_directory(src DIR_SRCS)
 #   vl53l0x.cpp   — 激光测距, 硬件未接
 #   zebra_detect.cpp — 经典斑马线检测, 已被 Mild 模型替代
 #   PIDController.cpp — PID 类未使用 (电机开环直驱)
-list(FILTER DIR_SRCS EXCLUDE REGEX "(vl53l0x\\.cpp|zebra_detect\\.cpp|PIDController\\.cpp)")
+#   serial.cpp — Vofa/串口图传未使用
+list(FILTER DIR_SRCS EXCLUDE REGEX "(zebra_detect\\.cpp|PIDController\\.cpp|serial\\.cpp)")
+
+# VL53L0X ST API 源码 (纯 C)
+set(VL53L0X_API_SRCS
+    lib/vl53l0x/vl53l0x_api.c
+    lib/vl53l0x/vl53l0x_api_core.c
+    lib/vl53l0x/vl53l0x_api_calibration.c
+    lib/vl53l0x/vl53l0x_api_ranging.c
+)
 
 # 静态库 + 主程序
-add_library(common_lib STATIC ${DIR_SRCS})
+add_library(common_lib STATIC ${DIR_SRCS} ${VL53L0X_API_SRCS})
 add_subdirectory(main)

ctl.sh

@@ -35,19 +35,15 @@ init_pins() {
     echo 11  > "$DIR/speed" 2>/dev/null
     echo 8   > "$DIR/deadband" 2>/dev/null
     echo 1.5 > "$DIR/steer_gain" 2>/dev/null
-    echo 3.5 > "$DIR/kp" 2>/dev/null
-    echo 0.3 > "$DIR/ki" 2>/dev/null
-    echo 2.0 > "$DIR/kd" 2>/dev/null
-    echo 0.6 > "$DIR/mortor_kp" 2>/dev/null
-    echo 0.2 > "$DIR/mortor_ki" 2>/dev/null
-    echo 0   > "$DIR/mortor_kd" 2>/dev/null
     echo 40  > "$DIR/foresee" 2>/dev/null
+    echo 0.7 > "$DIR/foresee_lost_scale" 2>/dev/null
+    echo 0.5 > "$DIR/sharp_turn_scale" 2>/dev/null
     echo 0   > "$DIR/start" 2>/dev/null
 
     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 1    > "$DIR/cone_min_frames"   2>/dev/null
     echo 0    > "$DIR/cone_margin"       2>/dev/null
     echo 0.80 > "$DIR/cone_thresh"       2>/dev/null
     echo 30   > "$DIR/cone_hold_frames"   2>/dev/null
@@ -55,6 +51,19 @@ init_pins() {
     echo 10   > "$DIR/brake_scale"        2>/dev/null
     echo 10000 > "$DIR/brake_max"         2>/dev/null
 
+    echo 300  > "$DIR/lidar_thresh"       2>/dev/null
+    echo 50   > "$DIR/lidar_near"         2>/dev/null
+    echo 1200 > "$DIR/lidar_far"          2>/dev/null
+    echo 1    > "$DIR/lidar_near_start"   2>/dev/null
+    echo 4    > "$DIR/lidar_near_end"     2>/dev/null
+    echo 6    > "$DIR/lidar_far_span"     2>/dev/null
+    echo 3    > "$DIR/lidar_min_frames"   2>/dev/null
+    echo 0.4  > "$DIR/lidar_avoid_gain"   2>/dev/null
+    echo 30   > "$DIR/lidar_avoid_range"  2>/dev/null
+    echo 0.4  > "$DIR/lidar_speed"        2>/dev/null
+    echo 30   > "$DIR/lidar_hold_frames"  2>/dev/null
+    echo 1    > "$DIR/lidar_enable"       2>/dev/null
+
     echo "[demo] 引脚初始化完成"
 }
 

docs/ARCHITECTURE.md

@@ -3,119 +3,104 @@
 ## 坐标系统一览
 
 ```
-               Camera 输出 640×480 MJPEG
+             Camera 输出 640×480 MJPEG
-                      │
+                    │
-          CONVERT_RGB=0 + IMREAD_REDUCED_COLOR_4
+        CONVERT_RGB=0 + IMREAD_REDUCED_COLOR_4
-          → 1/4 MJPEG 解码 → raw_frame = 160×120
+        → 1/4 MJPEG 解码 → raw_frame = 160×120
-                      │
+                    │
-          ┌───────────┤
+        ┌───────────┤
-          ▼           ▼        (raw_frame.cols/rows 决定分母)
+        ▼           ▼        (raw_frame.cols/rows 决定分母)
-   巡线空间            显示空间                模型空间
+ 巡线空间            显示空间                模型空间
-   (line_tracking    (newWidth × newHeight)  160×120  (正常)
+ (line_tracking    (newWidth × newHeight)  160×120  (正常)
-   _width ×          calc_scale = 2          640×480  (回退模式)
+ _width ×          calc_scale = 2          640×480  (回退模式)
-    line_tracking    newWidth  = lt_w × 2
+  line_tracking    newWidth  = lt_w × 2
-    _height)         newHeight = lt_h × 2
+ _height)          newHeight = lt_h × 2
-   典型值 80×60
+ 典型值 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`
-- 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 渲染仍硬编码除以 160/120（会出错），但正常运行时不会进入此模式。
+此时 LCD 渲染的硬编码缩放 `newWidth/160.0f` 会出错，但正常运行时不会进入此模式。
 
 ---
 
-## 主循环 (main.cpp → CameraHandler)
+## 主循环 (`main.cpp`)
 
 ```
 main()
- ├── cfg_load_all()                   # 从工作目录文件读取全部配置
+ ├── cfg_load_all()                    # 从工作目录文件读取全部配置
- ├── CameraInit(0, dest_fps, 320, 240) # 打开摄像头, LCD, 模型, I2C
+ ├── CameraInit(0)                     # 打开摄像头, LCD, 模型, I2C, VL53L0X, 计算巡线尺寸
- ├── ControlInit()                    # 初始化双电机 GPIO + PWM
+ ├── ControlInit()                     # 初始化双电机 GPIO + PWM + 启动编码器线程
  │
  └── while(running):
-      └── CameraHandler()             # ★ 以下逐帧执行
+      ├── CameraHandler()              # ★ 逐帧执行
+      └── target_speed = g_cfg.speed   # (debug 模式下每 30 帧重载)
 ```
 
+`CameraInit` 现在只接受 `camera_id` 一个参数（之前有 dest_fps, width, height 三个遗留参数已移除）。
+巡线分辨率由屏幕自适应算法决定：`line_tracking_w = newWidth/2`, `line_tracking_h = newHeight/2`。
+
 ---
 
-## CameraHandler 9 步流水线
+## CameraHandler 10 步流水线 (`src/camera.cpp:755`)
 
 ```
-                     ┌─────────────────────┐
+Step 1  capture_frame()           640×480 MJPEG → IMREAD_REDUCED_COLOR_4
-Step 1               │ cap.read(raw_mat)    │  640×480 MJPEG 原始字节流
+  取帧+解码                        → raw_frame = 160×120 BGR
-  取帧+解码          │ raw_mat 单通道(字节) │  → IMREAD_REDUCED_COLOR_4
+                                   (回退: raw_mat 三通道 → 直用 640×480)
-                     │ → cv::imdecode      │  → raw_frame = 160×120 BGR
+
-                     │ raw_frame = decoded  │  (回退: raw_mat 三通道→直用 640×480)
+Step 2  save_image_if_requested()  debug=1 && saveImg 文件=1 → 存 ./image/XXXXX.jpg
-                     └────────┬────────────┘
+  保存图像 (条件)
-                              │
+
-Step 2               ┌────────▼────────────┐
+Step 3  image_main()              raw_frame → resize(lt_w×lt_h) → HSV 双通道Otsu
-  保存图像 (条件)     │ g_cfg.debug==1      │  写入 ./image/image_XXXXX.jpg
+  视觉巡线                        → floodFill(种子 (lt_w/2, lt_h-10), 半径5)
-                     │ && saveImg==1 →     │  (双重条件，缺一不可)
+                                  → 逐行最长连续段搜索 → 丢线补全(row 10~59)
-                     │ saveCameraImage()   │
+                                  输出: left_line[], right_line[], mid_line[]
-                     └────────┬────────────┘
+                                  随后: mid_line_raw = mid_line (原始中线快照)
-                              │
+
-Step 3               ┌────────▼────────────┐
+Step 4  lidar_avoid_process()     每 2 帧读距 → 距离→行映射 → 去抖确认
-  视觉巡线            │ image_main()        │  raw_frame → resize(lt_w×lt_h)
+  激光避障                        确认后中线变形 (固定朝右) + 消失后保持衰减
-  (每帧都跑)           │                    │  → HSV双通道Otsu → floodFill
+                                  方向判断基于 mid_line_raw, 变形修改 mid_line
-                     │ 输出: left_line[]   │  → 逐行最长连续段搜索
+
-                     │       right_line[]  │  → 丢线补全(仅 row 10~59)
+Step 5  run_model_inference()     每 2 帧推理一次
-                     │       mid_line[]    │  → track (赛道蒙版 128/0)
+  模型推理                        raw_frame (160×120) → model_v10_detect()
-                     └────────┬────────────┘
+                                  4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线
-                              │
+                                  g_thresh = {0.65, 0.75, 0.80, 0.82}
-Step 4               ┌────────▼────────────┐
+                                  → g_boxes[16], g_box_count
-  模型推理            │ model_v10_detect()  │  每 2 帧推理一次
+
-  (每 2 帧)            │ raw_frame (160×120) │  4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线
+Step 6a zebra_process()           去抖计数 + ZNORMAL→ZSTOP(4s)→ZCOOLDOWN(5s)
-                     │ → g_boxes[16]       │  g_thresh = [0.80,0.80,0.80,0.75]
+  斑马线状态机                    触发: 连续5帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee)
-                     │ → g_box_count       │  框坐标 ∈ raw_frame 空间
+                                  刹停时 I2C 语音播报
-                     └────────┬────────────┘
+
-                              │
+Step 6b traffic_light_process()   TLNORMAL→TLSTOP→TLWAIT_GREEN
-Step 5               ┌────────▼────────────┐
+  红绿灯状态机                    红灯≥3帧 → 停车; 红灯消失 → 等绿灯; 绿灯≥3帧 → 通行
-  斑马线状态机         │ 遍历 g_boxes        │  仅处理 cls=3，取第一个斑马线框
+
-                     │ 去抖计数+远近判断    │  NORMAL→STOP(4s)→COOLDOWN(5s)
+Step 6c cone_detect_and_deform()  去抖确认 + 中线变形 + 消失后保持衰减 + 回弹
-                     │                    │  I2C 语音播报 @ 0x34
+  锥桶检测 & 中线变形              cone_hold_frames 保持 → cone_return_frames 回弹
-                     └────────┬────────────┘
+
-                              │
+Step 7  steering_update()         foresee/2 → check_row → mid_line[row]×2 - newWidth/2
-Step 6               ┌────────▼────────────┐
+  舵机控制                        deadband 过滤 → servo duty: 1500000 ± offset ns
-  舵机控制            │ if g_cfg.start:     │
+
-                     │   foresee→check_row │  偏差 = mid_line[row]×2 - newWidth/2
+Step 8  motor_update()            开环PWM + 编码器比例刹车 + 弯道减速
-                     │   mid_line[check_row]│  → g_steer_deviation ∈ [-1, 1]
+  电机控制                        (锥桶/挡板减速代码存在但已注释)
-                     │   → deviation       │  → servo duty: 1500000 ± offset
+
-                     │   → deadband 过滤   │     clamp [1.2M, 1.8M] ns
+Step 9  lcd_render()              track→BGR→ROI + 边界线(红/绿/蓝) + 检测框 + 状态指示
-                     │   → servo.setDuty()│   mid_val==255 → 跳过(无效行)
+  LCD 渲染                        RGB565 → /dev/fb0 mmap
-                     └────────┬────────────┘
+
-                              │
+Step 10 fps_log()                 分步耗时日志
-Step 7               ┌────────▼────────────┐
+  FPS 统计
-  电机控制            │ 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帧平均
-                     └─────────────────────┘
 ```
 
+另：每 150 帧调用 `malloc_trim(0)` 回收空闲内存（嵌入式优化）。
+
 ---
 
-## 视觉巡线深度展开 (image_main)
+## 视觉巡线深度展开 (`image_main`)
 
 ```
 raw_frame (160×120 或 640×480 BGR)
@@ -130,7 +115,7 @@ raw_frame (160×120 或 640×480 BGR)
   │
   ├── find_road():
   │     MORPH_OPEN(2×2 CROSS) → 种子点(lt_w/2, lt_h-10)
-  │     → 种子点处画实心圆(半径10, 255) 防种子落在黑色区域
+  │     → 种子点处画实心圆(半径5, 255) 防种子落在黑色区域
   │     → floodFill(loDiff=20, upDiff=20, 8邻域, newVal=128)
   │     → mask 提取 ROI → track (赛道内部=128, 外部=0)
   │
@@ -141,28 +126,29 @@ raw_frame (160×120 或 640×480 BGR)
   │     → left_line[row], right_line[row]
   │     → 全零行: left=right=-1
   │
-  └── 中线 + 丢线补全 (row = lt_h-1 → 10):
+  └── 中线 + 丢线补全 (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 10~59 有可靠数据；row 0~9 的 mid_line 保持初始值 -1（不可靠，不要写入 box 过滤逻辑）。
+**边界线区间有效性：** row 10~59 有数据；row 0~9 的 mid_line 保持初始值 -1（不可靠）。
 
 ---
 
 ## 舵机控制数学
 
 ```
-foresee = g_cfg.foresee         ← 前瞻行索引（像素，显示空间），默认 40
+foresee = g_cfg.foresee              ← 前瞻行索引（显示空间像素），默认 40
-check_row = foresee / calc_scale ← 转为巡线空间行号（calc_scale=2, int 整除）
+check_row = (int)foresee / calc_scale ← 转为巡线空间行号（calc_scale=2, int 整除）
-mid_val = mid_line[check_row]   ← 该行中线列坐标 (0~79)
+mid_val = mid_line[check_row]        ← 该行中线列坐标 (0~79)
 
-如果 mid_val == 255 → 跳过（丢线补全也写不到 255，仅作防御）
+如果 mid_val == -1 → 跳过（丢线行无效）
 否则：
   deviation = mid_val × 2 - newWidth / 2       ← 转为显示空间偏差(px)
-  deviation -= g_cfg.center_bias               ← 中心偏置修正
+  deviation -= g_cfg.center_bias                ← 中心偏置修正
   g_steer_deviation = deviation / (newWidth/2)  ← 归一化到 [-1, 1]
 
   if |deviation| < deadband:
@@ -179,42 +165,65 @@ mid_val = mid_line[check_row]   ← 该行中线列坐标 (0~79)
 
 ---
 
-## 电机控制数学
+## 电机控制数学 (`ControlUpdate`)
 
 ```
-ControlUpdate(speed, zebra_block):
+ControlUpdate(speed, zebra_or_tl_block):
 
-  if zebra_block || !g_cfg.start:
+  if !g_cfg.start:
       motor[0].updateduty(0)   → 两电机停转
       motor[1].updateduty(0)
-      if !g_cfg.start: mortorEN.setValue(0)  → GPIO73 拉低
+      mortorEN.setValue(0)      → GPIO73 拉低
       return
 
-  curve = 1.0 - |g_steer_deviation| × 0.4
+  if zebra_or_tl_block:                          ← 斑马线或红灯刹停
-  curve = max(curve, 0.6)        ← 最低不低于 60% 速度
+      读取 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| × g_cfg.curve_slope  ← 弯道减速 (默认 0.4)
+  curve = max(curve, g_cfg.curve_min)                     ← 最低倍率 (默认 0.8)
   spd = speed × curve
+  // 锥桶减速 (cone_speed) / 挡板减速 (lidar_speed) 代码存在但已注释
 
   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()` 中包含锥桶和挡板减速代码（`cone_is_slow()` → `× cone_speed`，
+`lidar_is_active()` → `× lidar_speed`），但当前**已注释**，仅弯道减速生效。
+
 **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()` 方法，无编码器反馈。
+**速度控制是开环的：** `MotorController` 仅包含 `updateduty()` 方法，正常行驶无编码器反馈。
 `PIDController` 类存在但从未被实例化或调用。
-MotorController.h 中也无 `updateSpeed()` 方法（之前的文档中误记了此函数）。
+
+**编码器线程** (`ControlInit` → `encoder_thread`)：独立线程高频轮询 GPIO67（LSB脉冲）+ GPIO72（方向），
+每 100ms 计算一次速度 → `g_enc_speed`（脉冲/秒），含 500µs sleep 防止 CPU 饿死。
 
 ---
 
 ## 斑马线状态机
 
 ```
-检测到 cls=3 → 取第一个斑马线框 → 去抖计数+追踪最远cy
+检测到 cls=3 → 取第一个斑马线框（之后 break）→ 去抖计数+追踪最远cy
 
 ZNORMAL 期间:
   zebra_seen: g_zc_frames++, g_zc_min_cy = min(g_zc_min_cy, zebra_cy)
@@ -231,14 +240,14 @@ ZNORMAL 期间:
          │NORMAL│ ─────────────────────────────────────► ┌──────┐
          │      │ ◄───────────────────────────────────── │ STOP  │
          └──────┘   冷却5秒结束                           │      │
-              ▲                                           └──┬───┘
+              ▲                                          └──┬───┘
-              │                    4秒后                      │
+              │                   4秒后                      │
-              │           ┌──────────┐ ◄─────────────────────┘
+              │          ┌──────────┐ ◄─────────────────────┘
-              └───────────│ COOLDOWN │
+              └──────────│ COOLDOWN │
-                          └──────────┘
+                         └──────────┘
 
 NORMAL:   允许通行，检测斑马线
-STOP:     刹停 4 秒，g_zstate==Z_STOP 传给 ControlUpdate 第二个参数
+STOP:     刹停 4 秒，g_zstate==Z_STOP 传给 motor_update → 编码器刹车
 COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触发）
           仅跳过检测，不影响其他功能（舵机/巡线正常）
 ```
@@ -247,56 +256,178 @@ COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触
 - `g_zc_min_cy`：NORMAL 期间追踪斑马线出现时的最小 cy（越远值越小）。未检测到斑马线时衰减减2/帧，归零后重置为 120。
 - `ZEBRA_FAR_CY = 50`：斑马线的 cy 必须曾在 ≤50 处出现过（即"从远处来"）
 - `zebrasee`（默认 60）：当前斑马线 cy > 此值视为"足够近"，触发停车
-- 去抖衰减速度：未检测到时每次 `-2`（比锥桶设计中的 `-1` 更快下降）
+- 去抖衰减速度：未检测到时每次 `-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：
+所有配置通过工作目录下的纯文本文件读写。文件不存在时 `readDoubleFromFile` 返回 0：
 
 | 文件 | 类型 | 代码默认 | ctl.sh 写入 | 含义 |
 |---|---|---|---|---|
 | `./speed` | double | 60 | 11 | 目标速度 (% 占空比) |
 | `./start` | int(0/1) | 0 | 0 | 使能开关，1=电机运行 |
-| `./debug` | int(0/1) | 0 | (不写入) | 每7帧重载配置+允许存图 |
+| `./debug` | int(0/1) | 0 | (不写入) | 每30帧重载配置+允许存图 |
 | `./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 | 1 | 1 | 锥桶确认帧数 (单帧即确认) |
+| `./cone_margin` | int | 0 | 0 | 0=中心点, 1=框边缘检查 |
+| `./cone_thresh` | double | 0.80 | 0.80 | 锥桶置信度阈值 |
+| `./cone_hold_frames` | int | 15 | 30 | 锥桶消失后保持变形帧数 |
+| `./cone_return_gain` | double | 1.0 | (不写入) | 回弹力度 (1.0=等同避让) |
+| `./cone_return_frames` | int | 30 | (不写入) | 回弹持续帧数 |
+| `./curve_slope` | double | 0.4 | (不写入) | 弯道减速斜率 |
+| `./curve_min` | double | 0.8 | (不写入) | 弯道最低速度倍率 |
+| `./brake_scale` | double | 10 | 10 | 刹车速度→占空比系数 |
+| `./brake_max` | int | 10000 | 10000 | 最大刹车占空比 (ns) |
+| `./lidar_enable` | int(0/1) | 1 | 1 | 激光避障总开关 |
+| `./lidar_thresh` | int | 300 | 300 | 障碍判定距离阈值 (mm) |
+| `./lidar_pre` | int | 800 | (不写入) | 预触发去抖窗口 (mm) |
+| `./lidar_near` | int | 50 | 50 | row=lt_h-1 对应距离 (mm) |
+| `./lidar_far` | int | 1200 | 1200 | row=10 对应距离 (mm) |
+| `./lidar_near_start` | int | 1 | 1 | near_valid 起始偏移 |
+| `./lidar_near_end` | int | 4 | 4 | near_valid 终止偏移 |
+| `./lidar_far_span` | int | 6 | 6 | far_lost 检测跨度 |
+| `./lidar_min_frames` | int | 3 | 3 | 连续确认帧数 |
+| `./lidar_avoid_gain` | double | 0.4 | 0.4 | 绕行推离力度 (归一化) |
+| `./lidar_avoid_range` | int | 30 | 30 | 绕行斜坡陡峭度 |
+| `./lidar_speed` | double | 0.4 | 0.4 | 绕行速度倍率 (当前未生效) |
+| `./lidar_hold_frames` | int | 30 | 30 | 消失后保持帧数 |
 
-**debug 模式：** `./debug` = 1 时，主循环每7帧调用一次 `cfg_load_all()` 重读全部配置，支持热更新调参。同时允许 `saveImg` 保存图像。
+**debug 模式：** `./debug` = 1 时，主循环每 30 帧调用一次 `cfg_load_all()` 重读全部配置，支持热更新调参。同时允许 `saveImg` 保存图像。
 
-**注意**：`global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。
+**注意：** `global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。
 
 ---
 
 ## 代码中未使用的模块
 
-以下 `.cpp` 文件已编译进 `common_lib` 但主循环中从未调用：
+以下 `.cpp` 文件存在于 `src/` 中但已被 `CMakeLists.txt` 的 `list(FILTER ... EXCLUDE)` 排除编译：
 
-| 文件 | 功能 | 状态 |
+| 文件 | 功能 | 排除原因 |
 |---|---|---|
-| `PIDController.cpp` | 位置式/增量式 PID | 类存在但无实例化，无调用 |
+| `zebra_detect.cpp` | 经典斑马线检测 | 已被 Mild 模型替代 |
-| `serial.cpp` | VOFA 串口可视化 (vofa_justfloat/vofa_image) | 已实现但无调用 |
+| `PIDController.cpp` | 位置式/增量式 PID | 电机开环直驱，无调用点 |
-| `Timer.cpp` | 定时器线程 | 已实现但无调用（Video 依赖它但 Video 也未用） |
+| `serial.cpp` | VOFA 串口可视化 | 未使用 |
-| `video.cpp` | 视频文件流读取 | 已实现但无调用 |
+
+**注意：** `vl53l0x.cpp` 已恢复编译（激光测距已集成至 `lidar_avoid_process()`）。
+`PIDController.h` 仍在 `lib/` 中，`MotorController` 仅提供 `updateduty()`（开环占空比），
+不提供 `updateSpeed()` 方法。
 
 ---
 
-## 当前已知缺陷 / 未利用能力
+## 模型：Mild v12
 
-1. **cls=0 锥桶** — 模型已检测但被忽略（CONE_DESIGN.md 设计了方案）
+- **推理引擎**：`src/model_v10.{hpp,cpp}`（名称为兼容保留，内部为 Mild v3）
-2. **cls=1 红灯 / cls=2 绿灯** — 检测框跳过不画（LCD 渲染中 `if g_boxes[i].cls == 1 || g_boxes[i].cls == 2: continue`），无决策
+- **输入**：160×120 BGR
-3. **电机开环** — 无编码器反馈，PID 类已实现但无调用点，`MotorController` 无 `updateSpeed()` 方法
+- **输出**：4 类 + 背景 → 9 通道：0=锥桶, 1=红灯, 2=绿灯, 3=斑马线
-4. **舵机死区** — 用 `abs(deviation) < deadband` 比较像素值，deadband 单位是显示空间像素
+- **架构**：3→9→9→13→13→19→19→19→44→44→64→64 → head(64→64,dw) → out(64→9)
-5. **中线 row 范围** — 丢线补全仅覆盖 row 10~59，row 0~9 的 mid_line 保持 -1 不可靠
+- **阈值**：`{0.65, 0.75, 0.80, 0.82}`（锥桶/红灯/绿灯/斑马线）
-6. **mid_val == 255 防御** — 代码中写死但 255 永远不会出现在 mid_line 中（当前实现下）
+- **权重文件**：`mild_v12.bin`（105KB 自定义二进制格式）
-7. **LCD 坐标缩放硬编码** — 检测框绘制用 `newWidth/160.0f`，假设 raw_frame 宽=160。回退模式下 raw_frame=640 时出错
+- **推理频率**：每 2 帧一次（跳帧节省 CPU）
+
+---
+
+## 已知局限
+
+1. **电机开环** — 正常行驶无编码器反馈，仅制动时使用编码器速度做比例刹车。PID 类已实现但无调用点。
+2. **舵机死区** — 用 `abs(deviation) < deadband` 比较像素值，deadband 单位是显示空间像素。
+3. **中线 row 范围** — 丢线补全仅覆盖 row 10~59，row 0~9 的 mid_line 保持 -1 不可靠。
+4. **LCD 坐标缩放硬编码** — 检测框绘制用 `newWidth/160.0f`，假设 raw_frame 宽=160。回退模式（raw_frame=640）下会出错。
+5. **锥桶变形可能跳过 row 0~9** — 变形循环从 row=10 开始，row 0~9 不会改变。

docs/CONE_DESIGN.md

@@ -4,7 +4,27 @@
 
 **不改舵机代码，直接修改 `mid_line[]` 数组。** 在锥桶附近把中线"推开"一个凸起，后续巡线逻辑（deviation → deadband → servo）原样工作，车自然跟着变形后的中线绕开锥桶。
 
-不使用任何推入计时器、归还逻辑、EMA 平滑——锥桶消失后，下一帧 `image_main()` 重新计算 `mid_line`，自动恢复。
+锥桶消失后进入**保持衰减**阶段（`cone_hold_frames` 帧内变形量线性衰减），随后进入**回弹**阶段（`cone_return_frames` 帧内朝锥桶方向反推，帮助车回到赛道中央）。
+
+### 1.1 原始中线快照 (`mid_line_raw`)
+
+避障变形会原地修改 `mid_line[]`，导致方向判断被污染。为解决此问题，在 `image_main()` 之后立即保存一份快照：
+
+```
+image_main()             → mid_line[] = 赛道真实中线
+mid_line_raw = mid_line  → 快照 (60 个 int, 240 字节, 每帧复制)
+lidar_avoid_process()    → 修改 mid_line[] (lidar 推力)
+cone_detect_and_deform() → 修改 mid_line[] (锥桶推力)
+steering_update()        → 读 mid_line[] (变形后) 控制舵机
+```
+
+| 变量 | 含义 | 来源 | 被修改 | 用途 |
+|------|------|------|--------|------|
+| `mid_line_raw[]` | 赛道在哪 | `image_main()` 每帧重算 | 否 | 方向判断 |
+| `mid_line[]` | 车该往哪走 | `mid_line_raw` + 避障推力 | 是 | 舵机控制 |
+
+**所有方向判断（锥桶在赛道左侧还是右侧、回弹方向）均基于 `mid_line_raw`**，
+避免变形后的中线导致方向翻转。
 
 ---
 
@@ -158,126 +178,196 @@ foresee 行 (row=20) 收到 33% 的偏移，产生 +8px 偏差 → 刚好越过
 
 ## 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)
+位置容忍 (动态):
+  row_tol = max(2, line_tracking_height / 12)   典型值 5
+  col_tol = max(3, line_tracking_width  / 8)    典型值 10
+
+首次出现:         g_cone_frames = 1, 记录位置
+位置接近 (容差内): g_cone_frames++, 更新位置
+位置突变:         g_cone_frames = 1, 更新位置 (重新开始计数)
+未检测到:         g_cone_frames = max(0, g_cone_frames - 1)
+
+g_cone_confirmed = (g_cone_frames >= cone_min_frames)
 ```
 
-**关键差异：** 去抖只决定"是否触发变形"，变形本身没有持续时间概念。
+**cone_min_frames 默认值为 1（单帧即确认）。** 原因：视觉巡线（`image_main()` 的 floodFill）
-- 锥桶在 → 变形在（每帧重新计算 mid_line 变形）
+对赛道上的锥桶响应极快——锥桶进入视野后 1~2 帧内车就开始转弯，锥桶随即离开视野。
-- 锥桶消失 → 下帧 image_main() 重新计算 mid_line → 变形自动消失
+模型检测的窗口天然只有 1~2 帧，多帧去抖门槛（如 3）永远达不到。
-- 不去抖时的抖动：锥桶在边界附近闪烁会使 mid_line 来回跳，用去抖平滑
+边界过滤（row 有效、赛道内、conf ≥ cone_thresh）已经提供了足够的误触发保护。
 
 ---
 
-## 7. 舵机与电机
+## 7. 保持衰减 + 回弹
 
-**舵机：** 零改动。Step 6 读取 `mid_line[foresee/2]`，看到变形后的中线值，自然输出偏转方向。
+锥桶消失后不会立刻恢复正常巡线，而是经历三个阶段：
 
-**电机：** 推入期间减速：
 ```
-if (g_cone_confirmed && g_zstate != Z_STOP) {
+确认 (g_cone_confirmed && cone_seen)
-    effective_speed = target_speed * cone_speed_factor;
+  → g_cone_hold_ctr = 1, g_cone_return_ctr = 0
-} else {
+  → 冻结位置: hold_src_row/col = cone_row/col
-    effective_speed = target_speed;
+  → 记录回弹方向: 锥桶在中线左侧 → return_dir = -1 (回弹推左)
+                   锥桶在中线右侧 → return_dir = +1 (回弹推右)
+
+确认但不可见 (g_cone_confirmed && !cone_seen, 去抖衰减中)
+  → g_cone_hold_ctr = 1 (保持重置, 但不覆盖冻结位置)
+  → 使用 hold_src_row/col 继续避让变形
+
+保持衰减 (!g_cone_confirmed, 1 ~ cone_hold_frames 帧)
+  → g_cone_hold_ctr++
+  → decay = 1.0 - hold_ctr / hold_frames  (线性衰减到 0)
+  → 中线变形量乘 decay → 逐渐恢复
+
+回弹 (保持结束后 1 ~ cone_return_frames 帧)
+  → g_cone_return_ctr++
+  → 反方向推中线 (return_dir), 力度 = cone_return_gain
+  → 斜坡范围 = cone_avoid_range × 1.2 (比避让略缓)
+  → 覆盖全部有效行 (row 10 ~ lt_h-1), 不限于锥桶位置
+  → decay = 1.0 - return_ctr / return_frames (线性衰减)
+  → 回弹结束: 清零所有状态变量
+```
+
+**cone_is_slow() 在三个阶段中均返回 true**（确认 / 保持 / 回弹），但锥桶减速代码当前**已注释**。
+
+### 7.1 已修复的 BUG：hold_src_row 被 -1 覆盖导致回弹永远不执行
+
+**问题**：锥桶消失后，由于去抖计数 `g_cone_frames` 逐帧 -1 衰减，`g_cone_confirmed`
+在锥桶消失后仍保持 true 多帧（如锥桶可见 20 帧 → 消失后 confirmed 维持 18 帧）。
+在这些帧中 `cone_detect_and_deform()` 内的局部变量 `cone_row = -1`（没找到锥桶），
+但旧代码在 `if (g_cone_confirmed)` 块中无条件执行：
+
+```cpp
+// ★ 旧代码 (BUG):
+if (g_cone_confirmed) {
+    g_cone_hold_ctr     = 1;
+    g_cone_hold_src_row = cone_row;          // cone_row = -1, 覆盖有效值!
+    g_cone_hold_src_col = cone_col;          // 同上
+    g_cone_return_ctr   = 0;
+    double ms = mid_line[cone_row];          // mid_line[-1] 越界访问!
+    g_cone_return_dir = (cone_col < ms) ? -1.0 : 1.0;
 }
-ControlUpdate(effective_speed, g_zstate == Z_STOP);
 ```
 
+后果链：
+1. `hold_src_row` 被 -1 覆盖 → 有效位置丢失
+2. `mid_line[-1]` 越界访问 → 未定义行为
+3. 后续所有阶段 `src_row = hold_src_row = -1` → `if (src_row <= 10) return` 提前退出
+4. 保持变形 **从不执行**，回弹变形 **从不执行**
+
+```
+时间轴:   锥桶可见 20帧          去抖衰减 18帧        hold 15帧     return 30帧
+避让变形: ████████████████████  (src_row=-1,退出)  (同上,-1)     (同上,-1)
+保持变形:                       从不执行             从不执行
+回弹变形:                                                         从不执行
+```
+
+**修复**：只在 `cone_seen` 为 true 时更新冻结位置和回弹方向：
+
+```cpp
+// ★ 修复后:
+if (g_cone_confirmed) {
+    g_cone_hold_ctr   = 1;
+    g_cone_return_ctr = 0;
+    if (cone_seen) {                         // 只在锥桶实际可见时更新
+        g_cone_hold_src_row = cone_row;
+        g_cone_hold_src_col = cone_col;
+        double ms = mid_line[cone_row];
+        g_cone_return_dir = (cone_col < ms) ? -1.0 : 1.0;
+    }
+}
+```
+
+同时修复 `src_row` 选择逻辑：
+
+```cpp
+// ★ 旧代码:
+int src_row = g_cone_confirmed ? cone_row : g_cone_hold_src_row;
+// 当 confirmed=true 但 cone_seen=false 时 → src_row = cone_row = -1 → 提前退出
+
+// ★ 修复后:
+int src_row = (g_cone_confirmed && cone_seen) ? cone_row : g_cone_hold_src_row;
+// confirmed 但不可见时 → 使用冻结位置, 避让变形继续生效
+```
+
+修复后的时间轴：
+
+```
+时间轴:   锥桶可见 20帧          去抖衰减 18帧        hold 15帧     return 30帧
+避让变形: ████████████████████  ████████████████████ ▓▓▓▓▓▓▓▓▓▓▓▓▓▓
+保持变形:                       (用冻结位置继续避让)  (线性衰减→0)
+回弹变形:                                                          ░░░░░░░░░░░░░░
+```
+
+## 8. 舵机与电机
+
+**舵机：** 零改动。steering_update 读取 `mid_line[foresee/2]`，看到变形后的中线值，自然输出偏转方向。
+
+**电机：** `motor_update()` 中存在锥桶减速代码（`cone_is_slow()` → `speed × cone_speed`），
+但当前**已注释**，仅弯道减速生效。如需启用，取消 `motor_update()` 中对应行的注释即可。
+
 ---
 
-## 8. 参数配置
+## 9. 参数配置
 
-### 8.1 启动时一次性读取（运行中不重新读文件）
+所有锥桶参数均通过 `cfg_load_all()` 统一读取（`src/global.cpp`），debug 模式下每 30 帧热更新。
 
 | 文件 | 类型 | 默认 | 含义 |
 |---|---|---|---|
 | `./cone_avoid_gain` | double | 0.3 | 归一化推离量 (0=不推, 1=推到赛道边缘) |
 | `./cone_avoid_range` | int | 30 | 斜坡陡峭度 (越小越陡，foresee 行感受越强) |
-
+| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 (当前减速代码已注释) |
-### 8.2 热更新参数
+| `./cone_min_frames` | int | 1 | 确认帧数 (1=单帧即确认) |
-
-| 文件 | 类型 | 默认 | 含义 |
-|---|---|---|---|
-| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 |
-| `./cone_min_frames` | int | 3 | 连续确认帧数 |
 | `./cone_margin` | int | 0 | 0=中心点过滤, 1=框边缘过滤 |
 | `./cone_thresh` | double | 0.80 | 锥桶置信度阈值 (业务层, 不改 NMS) |
-
+| `./cone_hold_frames` | int | 15 | 消失后保持变形帧数 |
-### 8.3 读取实现
+| `./cone_return_gain` | double | 1.0 | 回弹力度 (1.0=等同避让力度) |
-
+| `./cone_return_frames` | int | 30 | 回弹持续帧数 |
-**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. 代码修改清单
+## 10. 代码位置
 
 ```
+lib/image_cv.h:
+  └── extern mid_line_raw  (原始中线快照声明)
+
+src/image_cv.cpp:
+  └── std::vector<int> mid_line_raw  (定义)
+
 lib/global.h:
-  ├── CfgCache 新增: cone_avoid_gain, cone_avoid_range,
+  └── CfgCache 包含: cone_avoid_gain, cone_avoid_range, cone_speed,
-  │                   cone_speed, cone_min_frames, cone_margin, cone_thresh
+                      cone_min_frames, cone_margin, cone_thresh,
-  └── 新增文件名常量
+                      cone_hold_frames, cone_return_gain, cone_return_frames
 
 src/global.cpp:
-  └── cfg_load_all() 追加热更新参数 (不含 avoid_gain/avoid_range)
+  └── cfg_load_all() 统一读取所有锥桶参数
-
-main/main.cpp:
-  └── CameraInit 之前: 读取 cone_avoid_gain, cone_avoid_range 写入 g_cfg
 
 src/camera.cpp:
-  ├── #define CONE_CLASS 0
+  ├── CameraHandler(): image_main() 后执行 mid_line_raw = mid_line (快照)
-  ├── 新增静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed
+  ├── 静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed,
-  ├── Step 5 之后: 插入锥桶检测+边界过滤+去抖
+  │             g_cone_hold_ctr, g_cone_hold_src_row/col,
-  ├── Step 5.5: 中线变形 (修改 mid_line[row] for row in [10, cone_row])
+  │             g_cone_return_ctr, g_cone_return_dir
-  ├── Step 7 电机: g_cone_confirmed → effective_speed *= cone_speed_factor
+  ├── cone_is_slow(): 判断是否处于避让/保持/回弹任一阶段
-  └── Step 8 LCD: 锥桶框改橙色
+  ├── cone_detect_and_deform(): 方向判断基于 mid_line_raw, 变形修改 mid_line
+  ├── motor_update(): cone_speed 减速 (已注释)
+  └── lcd_render(): 锥桶框橙色绘制
 
 ctl.sh:
-  └── init_pins() 追加 6 个新文件默认值
+  └── init_pins() 写入 cone_avoid_gain/range/speed/min_frames/margin/thresh/hold_frames
 ```
 
 ---
 
-## 10. 方案对比
+## 11. 方案对比
 
-| | 旧方案（舵机推入） | 新方案（中线变形） |
+| | 旧方案（舵机推入） | 当前方案（中线变形） |
 |---|---|---|
 | 舵机代码 | 需改动 | 零改动 |
-| deadband | 需绕过 | 自 然生效 |
+| deadband | 需绕过 | 自然生效 |
 | steer_gain | 需单独 cone_gain | 共用 |
-| 归还机制 | 需要计时器+衰减 | 自动 (image_main 重算) |
+| 归还机制 | 需计时器+衰减 | hold 衰减 + return 回弹 |
 | 平滑性 | 需 EMA | 斜坡自带平滑 |
-| 参数数量 | 7 | 6 |
+| 参数数量 | 7 | 9 (含 hold/return) |
-| 锥桶消失恢复 | 需衰减逻辑 | 下一帧自动 |
+| 锥桶消失恢复 | 需手动衰减 | 自动: hold 衰减 → return 回弹 → 正常 |

docs/ENCODER_BRAKE.md

@@ -2,7 +2,7 @@
 
 ## 1. 硬件
 
-编码器2，引脚：
+编码器引脚：
 
 | 信号 | 引脚 | 模式 |
 |---|---|---|
@@ -11,77 +11,99 @@
 
 两引脚均属 gpiochip64（GPA64，base=64，16 pins）。
 
-## 2. 触发条件
+## 2. 编码器线程
 
-`ControlUpdate(speed, zebra_block)` 中 `zebra_block == true` 时进入刹车流程。`zebra_block` 由 `CameraHandler` Step 5 斑马线状态机传入——当 `g_zstate == Z_STOP` 时为 true。
+`ControlInit()` 启动独立线程 `encoder_thread()`（`src/control.cpp:30`），持续运行直到 `ControlExit()`。
-
-## 3. 刹车状态机
 
 ```
-                 ┌──────────┐
+encoder_thread():
-   正常行驶 ───→ │ IDLE     │
+  高频轮询 GPIO67 (LSB 边沿) + GPIO72 (方向)
-                 └────┬─────┘
+  轮询间隔: 500µs sleep (~2kHz)
-                      │ zebra_block==true
+
-                      ▼
+  每 100ms 窗口:
-                 ┌──────────┐
+    g_enc_speed = pulse_cnt / elapsed_time   (脉冲/秒, 原子变量)
-                 │ BRAKING  │ ← 施加 -30% 占空比
+    g_enc_dir   = GPIO72 当前值             (原子变量)
-                 │          │    每帧读 GPIO 67
+    pulse_cnt 归零, 开始新窗口
-                 └────┬─────┘
-          ┌───────────┴───────────┐
-          │ 脉冲变化               │ 脉冲不变
-          ▼                       ▼
-   轮子在转 → 继续刹车        g_brake_still++
-   重置计数器                        │
-                            连续 5 帧无变化？
-                            或超时 60 帧？
-                                    │
-                                    ▼
-                              ┌──────────┐
-                              │ HOLDING  │ ← duty=0，保持静止
-                              └──────────┘
-                                    │
-                            zebra_block==false
-                                    │
-                                    ▼
-                              ┌──────────┐
-                              │ IDLE     │
-                              └──────────┘
 ```
 
-## 4. 参数
+通过 sysfs 文件描述符轮询（`GPIO::readValue()`），`lseek` + `read`。不依赖中断。
 
-定义在 `src/control.cpp`：
+## 3. 触发条件
 
-| 常量 | 值 | 含义 |
+`ControlUpdate(speed, block)` 中 `block == true` 时进入刹车流程。
-|---|---|---|
-| `BRAKE_STILL_THRESH` | 5 | 连续无脉冲帧数阈值，超过即判停 |
-| `BRAKE_TIMEOUT` | 60 | 最长刹车帧数（~2s @30fps），强制释放 |
-| `BRAKE_DUTY` | -30 | 刹车占空比（-100~100，负值=反转） |
 
-## 5. 读取方式
+`block` 信号来源（两者取 OR）：
+- **斑马线**: `zebra_process()` 返回 true 当 `g_zstate == Z_STOP`
+- **红绿灯**: `traffic_light_process()` 返回 true 当 `g_tl_state != TL_NORMAL`
 
-GPIO 67 通过 sysfs 文件描述符轮询（`GPIO::readValue()`），每帧 `lseek` + `read` 一次。不依赖中断。
+调用链: `motor_update(zebra_block, tl_block)` → `ControlUpdate(final_spd, zebra_block || tl_block)`
 
-**为什么轮询够用：** 30fps 帧率 = 33ms 间隔。编码器假设 100+ 脉冲/转，即使 1 转/秒的极慢速度也有 3+ 脉冲/帧间隔，不会漏判。
+## 4. 比例刹车
 
-## 6. 刹车强度
+无状态机，每帧根据编码器实时速度计算刹车力度：
 
 ```
-motor[i]->updateduty(-30)
+ControlUpdate(speed, zebra_block):
+
+  if !g_cfg.start → 两电机 duty=0, GPIO73 拉低, return
+
+  if zebra_block:
+      cur_speed = g_enc_speed.load()     ← 编码器线程原子读
+      cur_dir   = g_enc_dir.load()
+
+      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.0             ← ns → % (period=50000ns)
+          motor[i]->updateduty(±brake_pct)         ← 反向占空比 (方向取反)
+      else:                              ← 已停止 (< 0.5 pps)
+          motor[i]->updateduty(0)                  ← 不施加刹车
+      return
+
+  正常行驶:
+      motor[0].updateduty(speed)
+      motor[1].updateduty(speed)
+      mortorEN.setValue(1)
 ```
 
-`MotorController::updateduty()` 内：
+**特点：**
-- 占空比：`period * |duty| / 100 = 50000 * 30 / 100 = 15000 ns`
+- **比例刹车**: 速度越快 → 刹车占空比越大 → 刹车力越强
-- 方向：`duty < 0` → `directionGPIO.setValue(false)` → 反转
+- **自动停止**: 速度降到 0.5 pps 以下自动释放刹车，防止过冲
+- **反向驱动**: 根据编码器方向 `cur_dir` 施加反方向 PWM
 
-即 30% 占空比的反向驱动，足够刹停但不会让车向后猛冲。
+## 5. 参数
 
-## 7. 编码器生效范围
+可通过文本文件热更新（debug 模式下每 30 帧重载）：
 
-- **仅在斑马线 STOP 时**使用编码器
+| 文件 | 类型 | 默认 | 含义 |
-- 正常行驶时编码器处于"打开但未使用"状态（fd 保持 open，但不 read）
+|---|---|---|---|
-- 锥桶避障不使用编码器
+| `./brake_scale` | double | 10 | 速度(pps) → 刹车占空比(ns) 缩放系数 |
-- `g_cfg.start == 0` 时刹车状态机重置
+| `./brake_max` | int | 10000 | 最大刹车占空比 (ns), 上限保护 |
+
+**数值示例：** 编码器读数 500 pps → brake_ns = 500 × 10 = 5000 ns → brake_pct = 5000/500 = 10%
+（period=50000ns, 即 duty = 50000 × 10/100 = 5000 ns 的 PWM 反转输出）
+
+## 6. `updateduty()` 内部
+
+```
+MotorController::updateduty(duty):
+  pwm_ns = period × |duty| / 100        ← 转为占空比 ns
+  → 写入 sysfs duty_cycle
+
+  duty > 0  → GPIO 方向 = 1 (正转)
+  duty ≤ 0  → GPIO 方向 = 0 (反转)
+```
+
+motor[0]: pwmchip8/pwm2 + GPIO12（左电机）
+motor[1]: pwmchip8/pwm1 + GPIO13（右电机）
+GPIO73: 电机使能 (mortorEN)
+
+## 7. 生效范围
+
+- **斑马线 STOP + 红绿灯 STOP/WAIT_GREEN** 时使用编码器刹车
+- 正常行驶时编码器线程持续运行但刹车不触发
+- 锥桶/挡板避障不使用编码器刹车（仅减速+绕行）
+- `g_cfg.start == 0` 时全部电机停转，不进入刹车流程
 
 ## 8. 关键代码路径
 
@@ -89,7 +111,12 @@ motor[i]->updateduty(-30)
 main.cpp
   └── while(running)
         └── CameraHandler()
-              └── ControlUpdate(target_speed, g_zstate == Z_STOP)
+              ├── zebra_process()  → zebra_block
-                    └── if (zebra_block) → 刹车状态机
+              ├── traffic_light_process() → tl_block
-                              └── encoderLSB.readValue()  ← GPIO 67
+              └── motor_update(zebra_block, tl_block)
+                    └── ControlUpdate(final_spd, zebra_block || tl_block)
+                          └── if (block) → 读 g_enc_speed/g_enc_dir → 比例刹车
+
+ControlInit()
+  └── encoder_thread() 后台运行 → g_enc_speed, g_enc_dir
 ```

docs/LIDAR_AVOID.md

@@ -0,0 +1,187 @@
+# 激光雷达挡板避障设计
+
+## 概述
+
+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 刹车优先级最高（红灯/斑马线停车不绕行）。

docs/TRAFFIC_LIGHT.md

@@ -2,11 +2,11 @@
 
 ## 1. 模型检测
 
-NanoDet V10 模型 4 类输出中：
+Mild v12 模型 4 类输出中：
 - cls=1: 红灯
 - cls=2: 绿灯
 
-模型置信度阈值由 `g_thresh[1]` 和 `g_thresh[2]` 控制（`src/camera.cpp:25`），当前分别为 0.80。
+模型置信度阈值由 `g_thresh[1]` 和 `g_thresh[2]` 控制（`src/camera.cpp:42`），当前分别为 0.75（红灯）和 0.80（绿灯）。
 
 ## 2. 状态机
 
@@ -41,7 +41,8 @@ NanoDet V10 模型 4 类输出中：
 和斑马线完全一样，复用编码器比例刹车：
 
 ```
-ControlUpdate(spd, g_zstate == Z_STOP || tl_block)
+motor_update(zebra_block, tl_block)
+  → ControlUpdate(final_spd, zebra_block || tl_block)
 
 刹车信号 = 斑马线STOP || 红灯STOP || 等待绿灯
 ```
@@ -67,20 +68,21 @@ LCD 状态: "R"=红灯停车 "G"=等绿灯, 拼接斑马线状态
 ## 5. 关键代码路径
 
 ```
-CameraHandler (src/camera.cpp)
+CameraHandler (src/camera.cpp:755)
-  Step 4:   模型推理 → g_boxes[], g_box_count
+  Step 5:   模型推理 → g_boxes[], g_box_count
-  Step 5:   斑马线状态机
+  Step 6a:  斑马线状态机
-  Step 5.5: 红绿灯状态机
+  Step 6b:  红绿灯状态机
-             ├─ 遍历 g_boxes, 统计 red_seen/green_seen
+              ├─ 遍历 g_boxes, 统计 red_seen/green_seen
-             ├─ 去抖计数 (3帧确认, 衰减-1/帧)
+              ├─ 去抖计数 (3帧确认, 衰减-1/帧)
-             └─ 状态跃迁
+              └─ 状态跃迁
-  Step 5.6: 锥桶检测 (g_zstate!=Z_STOP && g_tl_state==TL_NORMAL 时才运行)
+  Step 6c:  锥桶检测 (g_zstate!=Z_STOP && g_tl_state==TL_NORMAL 时才运行)
-  Step 7:   电机控制
+  Step 8:   电机控制
-             ControlUpdate(spd, g_zstate==Z_STOP || tl_block)
+              motor_update(zebra_block, tl_block)
+              → ControlUpdate(final_spd, zebra_block || tl_block)
 
 control.cpp:
-  ControlUpdate(speed, zebra_block)
+  ControlUpdate(speed, block)
-    └─ zebra_block → 编码器线程读速度 → 比例刹车
+    └─ block → 编码器线程读速度 → 比例刹车
 ```
 
 ## 6. 状态变量

lib/camera.h

@@ -1,27 +1,15 @@
 #ifndef CAMERA_H_
 #define CAMERA_H_
 
+#include <cstdint>
 #include <opencv2/opencv.hpp>
-#include <iostream>
-#include <fstream>
-#include <sstream>
-#include <iomanip>
-#include <mutex>
-#include <fcntl.h>
-#include <unistd.h>
-#include <sys/mman.h>
-#include <sys/ioctl.h>
-#include <linux/fb.h>
-#include <algorithm>
-#include <chrono>
-#include <thread>
 
 #include "image_cv.h"
 #include "global.h"
 #include "frame_buffer.h"
 #include "control.h"
 
-int CameraInit(uint8_t camera_id, double dest_fps, int width, int height);
+int CameraInit(int camera_id);
 int CameraHandler(void);
 void cameraDeInit(void);
 

lib/global.h

@@ -12,6 +12,8 @@ const std::string start_file = "./start";
 const std::string showImg_file = "./showImg";
 const std::string destfps_file = "./destfps";
 const std::string foresee_file = "./foresee";
+const std::string foresee_lost_scale_file = "./foresee_lost_scale";
+const std::string sharp_turn_scale_file = "./sharp_turn_scale";
 const std::string zebrasee_file = "./zebrasee";
 const std::string saveImg_file = "./saveImg";
 const std::string speed_file = "./speed";
@@ -27,10 +29,29 @@ const std::string cone_min_frames_file  = "./cone_min_frames";
 const std::string cone_margin_file      = "./cone_margin";
 const std::string cone_thresh_file      = "./cone_thresh";
 const std::string cone_hold_frames_file  = "./cone_hold_frames";
+const std::string cone_return_gain_file  = "./cone_return_gain";
+const std::string cone_return_frames_file = "./cone_return_frames";
 
 const std::string brake_scale_file = "./brake_scale";
 const std::string brake_max_file   = "./brake_max";
 
+const std::string curve_slope_file = "./curve_slope";
+const std::string curve_min_file   = "./curve_min";
+
+const std::string lidar_thresh_file      = "./lidar_thresh";
+const std::string lidar_pre_file         = "./lidar_pre";
+const std::string lidar_near_file        = "./lidar_near";
+const std::string lidar_far_file         = "./lidar_far";
+const std::string lidar_near_start_file  = "./lidar_near_start";
+const std::string lidar_near_end_file    = "./lidar_near_end";
+const std::string lidar_far_span_file    = "./lidar_far_span";
+const std::string lidar_min_frames_file  = "./lidar_min_frames";
+const std::string lidar_avoid_gain_file  = "./lidar_avoid_gain";
+const std::string lidar_avoid_range_file = "./lidar_avoid_range";
+const std::string lidar_speed_file       = "./lidar_speed";
+const std::string lidar_hold_frames_file = "./lidar_hold_frames";
+const std::string lidar_enable_file      = "./lidar_enable";
+
 double readDoubleFromFile(const std::string &filename);
 bool   readFlag(const std::string &filename);
 void   cfg_load_all();
@@ -39,6 +60,8 @@ void   cfg_load_all();
 struct CfgCache {
     double speed       = 60;     // 目标速度(占空比 %)
     double foresee     = 80;     // 前瞻行
+    double foresee_lost_scale = 0.7;  // 丢线时前瞻缩放 (<1=看更远)
+    double sharp_turn_scale   = 0.5;  // 急弯时前瞻缩放 (<1=看更远)
     double zebrasee    = 60;     // 斑马线触发距离阈值
     double deadband    = 5;      // 舵机死区
     double steer_gain  = 1.0;    // 舵机增益
@@ -50,13 +73,32 @@ struct CfgCache {
     double cone_avoid_gain  = 0.3;   // 锥桶中线变形推离量 (归一化)
     int    cone_avoid_range = 30;    // 变形斜坡陡峭度
     double cone_speed       = 0.5;   // 锥桶触发时速度倍率
-    int    cone_min_frames  = 3;     // 连续确认帧数
+    int    cone_min_frames  = 1;     // 确认帧数 (1=单帧即确认, 视觉巡线响应快导致检测窗口极短)
     int    cone_margin      = 0;     // 0=中心点 1=框边缘
     double cone_thresh      = 0.80;  // 锥桶置信度阈值
-    int    cone_hold_frames  = 30;    // 锥桶消失后保持变形的帧数
+    int    cone_hold_frames  = 15;    // 锥桶消失后保持变形的帧数
+    double cone_return_gain  = 1.0;   // 回弹力度 (倍乘, 1.0=等同于避让力度)
+    int    cone_return_frames = 30;    // 回弹持续帧数
 
     double brake_scale     = 10;    // 速度(pps) → 刹车占空比(ns) 缩放系数
     int    brake_max       = 10000; // 最大刹车占空比 (ns)
+
+    double curve_slope     = 0.4;   // 弯道减速斜率 (越大越猛)
+    double curve_min       = 0.8;   // 弯道最低速度倍率
+
+    int    lidar_thresh      = 300;   // 激光障碍判定距离阈值 (mm), 必须<此值才触发
+    int    lidar_pre         = 800;   // 预触发去抖窗口, <此值开始攒帧
+    int    lidar_near        = 50;    // row=lt_h-1 对应的物理距离 (mm)
+    int    lidar_far         = 1200;  // row=10 对应的物理距离 (mm)
+    int    lidar_near_start  = 1;     // near_valid 检测起始偏移
+    int    lidar_near_end    = 4;     // near_valid 检测终止偏移
+    int    lidar_far_span    = 6;     // far_lost 检测跨度
+    int    lidar_min_frames  = 3;     // 连续确认帧数
+    double lidar_avoid_gain  = 0.4;   // 绕行中线推离力度 (归一化)
+    int    lidar_avoid_range = 30;    // 绕行斜坡陡峭度 (行数)
+    double lidar_speed       = 0.4;   // 绕行时速度倍率
+    int    lidar_hold_frames = 30;    // 消失后变形保持帧数
+    int    lidar_enable      = 1;     // 激光避障总开关
 };
 extern CfgCache g_cfg;
 

lib/image_cv.h

@@ -12,5 +12,7 @@ extern cv::Mat track;
 extern std::vector<int> left_line;
 extern std::vector<int> right_line;
 extern std::vector<int> mid_line;
+extern std::vector<int> mid_line_raw;
+extern int g_lost_rows;
 
 extern int line_tracking_height, line_tracking_width;

lib/vl53l0x.h

@@ -2,21 +2,27 @@
 #define VL53L0X_H
 
 #include <cstdint>
-#include "vl53l0x_def.h"
+
+extern "C" {
+#include "vl53l0x_platform.h"
+}
 
 class VL53L0X
 {
 public:
-    VL53L0X();
+	VL53L0X();
-    ~VL53L0X();
+	~VL53L0X();
 
-    bool init();
+	bool init();
-    bool readRange(VL53L0X_RangingMeasurementData_t &data);
+	bool startMeasure();
-    bool stop();
+	bool readResult(VL53L0X_RangingMeasurementData_t &data);
-    bool isOpen() const { return fd > 0; }
+	bool readRange(VL53L0X_RangingMeasurementData_t &data);
+	bool stop();
 
 private:
-    int fd;
+	int fd;
+	bool initialized;
+	vl53l0x_dev_t dev;
 };
 
 #endif

lib/vl53l0x/vl53l0x_api_calibration.h

@@ -0,0 +1,85 @@
+/*******************************************************************************
+ * Copyright © 2016, STMicroelectronics International N.V.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+	* Redistributions of source code must retain the above copyright
+	  notice, this list of conditions and the following disclaimer.
+	* Redistributions in binary form must reproduce the above copyright
+	  notice, this list of conditions and the following disclaimer in the
+	  documentation and/or other materials provided with the distribution.
+	* Neither the name of STMicroelectronics nor the
+	  names of its contributors may be used to endorse or promote products
+	  derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
+NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
+IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*******************************************************************************/
+
+#ifndef _VL53L0X_API_CALIBRATION_H_
+#define _VL53L0X_API_CALIBRATION_H_
+
+#include "vl53l0x_def.h"
+#include "vl53l0x_platform.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+VL53L0X_Error VL53L0X_perform_xtalk_calibration(VL53L0X_DEV Dev,
+		FixPoint1616_t XTalkCalDistance,
+		FixPoint1616_t *pXTalkCompensationRateMegaCps);
+
+VL53L0X_Error VL53L0X_perform_offset_calibration(VL53L0X_DEV Dev,
+		FixPoint1616_t CalDistanceMilliMeter,
+		int32_t *pOffsetMicroMeter);
+
+VL53L0X_Error VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
+		int32_t OffsetCalibrationDataMicroMeter);
+
+VL53L0X_Error VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
+		int32_t *pOffsetCalibrationDataMicroMeter);
+
+VL53L0X_Error VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev);
+
+VL53L0X_Error VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev,
+		uint32_t *refSpadCount, uint8_t *isApertureSpads);
+
+VL53L0X_Error VL53L0X_set_reference_spads(VL53L0X_DEV Dev,
+		uint32_t count, uint8_t isApertureSpads);
+
+VL53L0X_Error VL53L0X_get_reference_spads(VL53L0X_DEV Dev,
+		uint32_t *pSpadCount, uint8_t *pIsApertureSpads);
+
+VL53L0X_Error VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev,
+	uint8_t *pPhaseCal, const uint8_t get_data_enable,
+	const uint8_t restore_config);
+
+VL53L0X_Error VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev,
+	uint8_t *pVhvSettings, uint8_t *pPhaseCal, uint8_t get_data_enable);
+
+VL53L0X_Error VL53L0X_set_ref_calibration(VL53L0X_DEV Dev,
+		uint8_t VhvSettings, uint8_t PhaseCal);
+
+VL53L0X_Error VL53L0X_get_ref_calibration(VL53L0X_DEV Dev,
+		uint8_t *pVhvSettings, uint8_t *pPhaseCal);
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _VL53L0X_API_CALIBRATION_H_ */

lib/vl53l0x/vl53l0x_api_core.h

@@ -0,0 +1,113 @@
+/*******************************************************************************
+ * Copyright © 2016, STMicroelectronics International N.V.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of STMicroelectronics nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
+NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
+IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*******************************************************************************/
+
+#ifndef _VL53L0X_API_CORE_H_
+#define _VL53L0X_API_CORE_H_
+
+#include "vl53l0x_def.h"
+#include "vl53l0x_platform.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+VL53L0X_Error VL53L0X_reverse_bytes(uint8_t *data, uint32_t size);
+
+VL53L0X_Error VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev);
+
+uint8_t VL53L0X_encode_vcsel_period(uint8_t vcsel_period_pclks);
+
+uint8_t VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg);
+
+uint32_t VL53L0X_isqrt(uint32_t num);
+
+uint32_t VL53L0X_quadrature_sum(uint32_t a, uint32_t b);
+
+VL53L0X_Error VL53L0X_get_info_from_device(VL53L0X_DEV Dev, uint8_t option);
+
+VL53L0X_Error VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV Dev,
+	VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK);
+
+VL53L0X_Error VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV Dev,
+	VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK);
+
+uint32_t VL53L0X_decode_timeout(uint16_t encoded_timeout);
+
+VL53L0X_Error get_sequence_step_timeout(VL53L0X_DEV Dev,
+			VL53L0X_SequenceStepId SequenceStepId,
+			uint32_t *pTimeOutMicroSecs);
+
+VL53L0X_Error set_sequence_step_timeout(VL53L0X_DEV Dev,
+			VL53L0X_SequenceStepId SequenceStepId,
+			uint32_t TimeOutMicroSecs);
+
+VL53L0X_Error VL53L0X_set_measurement_timing_budget_micro_seconds(
+	VL53L0X_DEV Dev,
+	uint32_t MeasurementTimingBudgetMicroSeconds);
+
+VL53L0X_Error VL53L0X_get_measurement_timing_budget_micro_seconds(
+		VL53L0X_DEV Dev,
+		uint32_t *pMeasurementTimingBudgetMicroSeconds);
+
+VL53L0X_Error VL53L0X_load_tuning_settings(VL53L0X_DEV Dev,
+		uint8_t *pTuningSettingBuffer);
+
+VL53L0X_Error VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev,
+		VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
+		FixPoint1616_t *pSigmaEstimate);
+
+VL53L0X_Error VL53L0X_calc_dmax(
+	VL53L0X_DEV Dev, FixPoint1616_t ambRateMeas, uint32_t *pdmax_mm);
+
+VL53L0X_Error VL53L0X_get_total_xtalk_rate(VL53L0X_DEV Dev,
+	VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
+	FixPoint1616_t *ptotal_xtalk_rate_mcps);
+
+VL53L0X_Error VL53L0X_get_total_signal_rate(VL53L0X_DEV Dev,
+	VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
+	FixPoint1616_t *ptotal_signal_rate_mcps);
+
+VL53L0X_Error VL53L0X_get_pal_range_status(VL53L0X_DEV Dev,
+		 uint8_t DeviceRangeStatus,
+		 FixPoint1616_t SignalRate,
+		 uint16_t EffectiveSpadRtnCount,
+		 VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
+		 uint8_t *pPalRangeStatus);
+
+uint32_t VL53L0X_calc_timeout_mclks(VL53L0X_DEV Dev,
+	uint32_t timeout_period_us, uint8_t vcsel_period_pclks);
+
+uint16_t VL53L0X_encode_timeout(uint32_t timeout_macro_clks);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _VL53L0X_API_CORE_H_ */

lib/vl53l0x/vl53l0x_api_ranging.c

@@ -0,0 +1,42 @@
+/*******************************************************************************
+ * Copyright © 2016, STMicroelectronics International N.V.
+ All rights reserved.
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of STMicroelectronics nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
+ NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
+ IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
+ DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ ******************************************************************************/
+
+#include "vl53l0x_api.h"
+#include "vl53l0x_api_core.h"
+
+
+#ifndef __KERNEL__
+#include <stdlib.h>
+#endif
+#define LOG_FUNCTION_START(fmt, ...) \
+	_LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__)
+#define LOG_FUNCTION_END(status, ...) \
+	_LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__)
+#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
+	_LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__)
+

lib/vl53l0x/vl53l0x_api_ranging.h

@@ -0,0 +1,47 @@
+/*******************************************************************************
+ * Copyright © 2016, STMicroelectronics International N.V.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of STMicroelectronics nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
+NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
+IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*******************************************************************************/
+
+#ifndef _VL53L0X_API_RANGING_H_
+#define _VL53L0X_API_RANGING_H_
+
+#include "vl53l0x_def.h"
+#include "vl53l0x_platform.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _VL53L0X_API_RANGING_H_ */

lib/vl53l0x/vl53l0x_api_strings.h

@@ -0,0 +1,21 @@
+#ifndef VL53L0X_API_STRINGS_H_
+#define VL53L0X_API_STRINGS_H_
+
+#include "vl53l0x_def.h"
+
+#ifndef VL53L0X_API
+#define VL53L0X_API
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+VL53L0X_API VL53L0X_Error VL53L0X_GetStatusErrorString(VL53L0X_Error Status, char *pErrorString);
+VL53L0X_API VL53L0X_Error VL53L0X_GetStatusString(VL53L0X_Error Status, char *pStatusString);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

lib/vl53l0x/vl53l0x_def.h

@@ -0,0 +1,663 @@
+/*******************************************************************************
+ * Copyright © 2016, STMicroelectronics International N.V.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+	* Redistributions of source code must retain the above copyright
+	  notice, this list of conditions and the following disclaimer.
+	* Redistributions in binary form must reproduce the above copyright
+	  notice, this list of conditions and the following disclaimer in the
+	  documentation and/or other materials provided with the distribution.
+	* Neither the name of STMicroelectronics nor the
+	  names of its contributors may be used to endorse or promote products
+	  derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
+NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
+IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*******************************************************************************/
+
+/**
+ * @file VL53L0X_def.h
+ *
+ * @brief Type definitions for VL53L0X API.
+ *
+ */
+
+
+#ifndef _VL53L0X_DEF_H_
+#define _VL53L0X_DEF_H_
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup VL53L0X_globaldefine_group VL53L0X Defines
+ *	@brief	  VL53L0X Defines
+ *	@{
+ */
+
+
+/** PAL SPECIFICATION major version */
+#define VL53L0X10_SPECIFICATION_VER_MAJOR   1
+/** PAL SPECIFICATION minor version */
+#define VL53L0X10_SPECIFICATION_VER_MINOR   2
+/** PAL SPECIFICATION sub version */
+#define VL53L0X10_SPECIFICATION_VER_SUB	   7
+/** PAL SPECIFICATION sub version */
+#define VL53L0X10_SPECIFICATION_VER_REVISION 1440
+
+/** VL53L0X PAL IMPLEMENTATION major version */
+#define VL53L0X10_IMPLEMENTATION_VER_MAJOR	1
+/** VL53L0X PAL IMPLEMENTATION minor version */
+#define VL53L0X10_IMPLEMENTATION_VER_MINOR	0
+/** VL53L0X PAL IMPLEMENTATION sub version */
+#define VL53L0X10_IMPLEMENTATION_VER_SUB		9
+/** VL53L0X PAL IMPLEMENTATION sub version */
+#define VL53L0X10_IMPLEMENTATION_VER_REVISION	3673
+
+/** PAL SPECIFICATION major version */
+#define VL53L0X_SPECIFICATION_VER_MAJOR	 1
+/** PAL SPECIFICATION minor version */
+#define VL53L0X_SPECIFICATION_VER_MINOR	 2
+/** PAL SPECIFICATION sub version */
+#define VL53L0X_SPECIFICATION_VER_SUB	 7
+/** PAL SPECIFICATION sub version */
+#define VL53L0X_SPECIFICATION_VER_REVISION 1440
+
+/** VL53L0X PAL IMPLEMENTATION major version */
+#define VL53L0X_IMPLEMENTATION_VER_MAJOR	  1
+/** VL53L0X PAL IMPLEMENTATION minor version */
+#define VL53L0X_IMPLEMENTATION_VER_MINOR	  0
+/** VL53L0X PAL IMPLEMENTATION sub version */
+#define VL53L0X_IMPLEMENTATION_VER_SUB	  4
+/** VL53L0X PAL IMPLEMENTATION sub version */
+#define VL53L0X_IMPLEMENTATION_VER_REVISION	  4960
+#define VL53L0X_DEFAULT_MAX_LOOP 2000
+#define VL53L0X_MAX_STRING_LENGTH 32
+
+
+#include "vl53l0x_device.h"
+#include "vl53l0x_types.h"
+
+
+/****************************************
+ * PRIVATE define do not edit
+ ****************************************/
+
+/** @brief Defines the parameters of the Get Version Functions
+ */
+typedef struct {
+	uint32_t	 revision; /*!< revision number */
+	uint8_t		 major;	   /*!< major number */
+	uint8_t		 minor;	   /*!< minor number */
+	uint8_t		 build;	   /*!< build number */
+} VL53L0X_Version_t;
+
+
+/** @brief Defines the parameters of the Get Device Info Functions
+ */
+typedef struct {
+	char Name[VL53L0X_MAX_STRING_LENGTH];
+		/*!< Name of the Device e.g. Left_Distance */
+	char Type[VL53L0X_MAX_STRING_LENGTH];
+		/*!< Type of the Device e.g VL53L0X */
+	char ProductId[VL53L0X_MAX_STRING_LENGTH];
+		/*!< Product Identifier String	*/
+	uint8_t ProductType;
+		/*!< Product Type, VL53L0X = 1, VL53L1 = 2 */
+	uint8_t ProductRevisionMajor;
+		/*!< Product revision major */
+	uint8_t ProductRevisionMinor;
+		/*!< Product revision minor */
+} VL53L0X_DeviceInfo_t;
+
+
+/** @defgroup VL53L0X_define_Error_group Error and Warning code returned by API
+ *	The following DEFINE are used to identify the PAL ERROR
+ *	@{
+ */
+
+typedef int8_t VL53L0X_Error;
+
+#define VL53L0X_ERROR_NONE			   ((VL53L0X_Error)	0)
+#define VL53L0X_ERROR_CALIBRATION_WARNING	   ((VL53L0X_Error) - 1)
+	/*!< Warning invalid calibration data may be in used
+	 *	\a	VL53L0X_InitData()
+	 *	\a VL53L0X_GetOffsetCalibrationData
+	 *	\a VL53L0X_SetOffsetCalibrationData
+	 */
+#define VL53L0X_ERROR_MIN_CLIPPED		   ((VL53L0X_Error) - 2)
+	/*!< Warning parameter passed was clipped to min before to be applied */
+
+#define VL53L0X_ERROR_UNDEFINED			   ((VL53L0X_Error) - 3)
+	/*!< Unqualified error */
+#define VL53L0X_ERROR_INVALID_PARAMS		   ((VL53L0X_Error) - 4)
+	/*!< Parameter passed is invalid or out of range */
+#define VL53L0X_ERROR_NOT_SUPPORTED		   ((VL53L0X_Error) - 5)
+	/*!< Function is not supported in current mode or configuration */
+#define VL53L0X_ERROR_RANGE_ERROR		   ((VL53L0X_Error) - 6)
+	/*!< Device report a ranging error interrupt status */
+#define VL53L0X_ERROR_TIME_OUT			   ((VL53L0X_Error) - 7)
+	/*!< Aborted due to time out */
+#define VL53L0X_ERROR_MODE_NOT_SUPPORTED		   ((VL53L0X_Error) - 8)
+	/*!< Asked mode is not supported by the device */
+#define VL53L0X_ERROR_BUFFER_TOO_SMALL		   ((VL53L0X_Error) - 9)
+	/*!< ... */
+#define VL53L0X_ERROR_GPIO_NOT_EXISTING		   ((VL53L0X_Error) - 10)
+	/*!< User tried to setup a non-existing GPIO pin */
+#define VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED  ((VL53L0X_Error) - 11)
+	/*!< unsupported GPIO functionality */
+#define VL53L0X_ERROR_INTERRUPT_NOT_CLEARED		((VL53L0X_Error) - 12)
+	/*!< Error during interrupt clear */
+#define VL53L0X_ERROR_CONTROL_INTERFACE		   ((VL53L0X_Error) - 20)
+	/*!< error reported from IO functions */
+#define VL53L0X_ERROR_INVALID_COMMAND		   ((VL53L0X_Error) - 30)
+	/*!< The command is not allowed in the current device state
+	 *	(power down)
+	 */
+#define VL53L0X_ERROR_DIVISION_BY_ZERO		   ((VL53L0X_Error) - 40)
+	/*!< In the function a division by zero occurs */
+#define VL53L0X_ERROR_REF_SPAD_INIT		   ((VL53L0X_Error) - 50)
+	/*!< Error during reference SPAD initialization */
+#define VL53L0X_ERROR_NOT_IMPLEMENTED		   ((VL53L0X_Error) - 99)
+	/*!< Tells requested functionality has not been implemented yet or
+	 * not compatible with the device
+	 */
+/** @} VL53L0X_define_Error_group */
+
+
+/** @defgroup VL53L0X_define_DeviceModes_group Defines Device modes
+ *	Defines all possible modes for the device
+ *	@{
+ */
+typedef uint8_t VL53L0X_DeviceModes;
+
+#define VL53L0X_DEVICEMODE_SINGLE_RANGING	   ((VL53L0X_DeviceModes)  0)
+#define VL53L0X_DEVICEMODE_CONTINUOUS_RANGING	   ((VL53L0X_DeviceModes)  1)
+#define VL53L0X_DEVICEMODE_SINGLE_HISTOGRAM	   ((VL53L0X_DeviceModes)  2)
+#define VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING ((VL53L0X_DeviceModes)  3)
+#define VL53L0X_DEVICEMODE_SINGLE_ALS		   ((VL53L0X_DeviceModes) 10)
+#define VL53L0X_DEVICEMODE_GPIO_DRIVE		   ((VL53L0X_DeviceModes) 20)
+#define VL53L0X_DEVICEMODE_GPIO_OSC		   ((VL53L0X_DeviceModes) 21)
+	/* ... Modes to be added depending on device */
+/** @} VL53L0X_define_DeviceModes_group */
+
+
+
+/** @defgroup VL53L0X_define_HistogramModes_group Defines Histogram modes
+ *	Defines all possible Histogram modes for the device
+ *	@{
+ */
+typedef uint8_t VL53L0X_HistogramModes;
+
+#define VL53L0X_HISTOGRAMMODE_DISABLED		((VL53L0X_HistogramModes) 0)
+	/*!< Histogram Disabled */
+#define VL53L0X_HISTOGRAMMODE_REFERENCE_ONLY	((VL53L0X_HistogramModes) 1)
+	/*!< Histogram Reference array only */
+#define VL53L0X_HISTOGRAMMODE_RETURN_ONLY	((VL53L0X_HistogramModes) 2)
+	/*!< Histogram Return array only */
+#define VL53L0X_HISTOGRAMMODE_BOTH		 ((VL53L0X_HistogramModes) 3)
+	/*!< Histogram both Reference and Return Arrays */
+	/* ... Modes to be added depending on device */
+/** @} VL53L0X_define_HistogramModes_group */
+
+
+/** @defgroup VL53L0X_define_PowerModes_group List of available Power Modes
+ *	List of available Power Modes
+ *	@{
+ */
+
+typedef uint8_t VL53L0X_PowerModes;
+
+#define VL53L0X_POWERMODE_STANDBY_LEVEL1 ((VL53L0X_PowerModes) 0)
+	/*!< Standby level 1 */
+#define VL53L0X_POWERMODE_STANDBY_LEVEL2 ((VL53L0X_PowerModes) 1)
+	/*!< Standby level 2 */
+#define VL53L0X_POWERMODE_IDLE_LEVEL1	((VL53L0X_PowerModes) 2)
+	/*!< Idle level 1 */
+#define VL53L0X_POWERMODE_IDLE_LEVEL2	((VL53L0X_PowerModes) 3)
+	/*!< Idle level 2 */
+
+/** @} VL53L0X_define_PowerModes_group */
+
+
+#define VL53L0X_DMAX_LUT_SIZE 7
+	/*!< Defines the number of items in the DMAX lookup table */
+
+/** @brief Structure defining data pair that makes up the DMAX Lookup table.
+ */
+typedef struct {
+	FixPoint1616_t ambRate_mcps[VL53L0X_DMAX_LUT_SIZE];
+		/*!< Ambient rate (mcps) */
+	FixPoint1616_t dmax_mm[VL53L0X_DMAX_LUT_SIZE];
+		/*!< DMAX Value (mm)  */
+} VL53L0X_DMaxLUT_t;
+
+/** @brief Defines all parameters for the device
+ */
+typedef struct {
+	VL53L0X_DeviceModes DeviceMode;
+	/*!< Defines type of measurement to be done for the next measure */
+	VL53L0X_HistogramModes HistogramMode;
+	/*!< Defines type of histogram measurement to be done for the next
+	 *	measure
+	 */
+	uint32_t MeasurementTimingBudgetMicroSeconds;
+	/*!< Defines the allowed total time for a single measurement */
+	uint32_t InterMeasurementPeriodMilliSeconds;
+	/*!< Defines time between two consecutive measurements (between two
+	 *	measurement starts). If set to 0 means back-to-back mode
+	 */
+	uint8_t XTalkCompensationEnable;
+	/*!< Tells if Crosstalk compensation shall be enable or not	 */
+	uint16_t XTalkCompensationRangeMilliMeter;
+	/*!< CrossTalk compensation range in millimeter	 */
+	FixPoint1616_t XTalkCompensationRateMegaCps;
+	/*!< CrossTalk compensation rate in Mega counts per seconds.
+	 *	Expressed in 16.16 fixed point format.
+	 */
+	int32_t RangeOffsetMicroMeters;
+	/*!< Range offset adjustment (mm).	*/
+
+	uint8_t LimitChecksEnable[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
+	/*!< This Array store all the Limit Check enable for this device. */
+	uint8_t LimitChecksStatus[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
+	/*!< This Array store all the Status of the check linked to last
+	 * measurement.
+	 */
+	FixPoint1616_t LimitChecksValue[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
+	/*!< This Array store all the Limit Check value for this device */
+
+	VL53L0X_DMaxLUT_t dmax_lut;
+	/*!< Lookup table defining ambient rates and associated
+	 * dmax values.
+	 */
+
+	uint8_t WrapAroundCheckEnable;
+	/*!< Tells if Wrap Around Check shall be enable or not */
+} VL53L0X_DeviceParameters_t;
+
+
+/** @defgroup VL53L0X_define_State_group Defines the current status
+ * of the device
+ *	Defines the current status of the device
+ *	@{
+ */
+
+typedef uint8_t VL53L0X_State;
+
+#define VL53L0X_STATE_POWERDOWN		 ((VL53L0X_State)  0)
+	/*!< Device is in HW reset	*/
+#define VL53L0X_STATE_WAIT_STATICINIT ((VL53L0X_State)  1)
+	/*!< Device is initialized and wait for static initialization  */
+#define VL53L0X_STATE_STANDBY		 ((VL53L0X_State)  2)
+	/*!< Device is in Low power Standby mode   */
+#define VL53L0X_STATE_IDLE			 ((VL53L0X_State)  3)
+	/*!< Device has been initialized and ready to do measurements  */
+#define VL53L0X_STATE_RUNNING		 ((VL53L0X_State)  4)
+	/*!< Device is performing measurement */
+#define VL53L0X_STATE_UNKNOWN		 ((VL53L0X_State)  98)
+	/*!< Device is in unknown state and need to be rebooted	 */
+#define VL53L0X_STATE_ERROR			 ((VL53L0X_State)  99)
+	/*!< Device is in error state and need to be rebooted  */
+
+/** @} VL53L0X_define_State_group */
+
+
+/**
+ * @struct VL53L0X_RangeData_t
+ * @brief Range measurement data.
+ */
+typedef struct {
+	uint32_t TimeStamp;			  /*!< 32-bit time stamp. */
+	uint32_t MeasurementTimeUsec;
+		/*!< Give the Measurement time needed by the device to do the
+		 * measurement.
+		 */
+
+
+	uint16_t RangeMilliMeter;	  /*!< range distance in millimeter. */
+
+	uint16_t RangeDMaxMilliMeter;
+		/*!< Tells what is the maximum detection distance of the device
+		 * in current setup and environment conditions (Filled when
+		 *	applicable)
+		 */
+
+	FixPoint1616_t SignalRateRtnMegaCps;
+		/*!< Return signal rate (MCPS)\n these is a 16.16 fix point
+		 *	value, which is effectively a measure of target
+		 *	 reflectance.
+		 */
+	FixPoint1616_t AmbientRateRtnMegaCps;
+		/*!< Return ambient rate (MCPS)\n these is a 16.16 fix point
+		 *	value, which is effectively a measure of the ambien
+		 *	t light.
+		 */
+
+	uint16_t EffectiveSpadRtnCount;
+		/*!< Return the effective SPAD count for the return signal.
+		 *	To obtain Real value it should be divided by 256
+		 */
+
+	uint8_t ZoneId;
+		/*!< Denotes which zone and range scheduler stage the range
+		 *	data relates to.
+		 */
+	uint8_t RangeFractionalPart;
+		/*!< Fractional part of range distance. Final value is a
+		 *	FixPoint168 value.
+		 */
+	uint8_t RangeStatus;
+		/*!< Range Status for the current measurement. This is device
+		 *	dependent. Value = 0 means value is valid.
+		 *	See \ref RangeStatusPage
+		 */
+} VL53L0X_RangingMeasurementData_t;
+
+
+#define VL53L0X_HISTOGRAM_BUFFER_SIZE 24
+
+/**
+ * @struct VL53L0X_HistogramData_t
+ * @brief Histogram measurement data.
+ */
+typedef struct {
+	/* Histogram Measurement data */
+	uint32_t HistogramData[VL53L0X_HISTOGRAM_BUFFER_SIZE];
+	/*!< Histogram data */
+	/*!< Indicate the types of histogram data :
+	 *Return only, Reference only, both Return and Reference
+	 */
+	uint8_t FirstBin; /*!< First Bin value */
+	uint8_t BufferSize; /*!< Buffer Size - Set by the user.*/
+	uint8_t NumberOfBins;
+	/*!< Number of bins filled by the histogram measurement */
+
+	VL53L0X_DeviceError ErrorStatus;
+	/*!< Error status of the current measurement. \n
+	 * see @a ::VL53L0X_DeviceError @a VL53L0X_GetStatusErrorString()
+	 */
+} VL53L0X_HistogramMeasurementData_t;
+
+#define VL53L0X_REF_SPAD_BUFFER_SIZE 6
+
+/**
+ * @struct VL53L0X_SpadData_t
+ * @brief Spad Configuration Data.
+ */
+typedef struct {
+	uint8_t RefSpadEnables[VL53L0X_REF_SPAD_BUFFER_SIZE];
+	/*!< Reference Spad Enables */
+	uint8_t RefGoodSpadMap[VL53L0X_REF_SPAD_BUFFER_SIZE];
+	/*!< Reference Spad Good Spad Map */
+} VL53L0X_SpadData_t;
+
+typedef struct {
+	FixPoint1616_t OscFrequencyMHz; /* Frequency used */
+
+	uint16_t LastEncodedTimeout;
+	/* last encoded Time out used for timing budget*/
+
+	VL53L0X_GpioFunctionality Pin0GpioFunctionality;
+	/* store the functionality of the GPIO: pin0 */
+
+	uint32_t FinalRangeTimeoutMicroSecs;
+	 /*!< Execution time of the final range*/
+	uint8_t FinalRangeVcselPulsePeriod;
+	 /*!< Vcsel pulse period (pll clocks) for the final range measurement*/
+	uint32_t PreRangeTimeoutMicroSecs;
+	 /*!< Execution time of the final range*/
+	uint8_t PreRangeVcselPulsePeriod;
+	 /*!< Vcsel pulse period (pll clocks) for the pre-range measurement*/
+
+	uint16_t SigmaEstRefArray;
+	 /*!< Reference array sigma value in 1/100th of [mm] e.g. 100 = 1mm */
+	uint16_t SigmaEstEffPulseWidth;
+	 /*!< Effective Pulse width for sigma estimate in 1/100th
+	  * of ns e.g. 900 = 9.0ns
+	  */
+	uint16_t SigmaEstEffAmbWidth;
+	 /*!< Effective Ambient width for sigma estimate in 1/100th of ns
+	  * e.g. 500 = 5.0ns
+	  */
+
+
+	/* Indicate if read from device has been done (==1) or not (==0) */
+	uint8_t ReadDataFromDeviceDone;
+	uint8_t ModuleId; /* Module ID */
+	uint8_t Revision; /* test Revision */
+	char ProductId[VL53L0X_MAX_STRING_LENGTH];
+		/* Product Identifier String  */
+	uint8_t ReferenceSpadCount; /* used for ref spad management */
+	uint8_t ReferenceSpadType;	/* used for ref spad management */
+	uint8_t RefSpadsInitialised; /* reports if ref spads are initialised. */
+	uint32_t PartUIDUpper; /*!< Unique Part ID Upper */
+	uint32_t PartUIDLower; /*!< Unique Part ID Lower */
+	/*!< Peek Signal rate at 400 mm*/
+	FixPoint1616_t SignalRateMeasFixed400mm;
+
+} VL53L0X_DeviceSpecificParameters_t;
+
+/**
+ * @struct VL53L0X_DevData_t
+ *
+ * @brief VL53L0X PAL device ST private data structure \n
+ * End user should never access any of these field directly
+ *
+ * These must never access directly but only via macro
+ */
+typedef struct {
+	int32_t	 Part2PartOffsetNVMMicroMeter;
+	/*!< backed up NVM value */
+	int32_t	 Part2PartOffsetAdjustmentNVMMicroMeter;
+	/*!< backed up NVM value representing additional offset adjustment */
+	VL53L0X_DeviceParameters_t CurrentParameters;
+	/*!< Current Device Parameter */
+	VL53L0X_RangingMeasurementData_t LastRangeMeasure;
+	/*!< Ranging Data */
+	VL53L0X_HistogramMeasurementData_t LastHistogramMeasure;
+	/*!< Histogram Data */
+	VL53L0X_DeviceSpecificParameters_t DeviceSpecificParameters;
+	/*!< Parameters specific to the device */
+	VL53L0X_SpadData_t SpadData;
+	/*!< Spad Data */
+	uint8_t SequenceConfig;
+	/*!< Internal value for the sequence config */
+	uint8_t RangeFractionalEnable;
+	/*!< Enable/Disable fractional part of ranging data */
+	VL53L0X_State PalState;
+	/*!< Current state of the PAL for this device */
+	VL53L0X_PowerModes PowerMode;
+	/*!< Current Power Mode	 */
+	uint16_t SigmaEstRefArray;
+	/*!< Reference array sigma value in 1/100th of [mm] e.g. 100 = 1mm */
+	uint16_t SigmaEstEffPulseWidth;
+	/*!< Effective Pulse width for sigma estimate in 1/100th
+	 * of ns e.g. 900 = 9.0ns
+	 */
+	uint16_t SigmaEstEffAmbWidth;
+	/*!< Effective Ambient width for sigma estimate in 1/100th of ns
+	 * e.g. 500 = 5.0ns
+	 */
+	uint8_t StopVariable;
+	/*!< StopVariable used during the stop sequence */
+	uint16_t targetRefRate;
+	/*!< Target Ambient Rate for Ref spad management */
+	FixPoint1616_t SigmaEstimate;
+	/*!< Sigma Estimate - based on ambient & VCSEL rates and
+	 * signal_total_events
+	 */
+	FixPoint1616_t SignalEstimate;
+	/*!< Signal Estimate - based on ambient & VCSEL rates and cross talk */
+	FixPoint1616_t LastSignalRefMcps;
+	/*!< Latest Signal ref in Mcps */
+	uint8_t *pTuningSettingsPointer;
+	/*!< Pointer for Tuning Settings table */
+	uint8_t UseInternalTuningSettings;
+	/*!< Indicate if we use	 Tuning Settings table */
+	uint16_t LinearityCorrectiveGain;
+	/*!< Linearity Corrective Gain value in x1000 */
+} VL53L0X_DevData_t;
+
+
+/** @defgroup VL53L0X_define_InterruptPolarity_group Defines the Polarity
+ * of the Interrupt
+ *	Defines the Polarity of the Interrupt
+ *	@{
+ */
+typedef uint8_t VL53L0X_InterruptPolarity;
+
+#define VL53L0X_INTERRUPTPOLARITY_LOW	   ((VL53L0X_InterruptPolarity)	0)
+/*!< Set active low polarity best setup for falling edge. */
+#define VL53L0X_INTERRUPTPOLARITY_HIGH	   ((VL53L0X_InterruptPolarity)	1)
+/*!< Set active high polarity best setup for rising edge. */
+
+/** @} VL53L0X_define_InterruptPolarity_group */
+
+
+/** @defgroup VL53L0X_define_VcselPeriod_group Vcsel Period Defines
+ *	Defines the range measurement for which to access the vcsel period.
+ *	@{
+ */
+typedef uint8_t VL53L0X_VcselPeriod;
+
+#define VL53L0X_VCSEL_PERIOD_PRE_RANGE	((VL53L0X_VcselPeriod) 0)
+/*!<Identifies the pre-range vcsel period. */
+#define VL53L0X_VCSEL_PERIOD_FINAL_RANGE ((VL53L0X_VcselPeriod) 1)
+/*!<Identifies the final range vcsel period. */
+
+/** @} VL53L0X_define_VcselPeriod_group */
+
+/** @defgroup VL53L0X_define_SchedulerSequence_group Defines the steps
+ * carried out by the scheduler during a range measurement.
+ *	@{
+ *	Defines the states of all the steps in the scheduler
+ *	i.e. enabled/disabled.
+ */
+typedef struct {
+	uint8_t		 TccOn;	   /*!<Reports if Target Centre Check On  */
+	uint8_t		 MsrcOn;	   /*!<Reports if MSRC On  */
+	uint8_t		 DssOn;		   /*!<Reports if DSS On  */
+	uint8_t		 PreRangeOn;   /*!<Reports if Pre-Range On	*/
+	uint8_t		 FinalRangeOn; /*!<Reports if Final-Range On  */
+} VL53L0X_SchedulerSequenceSteps_t;
+
+/** @} VL53L0X_define_SchedulerSequence_group */
+
+/** @defgroup VL53L0X_define_SequenceStepId_group Defines the Polarity
+ *	of the Interrupt
+ *	Defines the the sequence steps performed during ranging..
+ *	@{
+ */
+typedef uint8_t VL53L0X_SequenceStepId;
+
+#define	 VL53L0X_SEQUENCESTEP_TCC		 ((VL53L0X_VcselPeriod) 0)
+/*!<Target CentreCheck identifier. */
+#define	 VL53L0X_SEQUENCESTEP_DSS		 ((VL53L0X_VcselPeriod) 1)
+/*!<Dynamic Spad Selection function Identifier. */
+#define	 VL53L0X_SEQUENCESTEP_MSRC		 ((VL53L0X_VcselPeriod) 2)
+/*!<Minimum Signal Rate Check function Identifier. */
+#define	 VL53L0X_SEQUENCESTEP_PRE_RANGE	 ((VL53L0X_VcselPeriod) 3)
+/*!<Pre-Range check Identifier. */
+#define	 VL53L0X_SEQUENCESTEP_FINAL_RANGE ((VL53L0X_VcselPeriod) 4)
+/*!<Final Range Check Identifier. */
+
+#define	 VL53L0X_SEQUENCESTEP_NUMBER_OF_CHECKS			 5
+/*!<Number of Sequence Step Managed by the API. */
+
+/** @} VL53L0X_define_SequenceStepId_group */
+
+
+/* MACRO Definitions */
+/** @defgroup VL53L0X_define_GeneralMacro_group General Macro Defines
+ *	General Macro Defines
+ *	@{
+ */
+
+/* Defines */
+#define VL53L0X_SETPARAMETERFIELD(Dev, field, value) \
+	PALDevDataSet(Dev, CurrentParameters.field, value)
+
+#define VL53L0X_GETPARAMETERFIELD(Dev, field, variable) \
+	(variable = ((PALDevDataGet(Dev, CurrentParameters)).field))
+
+
+#define VL53L0X_SETARRAYPARAMETERFIELD(Dev, field, index, value) \
+	PALDevDataSet(Dev, CurrentParameters.field[index], value)
+
+#define VL53L0X_GETARRAYPARAMETERFIELD(Dev, field, index, variable) \
+	(variable = (PALDevDataGet(Dev, CurrentParameters)).field[index])
+
+#define VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, field, value) \
+		PALDevDataSet(Dev, DeviceSpecificParameters.field, value)
+
+#define VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, field) \
+		PALDevDataGet(Dev, DeviceSpecificParameters).field
+
+
+#define VL53L0X_FIXPOINT1616TOFIXPOINT97(Value) \
+	(uint16_t)((Value>>9)&0xFFFF)
+#define VL53L0X_FIXPOINT97TOFIXPOINT1616(Value) \
+	(FixPoint1616_t)(Value<<9)
+
+#define VL53L0X_FIXPOINT1616TOFIXPOINT88(Value) \
+	(uint16_t)((Value>>8)&0xFFFF)
+#define VL53L0X_FIXPOINT88TOFIXPOINT1616(Value) \
+	(FixPoint1616_t)(Value<<8)
+
+#define VL53L0X_FIXPOINT1616TOFIXPOINT412(Value) \
+	(uint16_t)((Value>>4)&0xFFFF)
+#define VL53L0X_FIXPOINT412TOFIXPOINT1616(Value) \
+	(FixPoint1616_t)(Value<<4)
+
+#define VL53L0X_FIXPOINT1616TOFIXPOINT313(Value) \
+	(uint16_t)((Value>>3)&0xFFFF)
+#define VL53L0X_FIXPOINT313TOFIXPOINT1616(Value) \
+	(FixPoint1616_t)(Value<<3)
+
+#define VL53L0X_FIXPOINT1616TOFIXPOINT08(Value) \
+	(uint8_t)((Value>>8)&0x00FF)
+#define VL53L0X_FIXPOINT08TOFIXPOINT1616(Value) \
+	(FixPoint1616_t)(Value<<8)
+
+#define VL53L0X_FIXPOINT1616TOFIXPOINT53(Value) \
+	(uint8_t)((Value>>13)&0x00FF)
+#define VL53L0X_FIXPOINT53TOFIXPOINT1616(Value) \
+	(FixPoint1616_t)(Value<<13)
+
+#define VL53L0X_FIXPOINT1616TOFIXPOINT102(Value) \
+	(uint16_t)((Value>>14)&0x0FFF)
+#define VL53L0X_FIXPOINT102TOFIXPOINT1616(Value) \
+	(FixPoint1616_t)(Value<<12)
+
+#define VL53L0X_MAKEUINT16(lsb, msb) (uint16_t)((((uint16_t)msb)<<8) + \
+		(uint16_t)lsb)
+
+/** @} VL53L0X_define_GeneralMacro_group */
+
+/** @} VL53L0X_globaldefine_group */
+
+
+
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif /* _VL53L0X_DEF_H_ */

lib/vl53l0x/vl53l0x_device.h

@@ -0,0 +1,262 @@
+/*******************************************************************************
+ * Copyright © 2016, STMicroelectronics International N.V.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of STMicroelectronics nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
+NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
+IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*******************************************************************************/
+
+/**
+ * Device specific defines. To be adapted by implementer for the targeted
+ * device.
+ */
+
+#ifndef _VL53L0X_DEVICE_H_
+#define _VL53L0X_DEVICE_H_
+
+#include "vl53l0x_types.h"
+
+
+/** @defgroup VL53L0X_DevSpecDefines_group VL53L0X cut1.1 Device
+ * Specific Defines
+ *  @brief VL53L0X cut1.1 Device Specific Defines
+ *  @{
+ */
+
+
+/** @defgroup VL53L0X_DeviceError_group Device Error
+ *  @brief Device Error code
+ *
+ *  This enum is Device specific it should be updated in the implementation
+ *  Use @a VL53L0X_GetStatusErrorString() to get the string.
+ *  It is related to Status Register of the Device.
+ *  @{
+ */
+typedef uint8_t VL53L0X_DeviceError;
+
+#define VL53L0X_DEVICEERROR_NONE                       ((VL53L0X_DeviceError) 0)
+	/*!< 0  NoError  */
+#define VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ((VL53L0X_DeviceError) 1)
+#define VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE   ((VL53L0X_DeviceError) 2)
+#define VL53L0X_DEVICEERROR_NOVHVVALUEFOUND            ((VL53L0X_DeviceError) 3)
+#define VL53L0X_DEVICEERROR_MSRCNOTARGET               ((VL53L0X_DeviceError) 4)
+#define VL53L0X_DEVICEERROR_SNRCHECK                   ((VL53L0X_DeviceError) 5)
+#define VL53L0X_DEVICEERROR_RANGEPHASECHECK            ((VL53L0X_DeviceError) 6)
+#define VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK        ((VL53L0X_DeviceError) 7)
+#define VL53L0X_DEVICEERROR_TCC                        ((VL53L0X_DeviceError) 8)
+#define VL53L0X_DEVICEERROR_PHASECONSISTENCY           ((VL53L0X_DeviceError) 9)
+#define VL53L0X_DEVICEERROR_MINCLIP                   ((VL53L0X_DeviceError) 10)
+#define VL53L0X_DEVICEERROR_RANGECOMPLETE             ((VL53L0X_DeviceError) 11)
+#define VL53L0X_DEVICEERROR_ALGOUNDERFLOW             ((VL53L0X_DeviceError) 12)
+#define VL53L0X_DEVICEERROR_ALGOOVERFLOW              ((VL53L0X_DeviceError) 13)
+#define VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD      ((VL53L0X_DeviceError) 14)
+
+/** @} end of VL53L0X_DeviceError_group */
+
+
+/** @defgroup VL53L0X_CheckEnable_group Check Enable list
+ *  @brief Check Enable code
+ *
+ *  Define used to specify the LimitCheckId.
+ *  Use @a VL53L0X_GetLimitCheckInfo() to get the string.
+ *  @{
+ */
+
+#define VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE           0
+#define VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE     1
+#define VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP             2
+#define VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD      3
+#define VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC            4
+#define VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE       5
+
+#define VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS            6
+
+/** @}  end of VL53L0X_CheckEnable_group */
+
+
+/** @defgroup VL53L0X_GpioFunctionality_group Gpio Functionality
+ *  @brief Defines the different functionalities for the device GPIO(s)
+ *  @{
+ */
+typedef uint8_t VL53L0X_GpioFunctionality;
+
+#define VL53L0X_GPIOFUNCTIONALITY_OFF                     \
+	((VL53L0X_GpioFunctionality)  0) /*!< NO Interrupt  */
+#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW   \
+	((VL53L0X_GpioFunctionality)  1) /*!< Level Low (value < thresh_low)  */
+#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH   \
+	((VL53L0X_GpioFunctionality)  2) /*!< Level High (value>thresh_high) */
+#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT    \
+	((VL53L0X_GpioFunctionality)  3)
+	/*!< Out Of Window (value < thresh_low OR value > thresh_high)  */
+#define VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY        \
+	((VL53L0X_GpioFunctionality)  4) /*!< New Sample Ready  */
+
+/** @} end of VL53L0X_GpioFunctionality_group */
+
+
+/* Device register map */
+
+/** @defgroup VL53L0X_DefineRegisters_group Define Registers
+ *  @brief List of all the defined registers
+ *  @{
+ */
+#define VL53L0X_REG_SYSRANGE_START                        0x000
+	/** mask existing bit in #VL53L0X_REG_SYSRANGE_START*/
+	#define VL53L0X_REG_SYSRANGE_MODE_MASK          0x0F
+	/** bit 0 in #VL53L0X_REG_SYSRANGE_START write 1 toggle state in
+	 * continuous mode and arm next shot in single shot mode
+	 */
+	#define VL53L0X_REG_SYSRANGE_MODE_START_STOP    0x01
+	/** bit 1 write 0 in #VL53L0X_REG_SYSRANGE_START set single shot mode */
+	#define VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT    0x00
+	/** bit 1 write 1 in #VL53L0X_REG_SYSRANGE_START set back-to-back
+	 *  operation mode
+	 */
+	#define VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK    0x02
+	/** bit 2 write 1 in #VL53L0X_REG_SYSRANGE_START set timed operation
+	 *  mode
+	 */
+	#define VL53L0X_REG_SYSRANGE_MODE_TIMED         0x04
+	/** bit 3 write 1 in #VL53L0X_REG_SYSRANGE_START set histogram operation
+	 *  mode
+	 */
+	#define VL53L0X_REG_SYSRANGE_MODE_HISTOGRAM     0x08
+
+
+#define VL53L0X_REG_SYSTEM_THRESH_HIGH               0x000C
+#define VL53L0X_REG_SYSTEM_THRESH_LOW                0x000E
+
+
+#define VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG		0x0001
+#define VL53L0X_REG_SYSTEM_RANGE_CONFIG			0x0009
+#define VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD	0x0004
+
+
+#define VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO               0x000A
+	#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_DISABLED	0x00
+	#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_LOW	0x01
+	#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_HIGH	0x02
+	#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_OUT_OF_WINDOW	0x03
+	#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY	0x04
+
+#define VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH          0x0084
+
+
+#define VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR           0x000B
+
+/* Result registers */
+#define VL53L0X_REG_RESULT_INTERRUPT_STATUS          0x0013
+#define VL53L0X_REG_RESULT_RANGE_STATUS              0x0014
+
+#define VL53L0X_REG_RESULT_CORE_PAGE  1
+#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN   0x00BC
+#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_RTN    0x00C0
+#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF   0x00D0
+#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_REF    0x00D4
+#define VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF             0x00B6
+
+/* Algo register */
+
+#define VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM       0x0028
+
+#define VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS                0x008a
+
+/* Check Limit registers */
+#define VL53L0X_REG_MSRC_CONFIG_CONTROL                     0x0060
+
+#define VL53L0X_REG_PRE_RANGE_CONFIG_MIN_SNR                      0X0027
+#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW              0x0056
+#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH             0x0057
+#define VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT            0x0064
+
+#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_SNR                    0X0067
+#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW            0x0047
+#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH           0x0048
+#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT   0x0044
+
+
+#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_HI              0X0061
+#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_LO              0X0062
+
+/* PRE RANGE registers */
+#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD                 0x0050
+#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI            0x0051
+#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO            0x0052
+
+#define VL53L0X_REG_SYSTEM_HISTOGRAM_BIN                          0x0081
+#define VL53L0X_REG_HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT         0x0033
+#define VL53L0X_REG_HISTOGRAM_CONFIG_READOUT_CTRL                 0x0055
+
+#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD               0x0070
+#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI          0x0071
+#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO          0x0072
+#define VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS         0x0020
+
+#define VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP                    0x0046
+
+
+#define VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N	                 0x00bf
+#define VL53L0X_REG_IDENTIFICATION_MODEL_ID                       0x00c0
+#define VL53L0X_REG_IDENTIFICATION_REVISION_ID                    0x00c2
+
+#define VL53L0X_REG_OSC_CALIBRATE_VAL                             0x00f8
+
+
+#define VL53L0X_SIGMA_ESTIMATE_MAX_VALUE                          65535
+/* equivalent to a range sigma of 655.35mm */
+
+#define VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH          0x032
+#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0   0x0B0
+#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_1   0x0B1
+#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_2   0x0B2
+#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_3   0x0B3
+#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_4   0x0B4
+#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_5   0x0B5
+
+#define VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT   0xB6
+#define VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD 0x4E /* 0x14E */
+#define VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET    0x4F /* 0x14F */
+#define VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE    0x80
+
+/*
+ * Speed of light in um per 1E-10 Seconds
+ */
+
+#define VL53L0X_SPEED_OF_LIGHT_IN_AIR 2997
+
+#define VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV     0x0089
+
+#define VL53L0X_REG_ALGO_PHASECAL_LIM                         0x0030 /* 0x130 */
+#define VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT              0x0030
+
+/** @} VL53L0X_DefineRegisters_group */
+
+/** @} VL53L0X_DevSpecDefines_group */
+
+
+#endif
+
+/* _VL53L0X_DEVICE_H_ */
+
+

lib/vl53l0x/vl53l0x_i2c_platform.h

@@ -0,0 +1,402 @@
+/*
+ *  vl53l0x_i2c_platform.h - Linux kernel modules for STM VL53L0 FlightSense TOF
+ *							sensor
+ *
+ *  Copyright (C) 2016 STMicroelectronics Imaging Division.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+/**
+ * @file   VL53L0X_i2c_platform.h
+ * @brief  Function prototype definitions for EWOK Platform layer.
+ *
+ */
+
+
+#ifndef _VL53L0X_I2C_PLATFORM_H_
+#define _VL53L0X_I2C_PLATFORM_H_
+
+#include "vl53l0x_def.h"
+
+
+/** Maximum buffer size to be used in i2c */
+#define VL53L0X_MAX_I2C_XFER_SIZE   64
+
+/**
+ *  @brief Typedef defining .\n
+ * The developer should modify this to suit the platform being deployed.
+ *
+ */
+
+/**
+ * @brief Typedef defining 8 bit unsigned char type.\n
+ * The developer should modify this to suit the platform being deployed.
+ *
+ */
+
+#ifndef bool_t
+typedef unsigned char bool_t;
+#endif
+
+
+#define	   I2C                0x01
+#define	   SPI                0x00
+
+#define    COMMS_BUFFER_SIZE    64
+/*MUST be the same size as the SV task buffer */
+
+#define    BYTES_PER_WORD        2
+#define    BYTES_PER_DWORD       4
+
+#define    VL53L0X_MAX_STRING_LENGTH_PLT       256
+
+/**
+ * @brief  Initialise platform comms.
+ *
+ * @param  comms_type      - selects between I2C and SPI
+ * @param  comms_speed_khz - unsigned short containing the I2C speed in kHz
+ *
+ * @return status - status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_comms_initialise(uint8_t  comms_type,
+			uint16_t comms_speed_khz);
+
+/**
+ * @brief  Close platform comms.
+ *
+ * @return status - status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_comms_close(void);
+
+/**
+ * @brief  Cycle Power to Device
+ *
+ * @return status - status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_cycle_power(void);
+
+int32_t VL53L0X_set_page(VL53L0X_DEV dev, uint8_t page_data);
+
+/**
+ * @brief Writes the supplied byte buffer to the device
+ *
+ * Wrapper for SystemVerilog Write Multi task
+ *
+ * @code
+ *
+ * Example:
+ *
+ * uint8_t *spad_enables;
+ *
+ * int status = VL53L0X_write_multi(RET_SPAD_EN_0, spad_enables, 36);
+ *
+ * @endcode
+ *
+ * @param  address - uint8_t device address value
+ * @param  index - uint8_t register index value
+ * @param  pdata - pointer to uint8_t buffer containing the data to be written
+ * @param  count - number of bytes in the supplied byte buffer
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_write_multi(VL53L0X_DEV dev, uint8_t index, uint8_t  *pdata,
+		int32_t count);
+
+
+/**
+ * @brief  Reads the requested number of bytes from the device
+ *
+ * Wrapper for SystemVerilog Read Multi task
+ *
+ * @code
+ *
+ * Example:
+ *
+ * uint8_t buffer[COMMS_BUFFER_SIZE];
+ *
+ * int status = status  = VL53L0X_read_multi(DEVICE_ID, buffer, 2)
+ *
+ * @endcode
+ *
+ * @param  address - uint8_t device address value
+ * @param  index - uint8_t register index value
+ * @param  pdata - pointer to the uint8_t buffer to store read data
+ * @param  count - number of uint8_t's to read
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_read_multi(VL53L0X_DEV dev,  uint8_t index, uint8_t  *pdata,
+		int32_t count);
+
+
+/**
+ * @brief  Writes a single byte to the device
+ *
+ * Wrapper for SystemVerilog Write Byte task
+ *
+ * @code
+ *
+ * Example:
+ *
+ * uint8_t page_number = MAIN_SELECT_PAGE;
+ *
+ * int status = VL53L0X_write_byte(PAGE_SELECT, page_number);
+ *
+ * @endcode
+ *
+ * @param  address - uint8_t device address value
+ * @param  index - uint8_t register index value
+ * @param  data  - uint8_t data value to write
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_write_byte(VL53L0X_DEV dev,  uint8_t index, uint8_t   data);
+
+
+/**
+ * @brief  Writes a single word (16-bit unsigned) to the device
+ *
+ * Manages the big-endian nature of the device (first byte written is the
+ * MS byte).
+ * Uses SystemVerilog Write Multi task.
+ *
+ * @code
+ *
+ * Example:
+ *
+ * uint16_t nvm_ctrl_pulse_width = 0x0004;
+ *
+ * int status = VL53L0X_write_word(NVM_CTRL__PULSE_WIDTH_MSB,
+ * nvm_ctrl_pulse_width);
+ *
+ * @endcode
+ *
+ * @param  address - uint8_t device address value
+ * @param  index - uint8_t register index value
+ * @param  data  - uin16_t data value write
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_write_word(VL53L0X_DEV dev,  uint8_t index, uint16_t  data);
+
+
+/**
+ * @brief  Writes a single dword (32-bit unsigned) to the device
+ *
+ * Manages the big-endian nature of the device (first byte written is the
+ * MS byte).
+ * Uses SystemVerilog Write Multi task.
+ *
+ * @code
+ *
+ * Example:
+ *
+ * uint32_t nvm_data = 0x0004;
+ *
+ * int status = VL53L0X_write_dword(NVM_CTRL__DATAIN_MMM, nvm_data);
+ *
+ * @endcode
+ *
+ * @param  address - uint8_t device address value
+ * @param  index - uint8_t register index value
+ * @param  data  - uint32_t data value to write
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_write_dword(VL53L0X_DEV dev, uint8_t index, uint32_t  data);
+
+
+
+/**
+ * @brief  Reads a single byte from the device
+ *
+ * Uses SystemVerilog Read Byte task.
+ *
+ * @code
+ *
+ * Example:
+ *
+ * uint8_t device_status = 0;
+ *
+ * int status = VL53L0X_read_byte(STATUS, &device_status);
+ *
+ * @endcode
+ *
+ * @param  address - uint8_t device address value
+ * @param  index  - uint8_t register index value
+ * @param  pdata  - pointer to uint8_t data value
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_read_byte(VL53L0X_DEV dev,  uint8_t index, uint8_t  *pdata);
+
+
+/**
+ * @brief  Reads a single word (16-bit unsigned) from the device
+ *
+ * Manages the big-endian nature of the device (first byte read is the MS byte).
+ * Uses SystemVerilog Read Multi task.
+ *
+ * @code
+ *
+ * Example:
+ *
+ * uint16_t timeout = 0;
+ *
+ * int status = VL53L0X_read_word(TIMEOUT_OVERALL_PERIODS_MSB, &timeout);
+ *
+ * @endcode
+ *
+ * @param  address - uint8_t device address value
+ * @param  index  - uint8_t register index value
+ * @param  pdata  - pointer to uint16_t data value
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_read_word(VL53L0X_DEV dev,  uint8_t index, uint16_t *pdata);
+
+
+/**
+ * @brief  Reads a single dword (32-bit unsigned) from the device
+ *
+ * Manages the big-endian nature of the device (first byte read is the MS byte).
+ * Uses SystemVerilog Read Multi task.
+ *
+ * @code
+ *
+ * Example:
+ *
+ * uint32_t range_1 = 0;
+ *
+ * int status = VL53L0X_read_dword(RANGE_1_MMM, &range_1);
+ *
+ * @endcode
+ *
+ * @param  address - uint8_t device address value
+ * @param  index - uint8_t register index value
+ * @param  pdata - pointer to uint32_t data value
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_read_dword(VL53L0X_DEV dev, uint8_t index, uint32_t *pdata);
+
+
+/**
+ * @brief  Implements a programmable wait in us
+ *
+ * Wrapper for SystemVerilog Wait in micro seconds task
+ *
+ * @param  wait_us - integer wait in micro seconds
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_platform_wait_us(int32_t wait_us);
+
+
+/**
+ * @brief  Implements a programmable wait in ms
+ *
+ * Wrapper for SystemVerilog Wait in milli seconds task
+ *
+ * @param  wait_ms - integer wait in milli seconds
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_wait_ms(int32_t wait_ms);
+
+
+/**
+ * @brief Set GPIO value
+ *
+ * @param  level  - input  level - either 0 or 1
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_set_gpio(uint8_t  level);
+
+
+/**
+ * @brief Get GPIO value
+ *
+ * @param  plevel - uint8_t pointer to store GPIO level (0 or 1)
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_get_gpio(uint8_t *plevel);
+
+/**
+ * @brief Release force on GPIO
+ *
+ * @return status - SystemVerilog status 0 = ok, 1 = error
+ *
+ */
+
+int32_t VL53L0X_release_gpio(void);
+
+
+/**
+* @brief Get the frequency of the timer used for ranging results time stamps
+*
+* @param[out] ptimer_freq_hz : pointer for timer frequency
+*
+* @return status : 0 = ok, 1 = error
+*
+*/
+
+int32_t VL53L0X_get_timer_frequency(int32_t *ptimer_freq_hz);
+
+/**
+* @brief Get the timer value in units of timer_freq_hz
+* (see VL53L0X_get_timestamp_frequency())
+*
+* @param[out] ptimer_count : pointer for timer count value
+*
+* @return status : 0 = ok, 1 = error
+*
+*/
+
+int32_t VL53L0X_get_timer_value(int32_t *ptimer_count);
+int VL53L0X_I2CWrite(VL53L0X_DEV dev, uint8_t *buff, uint8_t len);
+int VL53L0X_I2CRead(VL53L0X_DEV dev, uint8_t *buff, uint8_t len);
+
+#endif /* _VL53L0X_I2C_PLATFORM_H_ */
+

lib/vl53l0x/vl53l0x_interrupt_threshold_settings.h

@@ -0,0 +1,194 @@
+/*******************************************************************************
+ * Copyright © 2016, STMicroelectronics International N.V.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+	* Redistributions of source code must retain the above copyright
+	  notice, this list of conditions and the following disclaimer.
+	* Redistributions in binary form must reproduce the above copyright
+	  notice, this list of conditions and the following disclaimer in the
+	  documentation and/or other materials provided with the distribution.
+	* Neither the name of STMicroelectronics nor the
+	  names of its contributors may be used to endorse or promote products
+	  derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
+NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
+IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*******************************************************************************/
+
+
+#ifndef _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_
+#define _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+uint8_t InterruptThresholdSettings[] = {
+
+	/* Start of Interrupt Threshold Settings */
+	0x1, 0xff, 0x00,
+	0x1, 0x80, 0x01,
+	0x1, 0xff, 0x01,
+	0x1, 0x00, 0x00,
+	0x1, 0xff, 0x01,
+	0x1, 0x4f, 0x02,
+	0x1, 0xFF, 0x0E,
+	0x1, 0x00, 0x03,
+	0x1, 0x01, 0x84,
+	0x1, 0x02, 0x0A,
+	0x1, 0x03, 0x03,
+	0x1, 0x04, 0x08,
+	0x1, 0x05, 0xC8,
+	0x1, 0x06, 0x03,
+	0x1, 0x07, 0x8D,
+	0x1, 0x08, 0x08,
+	0x1, 0x09, 0xC6,
+	0x1, 0x0A, 0x01,
+	0x1, 0x0B, 0x02,
+	0x1, 0x0C, 0x00,
+	0x1, 0x0D, 0xD5,
+	0x1, 0x0E, 0x18,
+	0x1, 0x0F, 0x12,
+	0x1, 0x10, 0x01,
+	0x1, 0x11, 0x82,
+	0x1, 0x12, 0x00,
+	0x1, 0x13, 0xD5,
+	0x1, 0x14, 0x18,
+	0x1, 0x15, 0x13,
+	0x1, 0x16, 0x03,
+	0x1, 0x17, 0x86,
+	0x1, 0x18, 0x0A,
+	0x1, 0x19, 0x09,
+	0x1, 0x1A, 0x08,
+	0x1, 0x1B, 0xC2,
+	0x1, 0x1C, 0x03,
+	0x1, 0x1D, 0x8F,
+	0x1, 0x1E, 0x0A,
+	0x1, 0x1F, 0x06,
+	0x1, 0x20, 0x01,
+	0x1, 0x21, 0x02,
+	0x1, 0x22, 0x00,
+	0x1, 0x23, 0xD5,
+	0x1, 0x24, 0x18,
+	0x1, 0x25, 0x22,
+	0x1, 0x26, 0x01,
+	0x1, 0x27, 0x82,
+	0x1, 0x28, 0x00,
+	0x1, 0x29, 0xD5,
+	0x1, 0x2A, 0x18,
+	0x1, 0x2B, 0x0B,
+	0x1, 0x2C, 0x28,
+	0x1, 0x2D, 0x78,
+	0x1, 0x2E, 0x28,
+	0x1, 0x2F, 0x91,
+	0x1, 0x30, 0x00,
+	0x1, 0x31, 0x0B,
+	0x1, 0x32, 0x00,
+	0x1, 0x33, 0x0B,
+	0x1, 0x34, 0x00,
+	0x1, 0x35, 0xA1,
+	0x1, 0x36, 0x00,
+	0x1, 0x37, 0xA0,
+	0x1, 0x38, 0x00,
+	0x1, 0x39, 0x04,
+	0x1, 0x3A, 0x28,
+	0x1, 0x3B, 0x30,
+	0x1, 0x3C, 0x0C,
+	0x1, 0x3D, 0x04,
+	0x1, 0x3E, 0x0F,
+	0x1, 0x3F, 0x79,
+	0x1, 0x40, 0x28,
+	0x1, 0x41, 0x1E,
+	0x1, 0x42, 0x2F,
+	0x1, 0x43, 0x87,
+	0x1, 0x44, 0x00,
+	0x1, 0x45, 0x0B,
+	0x1, 0x46, 0x00,
+	0x1, 0x47, 0x0B,
+	0x1, 0x48, 0x00,
+	0x1, 0x49, 0xA7,
+	0x1, 0x4A, 0x00,
+	0x1, 0x4B, 0xA6,
+	0x1, 0x4C, 0x00,
+	0x1, 0x4D, 0x04,
+	0x1, 0x4E, 0x01,
+	0x1, 0x4F, 0x00,
+	0x1, 0x50, 0x00,
+	0x1, 0x51, 0x80,
+	0x1, 0x52, 0x09,
+	0x1, 0x53, 0x08,
+	0x1, 0x54, 0x01,
+	0x1, 0x55, 0x00,
+	0x1, 0x56, 0x0F,
+	0x1, 0x57, 0x79,
+	0x1, 0x58, 0x09,
+	0x1, 0x59, 0x05,
+	0x1, 0x5A, 0x00,
+	0x1, 0x5B, 0x60,
+	0x1, 0x5C, 0x05,
+	0x1, 0x5D, 0xD1,
+	0x1, 0x5E, 0x0C,
+	0x1, 0x5F, 0x3C,
+	0x1, 0x60, 0x00,
+	0x1, 0x61, 0xD0,
+	0x1, 0x62, 0x0B,
+	0x1, 0x63, 0x03,
+	0x1, 0x64, 0x28,
+	0x1, 0x65, 0x10,
+	0x1, 0x66, 0x2A,
+	0x1, 0x67, 0x39,
+	0x1, 0x68, 0x0B,
+	0x1, 0x69, 0x02,
+	0x1, 0x6A, 0x28,
+	0x1, 0x6B, 0x10,
+	0x1, 0x6C, 0x2A,
+	0x1, 0x6D, 0x61,
+	0x1, 0x6E, 0x0C,
+	0x1, 0x6F, 0x00,
+	0x1, 0x70, 0x0F,
+	0x1, 0x71, 0x79,
+	0x1, 0x72, 0x00,
+	0x1, 0x73, 0x0B,
+	0x1, 0x74, 0x00,
+	0x1, 0x75, 0x0B,
+	0x1, 0x76, 0x00,
+	0x1, 0x77, 0xA1,
+	0x1, 0x78, 0x00,
+	0x1, 0x79, 0xA0,
+	0x1, 0x7A, 0x00,
+	0x1, 0x7B, 0x04,
+	0x1, 0xFF, 0x04,
+	0x1, 0x79, 0x1D,
+	0x1, 0x7B, 0x27,
+	0x1, 0x96, 0x0E,
+	0x1, 0x97, 0xFE,
+	0x1, 0x98, 0x03,
+	0x1, 0x99, 0xEF,
+	0x1, 0x9A, 0x02,
+	0x1, 0x9B, 0x44,
+	0x1, 0x73, 0x07,
+	0x1, 0x70, 0x01,
+	0x1, 0xff, 0x01,
+	0x1, 0x00, 0x01,
+	0x1, 0xff, 0x00,
+	0x00, 0x00, 0x00
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_ */

lib/vl53l0x/vl53l0x_platform.h

@@ -0,0 +1,41 @@
+#ifndef VL53L0X_PLATFORM_H_
+#define VL53L0X_PLATFORM_H_
+
+#include "vl53l0x_def.h"
+#include "vl53l0x_platform_log.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define VL53L0X_MAX_I2C_XFER_SIZE  64
+
+typedef struct {
+	VL53L0X_DevData_t Data;
+	uint8_t I2cDevAddr;
+	int i2c_fd;
+} vl53l0x_dev_t;
+
+typedef vl53l0x_dev_t *VL53L0X_DEV;
+
+#define PALDevDataGet(Dev, field)    (Dev->Data.field)
+#define PALDevDataSet(Dev, field, data) ((Dev->Data.field) = (data))
+
+VL53L0X_Error VL53L0X_LockSequenceAccess(VL53L0X_DEV Dev);
+VL53L0X_Error VL53L0X_UnlockSequenceAccess(VL53L0X_DEV Dev);
+VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count);
+VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count);
+VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data);
+VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data);
+VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data);
+VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data);
+VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data);
+VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data);
+VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData);
+VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

lib/vl53l0x/vl53l0x_platform_log.h

@@ -0,0 +1,14 @@
+#ifndef VL53L0X_PLATFORM_LOG_H_
+#define VL53L0X_PLATFORM_LOG_H_
+
+#include <string.h>
+
+#define VL53L0X_COPYSTRING(str, ...) strncpy(str, ##__VA_ARGS__, sizeof(str))
+
+#define _LOG_FUNCTION_START(module, fmt, ...)   (void)0
+#define _LOG_FUNCTION_END(module, status, ...)  (void)0
+#define _LOG_FUNCTION_END_FMT(module, status, fmt, ...) (void)0
+
+#define VL53L0X_ErrLog(...) (void)0
+
+#endif

lib/vl53l0x/vl53l0x_tuning.h

@@ -0,0 +1,146 @@
+/*******************************************************************************
+ * Copyright © 2016, STMicroelectronics International N.V.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+	* Redistributions of source code must retain the above copyright
+	  notice, this list of conditions and the following disclaimer.
+	* Redistributions in binary form must reproduce the above copyright
+	  notice, this list of conditions and the following disclaimer in the
+	  documentation and/or other materials provided with the distribution.
+	* Neither the name of STMicroelectronics nor the
+	  names of its contributors may be used to endorse or promote products
+	  derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
+NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
+IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*******************************************************************************/
+
+
+#ifndef _VL53L0X_TUNING_H_
+#define _VL53L0X_TUNING_H_
+
+#include "vl53l0x_def.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+uint8_t DefaultTuningSettings[] = {
+
+	/* update 02/11/2015_v36 */
+	0x01, 0xFF, 0x01,
+	0x01, 0x00, 0x00,
+
+	0x01, 0xFF, 0x00,
+	0x01, 0x09, 0x00,
+	0x01, 0x10, 0x00,
+	0x01, 0x11, 0x00,
+
+	0x01, 0x24, 0x01,
+	0x01, 0x25, 0xff,
+	0x01, 0x75, 0x00,
+
+	0x01, 0xFF, 0x01,
+	0x01, 0x4e, 0x2c,
+	0x01, 0x48, 0x00,
+	0x01, 0x30, 0x20,
+
+	0x01, 0xFF, 0x00,
+	0x01, 0x30, 0x09, /* mja changed from 0x64. */
+	0x01, 0x54, 0x00,
+	0x01, 0x31, 0x04,
+	0x01, 0x32, 0x03,
+	0x01, 0x40, 0x83,
+	0x01, 0x46, 0x25,
+	0x01, 0x60, 0x00,
+	0x01, 0x27, 0x00,
+	0x01, 0x50, 0x06,
+	0x01, 0x51, 0x00,
+	0x01, 0x52, 0x96,
+	0x01, 0x56, 0x08,
+	0x01, 0x57, 0x30,
+	0x01, 0x61, 0x00,
+	0x01, 0x62, 0x00,
+	0x01, 0x64, 0x00,
+	0x01, 0x65, 0x00,
+	0x01, 0x66, 0xa0,
+
+	0x01, 0xFF, 0x01,
+	0x01, 0x22, 0x32,
+	0x01, 0x47, 0x14,
+	0x01, 0x49, 0xff,
+	0x01, 0x4a, 0x00,
+
+	0x01, 0xFF, 0x00,
+	0x01, 0x7a, 0x0a,
+	0x01, 0x7b, 0x00,
+	0x01, 0x78, 0x21,
+
+	0x01, 0xFF, 0x01,
+	0x01, 0x23, 0x34,
+	0x01, 0x42, 0x00,
+	0x01, 0x44, 0xff,
+	0x01, 0x45, 0x26,
+	0x01, 0x46, 0x05,
+	0x01, 0x40, 0x40,
+	0x01, 0x0E, 0x06,
+	0x01, 0x20, 0x1a,
+	0x01, 0x43, 0x40,
+
+	0x01, 0xFF, 0x00,
+	0x01, 0x34, 0x03,
+	0x01, 0x35, 0x44,
+
+	0x01, 0xFF, 0x01,
+	0x01, 0x31, 0x04,
+	0x01, 0x4b, 0x09,
+	0x01, 0x4c, 0x05,
+	0x01, 0x4d, 0x04,
+
+
+	0x01, 0xFF, 0x00,
+	0x01, 0x44, 0x00,
+	0x01, 0x45, 0x20,
+	0x01, 0x47, 0x08,
+	0x01, 0x48, 0x28,
+	0x01, 0x67, 0x00,
+	0x01, 0x70, 0x04,
+	0x01, 0x71, 0x01,
+	0x01, 0x72, 0xfe,
+	0x01, 0x76, 0x00,
+	0x01, 0x77, 0x00,
+
+	0x01, 0xFF, 0x01,
+	0x01, 0x0d, 0x01,
+
+	0x01, 0xFF, 0x00,
+	0x01, 0x80, 0x01,
+	0x01, 0x01, 0xF8,
+
+	0x01, 0xFF, 0x01,
+	0x01, 0x8e, 0x01,
+	0x01, 0x00, 0x01,
+	0x01, 0xFF, 0x00,
+	0x01, 0x80, 0x00,
+
+	0x00, 0x00, 0x00
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _VL53L0X_TUNING_H_ */

lib/vl53l0x/vl53l0x_types.h

@@ -0,0 +1,13 @@
+#ifndef VL53L0X_TYPES_H_
+#define VL53L0X_TYPES_H_
+
+#include <stdint.h>
+#include <stddef.h>
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+typedef unsigned int FixPoint1616_t;
+
+#endif

lib/vl53l0x_direct.h

@@ -0,0 +1,23 @@
+#pragma once
+
+extern "C" {
+#include "vl53l0x_platform.h"
+}
+
+class VL53L0X_Direct
+{
+public:
+	VL53L0X_Direct();
+	~VL53L0X_Direct();
+
+	bool init();
+	bool readRange(VL53L0X_RangingMeasurementData_t &data);
+	void stop();
+	bool isInitialized() const { return initialized; }
+
+private:
+	bool initialized;
+	vl53l0x_dev_t dev;
+
+	static void unbindKernel();
+};

main/CMakeLists.txt

@@ -1,3 +1,12 @@
 # 主程序
-add_executable(smartcar_demo1 main.cpp)
+add_executable(smartcar_test main.cpp)
-target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS})
+target_link_libraries(smartcar_test common_lib ${OpenCV_LIBS})
+
+add_executable(lidar_test lidar_test.cpp)
+target_link_libraries(lidar_test common_lib)
+
+add_executable(remote_control remote_control.cpp)
+target_link_libraries(remote_control common_lib)
+
+add_executable(lcd_test lcd_test.cpp)
+target_link_libraries(lcd_test common_lib ${OpenCV_LIBS})

main/lidar_test.cpp

@@ -0,0 +1,49 @@
+#include <cstdio>
+#include <csignal>
+#include <atomic>
+#include <unistd.h>
+#include "vl53l0x.h"
+
+static std::atomic<bool> running(true);
+static void on_signal(int) { running.store(false); }
+
+int main()
+{
+    setvbuf(stdout, NULL, _IONBF, 0);
+    std::signal(SIGINT, on_signal);
+    std::signal(SIGTERM, on_signal);
+
+    VL53L0X sensor;
+    if (!sensor.init()) {
+        fprintf(stderr, "VL53L0X init failed\n");
+        return 1;
+    }
+
+    sensor.startMeasure();
+
+    printf("lidar_test running, Ctrl+C to stop\n");
+    printf("%-8s %-8s %-10s\n", "cnt", "range_mm", "status");
+
+    int count = 0;
+    VL53L0X_RangingMeasurementData_t data;
+
+    while (running.load()) {
+        if (!sensor.readResult(data)) {
+            printf("%-8d %-8s %-10s\n", ++count, "ERR", "timeout");
+            sensor.startMeasure();
+            usleep(50000);
+            continue;
+        }
+
+        int range = data.RangeMilliMeter;
+        const char* st = (data.RangeStatus == 0) ? "OK" : "SIGMA";
+        printf("%-8d %-8d %-10s\n", ++count, range, st);
+
+        sensor.startMeasure();
+        usleep(30000);
+    }
+
+    sensor.stop();
+    printf("stopped\n");
+    return 0;
+}

main/main.cpp

@@ -1,59 +1,38 @@
 #include <iostream>
-#include <fstream>
 #include <opencv2/opencv.hpp>
-#include <string>
 #include <csignal>
 #include <atomic>
-#include <chrono>
 #include <thread>
+#include <chrono>
+#include <sched.h>
+#include <sys/resource.h>
 
 #include "global.h"
 #include "camera.h"
 #include "control.h"
 
 std::atomic<bool> running(true);
-void signalHandler(int signal)
+void signalHandler(int) { running.store(false); }
-{
-    running.store(false);
-}
 
 int main(void)
 {
     std::signal(SIGINT, signalHandler);
 
-    double dest_fps = readDoubleFromFile(destfps_file);
-    if (dest_fps <= 0) dest_fps = 30.0;
-
     cfg_load_all();
 
-    double avoid_gain_val  = readDoubleFromFile(cone_avoid_gain_file);
+    if (CameraInit(0) < 0) {
-    int    avoid_range_val = (int)readDoubleFromFile(cone_avoid_range_file);
-    int    hold_frames_val = (int)readDoubleFromFile(cone_hold_frames_file);
-    if (avoid_gain_val > 0)        g_cfg.cone_avoid_gain  = avoid_gain_val;
-    if (avoid_range_val > 0)       g_cfg.cone_avoid_range = avoid_range_val;
-    if (hold_frames_val > 0)       g_cfg.cone_hold_frames  = hold_frames_val;
-
-    if (CameraInit(0, dest_fps, 320, 240) < 0) {
         std::cerr << "CameraInit failed" << std::endl;
         return -1;
     }
     ControlInit();
     std::cout << "All services started" << std::endl;
 
-    int tick = 0;
+    setpriority(PRIO_PROCESS, 0, -10);
-    while (running.load())
+
-    {
+    while (running.load()) {
         CameraHandler();
-
-        if (++tick % 7 == 0) {
-            g_cfg.debug = readFlag(debug_file);
-        }
-
-        if (g_cfg.debug) {
-            cfg_load_all();
-        }
-
         target_speed = g_cfg.speed;
+        sched_yield();
     }
 
     std::cout << "Stopping..." << std::endl;

main/remote_control.cpp

@@ -0,0 +1,93 @@
+/*
+ * remote_control — 终端 WASD 遥控
+ *
+ * 用法: SSH 到小车后直接运行 ./remote_control
+ *   W/S = 前进/后退
+ *   A/D = 左转/右转
+ *   Q   = 退出
+ *
+ * 硬件:
+ *   左电机: pwmchip8/pwm2, GPIO12 方向, GPIO13 IN2=高
+ *   右电机: pwmchip8/pwm1, GPIO13 方向
+ *   使能:   GPIO73
+ *   舵机:   pwmchip1/pwm0, 3ms 周期, 1.5ms 中位
+ */
+#include "MotorController.h"
+#include "PwmController.h"
+#include "GPIO.h"
+#include <cstdio>
+#include <cstdlib>
+#include <termios.h>
+#include <unistd.h>
+
+static constexpr unsigned int MOTOR_PERIOD_NS = 50000;
+static constexpr unsigned int SERVO_PERIOD_NS = 3000000;
+static constexpr unsigned int SERVO_CENTER_NS = 1500000;
+static constexpr unsigned int SERVO_RANGE_NS  = 300000;
+
+int main()
+{
+    GPIO mortorEN(73);
+    mortorEN.setDirection("out");
+    mortorEN.setValue(1);
+
+    GPIO leftIn2(13);
+    leftIn2.setDirection("out");
+    leftIn2.setValue(1);
+
+    MotorController motorL(8, 2, 12, MOTOR_PERIOD_NS);
+    MotorController motorR(8, 1, 13, MOTOR_PERIOD_NS);
+
+    PwmController servo(1, 0);
+    servo.setPeriod(SERVO_PERIOD_NS);
+    servo.setDutyCycle(SERVO_CENTER_NS);
+    servo.enable();
+
+    struct termios old_tio, new_tio;
+    tcgetattr(STDIN_FILENO, &old_tio);
+    new_tio = old_tio;
+    new_tio.c_lflag &= ~(ICANON | ECHO);
+    tcsetattr(STDIN_FILENO, TCSANOW, &new_tio);
+
+    printf("WASD 遥控已启动\n");
+    printf("  W=前进  S=后退  A=左转  D=右转  Q=退出  松手即停\n");
+
+    double speed = 0, steer = 0;
+    bool running = true;
+
+    while (running) {
+        fd_set fds;
+        FD_ZERO(&fds);
+        FD_SET(STDIN_FILENO, &fds);
+        struct timeval tv = {0, 50000};
+
+        if (select(STDIN_FILENO + 1, &fds, nullptr, nullptr, &tv) > 0) {
+            char c;
+            if (read(STDIN_FILENO, &c, 1) == 1) {
+                switch (c) {
+                case 'w': case 'W': speed =  50; break;
+                case 's': case 'S': speed = -50; break;
+                case 'a': case 'A': steer = -100; break;
+                case 'd': case 'D': steer =  100; break;
+                case 'q': case 'Q': running = false; break;
+                }
+            }
+        } else {
+            speed = 0;
+            steer = 0;
+        }
+
+        unsigned int duty_ns = SERVO_CENTER_NS + (unsigned int)(steer / 100.0 * SERVO_RANGE_NS);
+        servo.setDutyCycle(duty_ns);
+        motorL.updateduty(speed);
+        motorR.updateduty(speed);
+    }
+
+    motorL.updateduty(0);
+    motorR.updateduty(0);
+    servo.setDutyCycle(SERVO_CENTER_NS);
+    mortorEN.setValue(0);
+    tcsetattr(STDIN_FILENO, TCSANOW, &old_tio);
+    printf("\n已退出\n");
+    return 0;
+}

pack_code.sh

@@ -0,0 +1,53 @@
+#!/bin/bash
+# pack_code.sh — 将所有代码源文件打包为单个 txt
+# 用法: bash pack_code.sh [输出文件名]
+# 默认输出: smartcar2_code.txt
+
+OUT="${1:-smartcar2_code.txt}"
+> "$OUT"
+
+# 分隔符
+sep() { echo "" >> "$OUT"; echo "============================================================" >> "$OUT"; }
+
+# 逐个追入，带文件路径头
+append_file() {
+    local f="$1"
+    if [ ! -f "$f" ]; then return; fi
+    sep
+    echo "// FILE: $f" >> "$OUT"
+    sep
+    cat "$f" >> "$OUT"
+}
+
+# ── src/ 源文件 ──
+for f in src/*.cpp src/*.hpp; do
+    [ -f "$f" ] && append_file "$f"
+done
+
+# ── lib/ 头文件 ──
+for f in lib/*.h; do
+    [ -f "$f" ] && append_file "$f"
+done
+
+# ── main/ ──
+append_file main/main.cpp
+append_file main/CMakeLists.txt
+
+# ── 顶层构建/配置 ──
+append_file CMakeLists.txt
+append_file cross.cmake
+append_file ctl.sh
+append_file start.sh
+append_file .gitignore
+
+# ── 独立 demo ──
+append_file encoder_brake_demo.cpp
+
+# ── 统计 ──
+LINES=$(wc -l < "$OUT")
+FILES=$(grep -c "// FILE:" "$OUT")
+echo ""
+echo "=== 打包完成 ==="
+echo "  输出: $OUT"
+echo "  文件数: $FILES"
+echo "  总行数: $LINES"

src/MotorController.cpp

@@ -7,6 +7,7 @@ MotorController::MotorController(int pwmchip, int pwmnum, int gpioNum, unsigned
     : pwmController(pwmchip, pwmnum), directionGPIO(gpioNum)
 {
     pwmController.setPeriod(period_ns);
+    pwmController.setDutyCycle(0);               // 先归零再使能, 防止 sysfs 残留值导致电机瞬动
     directionGPIO.setDirection("out");
     pwmController.enable();
 }

src/control.cpp

@@ -1,8 +1,15 @@
 /*
  * control — 电机控制层
  *
- * 开环占空比控制 + 斑马线编码器比例刹车。
+ * 开环占空比控制 + 编码器比例刹车（斑马线/红绿灯停车时触发）。
  * 编码器线程: 高速轮询 gpio67 → 算速度(脉冲/秒) → 原子变量共享。
+ *
+ * 硬件映射:
+ *   motor[0] = 左电机  pwmchip8/pwm2, GPIO12 方向
+ *   motor[1] = 右电机  pwmchip8/pwm1, GPIO13 方向
+ *   GPIO73   = 电机使能 (mortorEN)
+ *   GPIO67   = 编码器 LSB 脉冲 (输入)
+ *   GPIO72   = 编码器方向 (输入)
  */
 #include "control.h"
 #include "global.h"
@@ -10,19 +17,27 @@
 #include <atomic>
 #include <cmath>
 
-extern double g_steer_deviation;
+// ═══════════════════════════════════════════════════════════
-
+//  全局 & 静态变量
+// ═══════════════════════════════════════════════════════════
 MotorController *motorController[2] = {nullptr, nullptr};
-GPIO mortorEN(73);
+GPIO mortorEN(73);                      // 电机使能引脚
 
-static GPIO encoderLSB(67);
+static GPIO encoderLSB(67);            // 编码器 LSB 脉冲
-static GPIO encoderDIR(72);
+static GPIO encoderDIR(72);            // 编码器方向
 
 static std::thread        g_enc_thread;
 static std::atomic<bool>  g_enc_running{false};
-static std::atomic<double> g_enc_speed{0.0};
+static std::atomic<double> g_enc_speed{0.0};   // 编码器速度 (脉冲/秒)
-static std::atomic<int>    g_enc_dir{0};
+static std::atomic<int>    g_enc_dir{0};        // 编码器方向 (0/1)
 
+// ═══════════════════════════════════════════════════════════
+//  编码器线程 — 高频轮询测速
+//
+//  ~2kHz (500µs sleep) 轮询 GPIO67 边沿计脉冲，
+//  每 100ms 窗口计算一次速度写入原子变量 g_enc_speed。
+//  仅在刹车时被 ControlUpdate 读取。
+// ═══════════════════════════════════════════════════════════
 static long enc_now_ns() {
     struct timespec ts;
     clock_gettime(CLOCK_MONOTONIC, &ts);
@@ -38,43 +53,58 @@ static void encoder_thread() {
         int cur_lsb = encoderLSB.readValue() ? 1 : 0;
         int cur_dir = encoderDIR.readValue() ? 1 : 0;
 
+        // 检测 LSB 边沿变化 → 计脉冲
         if (cur_lsb != prev_lsb) {
             pulse_cnt++;
             prev_lsb = cur_lsb;
         }
         g_enc_dir.store(cur_dir);
 
+        // 每 100ms 窗口输出一次速度
         long t1 = enc_now_ns();
         long dt = t1 - t0;
-        if (dt >= 100000000L) {
+        if (dt >= 100000000L) {                     // 100ms
             g_enc_speed.store(pulse_cnt / (dt / 1e9));
             pulse_cnt = 0;
             t0 = t1;
         }
 
+        // 500µs 间隔，防止 CPU 饿死其他线程
         std::this_thread::sleep_for(std::chrono::microseconds(500));
     }
 }
 
+// ═══════════════════════════════════════════════════════════
+//  ControlInit — 初始化电机 + 编码器线程
+//
+//  调用时机: main.cpp 启动时，CameraInit 之后。
+// ═══════════════════════════════════════════════════════════
 void ControlInit()
 {
+    // 电机使能引脚
     mortorEN.setDirection("out");
     mortorEN.setValue(1);
 
+    // 编码器引脚
     encoderLSB.setDirection("in");
     encoderDIR.setDirection("in");
 
+    // 启动编码器测速线程
     g_enc_running.store(true);
     g_enc_thread = std::thread(encoder_thread);
 
+    // GPIO13 (左电机 IN2) 设为高电平，配合 PWM 实现 H 桥正/反转
     GPIO leftIn2(13);
     leftIn2.setDirection("out");
     leftIn2.setValue(1);
 
+    // 创建双电机控制器
+    //   motor[0]: pwmchip8/pwm2 + GPIO12 方向 (左电机)
+    //   motor[1]: pwmchip8/pwm1 + GPIO13 方向 (右电机)
     const int pwmchip[2]  = {8, 8};
     const int pwmnum[2]   = {2, 1};
     const int gpioNum[2]  = {12, 13};
-    const unsigned int period_ns = 50000;
+    const unsigned int period_ns = 50000;           // PWM 周期 50µs = 20kHz
 
     for (int i = 0; i < 2; ++i)
     {
@@ -84,59 +114,77 @@ void ControlInit()
     }
 }
 
+// ═══════════════════════════════════════════════════════════
+//  ControlUpdate — 每帧电机控制入口
+//
+//  speed:       目标速度 (占空比 %)，已经过弯道减速处理
+//  zebra_block: true = 斑马线 STOP 或红绿灯 STOP/WAIT_GREEN
+//
+//  三种模式:
+//    1. g_cfg.start == 0  → 全部停转，拉低使能
+//    2. zebra_block        → 编码器比例刹车 (反向 PWM)
+//    3. 正常               → 开环占空比驱动
+// ═══════════════════════════════════════════════════════════
 void ControlUpdate(double speed, bool zebra_block)
 {
+    // ── 模式 1: 未启动 → 停转 ──
     if (!g_cfg.start)
     {
         for (int i = 0; i < 2; ++i)
             if (motorController[i]) motorController[i]->updateduty(0);
-        mortorEN.setValue(0);
+        mortorEN.setValue(0);                       // 拉低使能
         return;
     }
 
+    // ── 模式 2: 刹车 (斑马线/红绿灯停车) ──
     if (zebra_block)
     {
-        double cur_speed = g_enc_speed.load();
+        double cur_speed = g_enc_speed.load();      // 原子读: 编码器速度 (pps)
-        int    cur_dir   = g_enc_dir.load();
+        int    cur_dir   = g_enc_dir.load();        // 原子读: 编码器方向
 
-        if (cur_speed > 0.5)
+        if (cur_speed > 0.5)                        // 仍在运动
         {
+            // 比例刹车: 速度越快 → 刹车占空比越大
             int brake_ns = (int)(cur_speed * g_cfg.brake_scale);
             if (brake_ns > g_cfg.brake_max) brake_ns = g_cfg.brake_max;
-            double brake_pct = brake_ns / 500.0;       // ns → % (period=50000ns)
+            double brake_pct = brake_ns / 500.0;    // ns → % (period=50000ns, 50000/100=500)
             int dir = cur_dir ? 1 : 0;
 
+            // 施加反向 PWM: 方向取反实现刹车
             for (int i = 0; i < 2; ++i)
                 if (motorController[i])
                     motorController[i]->updateduty(dir ? -brake_pct : brake_pct);
         }
-        else
+        else                                        // 已停止 (< 0.5 pps)
         {
+            // 不再施加刹车，防止反冲
             for (int i = 0; i < 2; ++i)
                 if (motorController[i]) motorController[i]->updateduty(0);
         }
         return;
     }
 
-    double curve = 1.0 - std::abs(g_steer_deviation) * 0.4;
+    // ── 模式 3: 正常行驶 → 开环占空比 ──
-    if (curve < 0.6) curve = 0.6;
-    double spd = speed * curve;
-
     for (int i = 0; i < 2; ++i)
-        if (motorController[i]) motorController[i]->updateduty(spd);
+        if (motorController[i]) motorController[i]->updateduty(speed);
 
-    mortorEN.setValue(1);
+    mortorEN.setValue(1);                            // 使能拉高
 }
 
+// ═══════════════════════════════════════════════════════════
+//  ControlExit — 停止编码器线程 + 释放电机资源
+// ═══════════════════════════════════════════════════════════
 void ControlExit()
 {
+    // 停止编码器线程
     g_enc_running.store(false);
     if (g_enc_thread.joinable()) g_enc_thread.join();
 
+    // 释放电机控制器
     for (int i = 0; i < 2; ++i)
     {
         delete motorController[i];
         motorController[i] = nullptr;
     }
-    mortorEN.setValue(0);
+    mortorEN.setValue(0);                            // 使能拉低
 }

src/global.cpp

@@ -24,6 +24,8 @@ void cfg_load_all()
 {
     g_cfg.speed       = readDoubleFromFile(speed_file);
     g_cfg.foresee     = readDoubleFromFile(foresee_file);
+    g_cfg.foresee_lost_scale = readDoubleFromFile(foresee_lost_scale_file);
+    g_cfg.sharp_turn_scale   = readDoubleFromFile(sharp_turn_scale_file);
     g_cfg.zebrasee    = readDoubleFromFile(zebrasee_file);
     g_cfg.deadband    = readDoubleFromFile(deadband_file);
     g_cfg.steer_gain  = readDoubleFromFile(steer_gain_file);
@@ -36,7 +38,29 @@ void cfg_load_all()
     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);
+    g_cfg.cone_avoid_gain = readDoubleFromFile(cone_avoid_gain_file);
+    g_cfg.cone_avoid_range = (int)readDoubleFromFile(cone_avoid_range_file);
+    g_cfg.cone_hold_frames = (int)readDoubleFromFile(cone_hold_frames_file);
+    g_cfg.cone_return_gain = readDoubleFromFile(cone_return_gain_file);
+    g_cfg.cone_return_frames = (int)readDoubleFromFile(cone_return_frames_file);
 
     g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
     g_cfg.brake_max   = readDoubleFromFile(brake_max_file);
+
+    g_cfg.curve_slope = readDoubleFromFile(curve_slope_file);
+    g_cfg.curve_min   = readDoubleFromFile(curve_min_file);
+
+    g_cfg.lidar_thresh      = (int)readDoubleFromFile(lidar_thresh_file);
+    g_cfg.lidar_pre         = (int)readDoubleFromFile(lidar_pre_file);
+    g_cfg.lidar_near        = (int)readDoubleFromFile(lidar_near_file);
+    g_cfg.lidar_far         = (int)readDoubleFromFile(lidar_far_file);
+    g_cfg.lidar_near_start  = (int)readDoubleFromFile(lidar_near_start_file);
+    g_cfg.lidar_near_end    = (int)readDoubleFromFile(lidar_near_end_file);
+    g_cfg.lidar_far_span    = (int)readDoubleFromFile(lidar_far_span_file);
+    g_cfg.lidar_min_frames  = (int)readDoubleFromFile(lidar_min_frames_file);
+    g_cfg.lidar_avoid_gain  = readDoubleFromFile(lidar_avoid_gain_file);
+    g_cfg.lidar_avoid_range = (int)readDoubleFromFile(lidar_avoid_range_file);
+    g_cfg.lidar_speed       = readDoubleFromFile(lidar_speed_file);
+    g_cfg.lidar_hold_frames = (int)readDoubleFromFile(lidar_hold_frames_file);
+    g_cfg.lidar_enable      = (int)readDoubleFromFile(lidar_enable_file);
 }

src/image_cv.cpp

@@ -30,6 +30,9 @@ cv::Mat track;
 std::vector<int> left_line;
 std::vector<int> right_line;
 std::vector<int> mid_line;
+std::vector<int> mid_line_raw;
+
+int g_lost_rows = 0;
 
 int line_tracking_width;                 // 处理宽度 = 80
 int line_tracking_height;                // 处理高度 = 60
@@ -58,29 +61,19 @@ int line_tracking_height;                // 处理高度 = 60
 // ============================================================
 cv::Mat image_binerize(cv::Mat &frame)
 {
-    cv::Mat output;
+    static cv::Mat output;
-    cv::Mat binarizedFrame;
+    static cv::Mat binarizedFrame;
-    cv::Mat hsvImage;
+    static cv::Mat hsvImage;
+    static std::vector<cv::Mat> hsvChannels(3);
 
-    // 1. BGR → HSV：分离色调(H)、饱和度(S)、明度(V) 三个通道
     cv::cvtColor(frame, hsvImage, cv::COLOR_BGR2HSV);
-
-    std::vector<cv::Mat> hsvChannels;
     cv::split(hsvImage, hsvChannels);
-    // hsvChannels[0] = H (色调, 0~180)
-    // hsvChannels[1] = S (饱和度, 0~255)
-    // hsvChannels[2] = V (明度, 0~255)
 
-    // 2. H 通道 Otsu 二值化：区分蓝色与非蓝色
     cv::threshold(hsvChannels[0], binarizedFrame,
                   0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
-
-    // 3. S 通道 Otsu 二值化：区分高饱和(蓝色赛道)与低饱和(灰色路面)
     cv::threshold(hsvChannels[1], output,
                   0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
 
-    // 4. 双通道融合：任一通道认为"非赛道"就排除
-    //    bitwise_or: 两幅图同为赛道(255)才保留，否则变为背景(0)
     cv::bitwise_or(output, binarizedFrame, output);
 
     return output;
@@ -111,15 +104,12 @@ cv::Mat image_binerize(cv::Mat &frame)
 // ============================================================
 cv::Mat find_road(cv::Mat &frame)
 {
-    // 1. 形态学开运算去噪
+    static cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3, 3));
-    //    MORPH_CROSS: 十字形结构元素，2×2
-    //    MORPH_OPEN: 先腐蚀(去除小白点) 再膨胀(恢复区域尺寸)
-    static cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2));
     cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel);
 
-    // 2. 创建洪泛填充蒙版
+    static cv::Mat mask;
-    //    尺寸比原图各边大 2 像素 (floodFill 要求)
+    if (mask.empty()) mask.create(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1);
-    cv::Mat mask = cv::Mat::zeros(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1);
+    mask.setTo(0);
 
     // 3. 种子点位置
     //    X = 图像水平中心 (line_tracking_width/2)
@@ -143,10 +133,9 @@ cv::Mat find_road(cv::Mat &frame)
     cv::floodFill(morphologyExFrame, mask, seedPoint, newVal,
                   nullptr, loDiff, upDiff, 8);
 
-    // 7. 从蒙版提取赛道区域
+    static cv::Mat outputImage;
-    //    mask 的外扩边框(±1) 用于 floodFill 的内部计算，实际区域在(1,1)起
+    if (outputImage.empty()) outputImage.create(line_tracking_height, line_tracking_width, CV_8UC1);
-    //    ROI 裁掉边框后即为赛道主体蒙版
+    outputImage.setTo(0);
-    cv::Mat outputImage = cv::Mat::zeros(line_tracking_height, line_tracking_width, CV_8UC1);
     mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage);
 
     return outputImage;
@@ -167,13 +156,23 @@ cv::Mat find_road(cv::Mat &frame)
 // ============================================================
 void image_main()
 {
-    cv::Mat resizedFrame;
+    static cv::Mat resizedFrame;
 
-    // ── 1. 降采样 ──────────────────────────────────────
-    // 320×240 → 80×60，大幅缩减计算量
     cv::resize(raw_frame, resizedFrame,
                cv::Size(line_tracking_width, line_tracking_height));
 
+    // ── 1.5 CLAHE 亮度归一化 → 压制反光 ────────────
+    // 先转 LAB, 对 L 通道做局部直方图均衡, 再转回 BGR
+    // 作用: 亮斑被压暗, 暗区被提亮, 赛道颜色更均匀
+    static cv::Mat labFrame;
+    static std::vector<cv::Mat> labCh(3);
+    static cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(2.0, cv::Size(8, 8));
+    cv::cvtColor(resizedFrame, labFrame, cv::COLOR_BGR2Lab);
+    cv::split(labFrame, labCh);
+    clahe->apply(labCh[0], labCh[0]);
+    cv::merge(labCh, labFrame);
+    cv::cvtColor(labFrame, resizedFrame, cv::COLOR_Lab2BGR);
+
     // ── 2. HSV 双通道 Otsu 二值化 ─────────────────────
     // 赛道区域 = 白色(255)，背景/边界 = 黑色(0)
     binarizedFrame = image_binerize(resizedFrame);
@@ -261,11 +260,14 @@ void image_main()
     }
 
     // ── 6. 中线计算 + 丢线补全 ─────
-    for (int row = line_tracking_height - 1; row >= 10; --row)
+    // 从倒数第二行开始向上，底行(row=height-1)单独兜底避免 mid_line[row+1] 越界
+    g_lost_rows = 0;
+    for (int row = line_tracking_height - 2; row >= 10; --row)
     {
         // ── 6a. 丢线补全 ──────────────────────────────
         if (left_line[row] == -1 && right_line[row] == -1)
         {
+            g_lost_rows++;
             // 当前行完全丢线: 用下行(row+1)的中线来虚拟补线
             mid_line[row] = mid_line[row + 1];
 
@@ -290,4 +292,19 @@ void image_main()
             mid_line[row] = (left_line[row] + right_line[row]) / 2;
         }
     }
+    // 底行兜底：正常计算，丢线时用图像中心
+    {
+        const int row = line_tracking_height - 1;
+        if (left_line[row] == -1 && right_line[row] == -1)
+        {
+            g_lost_rows++;
+            mid_line[row] = line_tracking_width / 2;
+            left_line[row]  = 0;
+            right_line[row] = line_tracking_width - 1;
+        }
+        else
+        {
+            mid_line[row] = (left_line[row] + right_line[row]) / 2;
+        }
+    }
 }

src/model_v10.cpp

@@ -305,7 +305,7 @@ static void model_compute() {
                  nullptr, nullptr, 15, 20, 64, 9, 1, 1, 1, false);
 }
 
-static int decode(DetectBoxV10* boxes, int max_boxes, float thr) {
+static int decode(DetectBoxV10* boxes, int max_boxes, const float* thresh) {
     const int N = OH * OW;
     int cnt = 0;
 
@@ -323,6 +323,7 @@ static int decode(DetectBoxV10* boxes, int max_boxes, float thr) {
     }
 
     for (int c = 0; c < NC && cnt < max_boxes; ++c) {
+        float thr = thresh ? thresh[c] : 0.6f;
         for (int gy = 0; gy < OH; ++gy) {
             for (int gx = 0; gx < OW; ++gx) {
                 const int idx = gy * OW + gx;
@@ -453,7 +454,7 @@ bool model_v10_init(const char* path) {
 }
 
 int model_v10_detect_fmt(const uint8_t* img, int w, int h, MildPixFmt fmt,
-                    DetectBoxV10* boxes, int max_boxes, float thr) {
+                    DetectBoxV10* boxes, int max_boxes, const float* thresh) {
     if (!g_rdy || !img || !boxes || max_boxes <= 0 || w <= 1 || h <= 1) return 0;
 
     const uint8_t* net_in = img;
@@ -463,7 +464,7 @@ int model_v10_detect_fmt(const uint8_t* img, int w, int h, MildPixFmt fmt,
     }
     preproc_160(net_in, fmt);
     model_compute();
-    const int n = decode(boxes, max_boxes, thr);
+    const int n = decode(boxes, max_boxes, thresh);
 
     const float sx = (float)w / (float)IN_W;
     const float sy = (float)h / (float)IN_H;
@@ -475,8 +476,7 @@ int model_v10_detect_fmt(const uint8_t* img, int w, int h, MildPixFmt fmt,
 }
 
 int model_v10_detect(const uint8_t* img, int w, int h, DetectBoxV10* boxes, int max_boxes, const float* thresh) {
-    float thr = thresh ? thresh[0] : 0.6f;
+    return model_v10_detect_fmt(img, w, h, MILD_FMT_BGR, boxes, max_boxes, thresh);
-    return model_v10_detect_fmt(img, w, h, MILD_FMT_BGR, boxes, max_boxes, thr);
 }
 
 const float* model_v10_forward_debug(const float* chw) {

src/vl53l0x.cpp

@@ -2,71 +2,173 @@
 #include <fcntl.h>
 #include <unistd.h>
 #include <sys/ioctl.h>
+#include <linux/i2c-dev.h>
 #include <cstring>
-#include <cerrno>
 #include <cstdio>
+#include <cerrno>
 
-#define VL53L0X_IOCTL_INIT    _IO('p', 0x01)
+extern "C" {
-#define VL53L0X_IOCTL_STOP    _IO('p', 0x05)
+#include "vl53l0x_api.h"
-#define VL53L0X_IOCTL_GETDATA _IOR('p', 0x0b, VL53L0X_RangingMeasurementData_t)
+}
 
-VL53L0X::VL53L0X() : fd(-1) {}
+static void unbind_kernel_driver()
+{
+	FILE *f = fopen("/sys/bus/i2c/drivers/stmvl53l0/unbind", "w");
+	if (f) {
+		fprintf(f, "1-0029");
+		fclose(f);
+	}
+}
+
+VL53L0X::VL53L0X()
+	: fd(-1), initialized(false)
+{
+	memset(&dev, 0, sizeof(dev));
+}
 
 VL53L0X::~VL53L0X()
 {
-    if (fd > 0)
+	stop();
-    {
-        stop();
-        close(fd);
-        fd = -1;
-    }
 }
 
 bool VL53L0X::init()
 {
-    fd = open("/dev/stmvl53l0x_ranging", O_RDWR | O_SYNC);
+	unbind_kernel_driver();
-    if (fd <= 0)
+	usleep(100000);
-    {
-        fprintf(stderr, "[VL53L0X] open failed: %s\n", strerror(errno));
-        return false;
-    }
 
-    ioctl(fd, VL53L0X_IOCTL_STOP, nullptr);
+	fd = open("/dev/i2c-1", O_RDWR);
+	if (fd < 0) {
+		fprintf(stderr, "[VL53L0X] open /dev/i2c-1 failed: %s\n", strerror(errno));
+		return false;
+	}
 
-    if (ioctl(fd, VL53L0X_IOCTL_INIT, nullptr) < 0)
+	if (ioctl(fd, I2C_SLAVE, 0x29) < 0) {
-    {
+		fprintf(stderr, "[VL53L0X] I2C_SLAVE failed: %s\n", strerror(errno));
-        fprintf(stderr, "[VL53L0X] init failed: %s\n", strerror(errno));
+		close(fd);
-        close(fd);
+		fd = -1;
-        fd = -1;
+		return false;
-        return false;
+	}
-    }
 
-    fprintf(stderr, "[VL53L0X] init OK\n");
+	dev.I2cDevAddr = 0x29;
-    return true;
+	dev.i2c_fd = fd;
+
+	VL53L0X_Error Status = VL53L0X_DataInit(&dev);
+	if (Status != VL53L0X_ERROR_NONE) {
+		fprintf(stderr, "[VL53L0X] DataInit failed: %d\n", Status);
+		close(fd);
+		fd = -1;
+		return false;
+	}
+
+	Status = VL53L0X_StaticInit(&dev);
+	if (Status != VL53L0X_ERROR_NONE) {
+		fprintf(stderr, "[VL53L0X] StaticInit failed: %d\n", Status);
+		close(fd);
+		fd = -1;
+		return false;
+	}
+
+	uint32_t refSpadCount;
+	uint8_t isApertureSpads;
+	uint8_t VhvSettings;
+	uint8_t PhaseCal;
+
+	VL53L0X_PerformRefCalibration(&dev, &VhvSettings, &PhaseCal);
+	VL53L0X_PerformRefSpadManagement(&dev, &refSpadCount, &isApertureSpads);
+
+	Status = VL53L0X_SetDeviceMode(&dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
+	if (Status != VL53L0X_ERROR_NONE) {
+		fprintf(stderr, "[VL53L0X] SetDeviceMode failed: %d\n", Status);
+		close(fd);
+		fd = -1;
+		return false;
+	}
+
+	VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&dev, 20000);
+
+	uint32_t actualBudget = 0;
+	VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&dev, &actualBudget);
+	fprintf(stderr, "[VL53L0X] timing budget = %u us\n", actualBudget);
+
+	initialized = true;
+	fprintf(stderr, "[VL53L0X] init OK\n");
+	return true;
 }
 
 bool VL53L0X::readRange(VL53L0X_RangingMeasurementData_t &data)
 {
-    if (fd <= 0)
+	if (!initialized) return false;
-        return false;
 
-    if (ioctl(fd, VL53L0X_IOCTL_GETDATA, &data) < 0)
+	VL53L0X_Error Status;
-    {
+
-        fprintf(stderr, "[VL53L0X] read failed: %s\n", strerror(errno));
+	VL53L0X_ClearInterruptMask(&dev, 0);
-        return false;
+	Status = VL53L0X_StartMeasurement(&dev);
-    }
+	if (Status != VL53L0X_ERROR_NONE) {
-    return true;
+		fprintf(stderr, "[VL53L0X] StartMeasurement failed: %d\n", Status);
+		return false;
+	}
+
+	uint8_t ready = 0;
+	int timeout = 500;
+	while (!ready && --timeout > 0) {
+		Status = VL53L0X_GetMeasurementDataReady(&dev, &ready);
+		if (Status != VL53L0X_ERROR_NONE)
+			break;
+		if (!ready)
+			usleep(1000);
+	}
+
+	if (!ready) {
+		fprintf(stderr, "[VL53L0X] measurement timeout\n");
+		return false;
+	}
+
+	Status = VL53L0X_GetRangingMeasurementData(&dev, &data);
+	if (Status != VL53L0X_ERROR_NONE) {
+		fprintf(stderr, "[VL53L0X] GetRangingMeasurementData failed: %d\n", Status);
+		return false;
+	}
+
+	VL53L0X_ClearInterruptMask(&dev, 0);
+	return true;
+}
+
+bool VL53L0X::startMeasure()
+{
+	if (!initialized) return false;
+	VL53L0X_ClearInterruptMask(&dev, 0);
+	VL53L0X_Error Status = VL53L0X_StartMeasurement(&dev);
+	return (Status == VL53L0X_ERROR_NONE);
+}
+
+bool VL53L0X::readResult(VL53L0X_RangingMeasurementData_t &data)
+{
+	if (!initialized) return false;
+
+	uint8_t ready = 0;
+	VL53L0X_Error Status;
+	int timeout = 25;  // 25ms max, 读不到就跳过
+	while (!ready && --timeout > 0) {
+		Status = VL53L0X_GetMeasurementDataReady(&dev, &ready);
+		if (Status != VL53L0X_ERROR_NONE)
+			return false;
+		if (!ready)
+			usleep(1000);
+	}
+
+	if (!ready) return false;
+
+	Status = VL53L0X_GetRangingMeasurementData(&dev, &data);
+	VL53L0X_ClearInterruptMask(&dev, 0);
+	return (Status == VL53L0X_ERROR_NONE);
 }
 
 bool VL53L0X::stop()
 {
-    if (fd <= 0)
+	if (fd >= 0) {
-        return false;
+		close(fd);
-
+		fd = -1;
-    if (ioctl(fd, VL53L0X_IOCTL_STOP, nullptr) < 0)
+	}
-    {
+	initialized = false;
-        fprintf(stderr, "[VL53L0X] stop failed: %s\n", strerror(errno));
+	return true;
-        return false;
-    }
-    return true;
 }

src/vl53l0x_platform_user.cpp

@@ -0,0 +1,159 @@
+extern "C" {
+#include "vl53l0x_platform.h"
+}
+
+#include <unistd.h>
+#include <cstring>
+
+VL53L0X_Error VL53L0X_LockSequenceAccess(VL53L0X_DEV Dev)
+{
+	(void)Dev;
+	return VL53L0X_ERROR_NONE;
+}
+
+VL53L0X_Error VL53L0X_UnlockSequenceAccess(VL53L0X_DEV Dev)
+{
+	(void)Dev;
+	return VL53L0X_ERROR_NONE;
+}
+
+VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index,
+	uint8_t *pdata, uint32_t count)
+{
+	if (count >= VL53L0X_MAX_I2C_XFER_SIZE)
+		return VL53L0X_ERROR_INVALID_PARAMS;
+
+	uint8_t buf[VL53L0X_MAX_I2C_XFER_SIZE + 1];
+	buf[0] = index;
+	memcpy(buf + 1, pdata, count);
+
+	if (write(Dev->i2c_fd, buf, count + 1) != (ssize_t)(count + 1))
+		return VL53L0X_ERROR_CONTROL_INTERFACE;
+
+	return VL53L0X_ERROR_NONE;
+}
+
+VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index,
+	uint8_t *pdata, uint32_t count)
+{
+	if (count >= VL53L0X_MAX_I2C_XFER_SIZE)
+		return VL53L0X_ERROR_INVALID_PARAMS;
+
+	if (write(Dev->i2c_fd, &index, 1) != 1)
+		return VL53L0X_ERROR_CONTROL_INTERFACE;
+
+	if (read(Dev->i2c_fd, pdata, count) != (ssize_t)count)
+		return VL53L0X_ERROR_CONTROL_INTERFACE;
+
+	return VL53L0X_ERROR_NONE;
+}
+
+VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
+{
+	return VL53L0X_WriteMulti(Dev, index, &data, 1);
+}
+
+VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data)
+{
+	uint8_t buf[2];
+	buf[0] = (data >> 8) & 0xFF;
+	buf[1] = data & 0xFF;
+	return VL53L0X_WriteMulti(Dev, index, buf, 2);
+}
+
+VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
+{
+	uint8_t buf[4];
+	buf[0] = (data >> 24) & 0xFF;
+	buf[1] = (data >> 16) & 0xFF;
+	buf[2] = (data >> 8) & 0xFF;
+	buf[3] = data & 0xFF;
+	return VL53L0X_WriteMulti(Dev, index, buf, 4);
+}
+
+VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data)
+{
+	return VL53L0X_ReadMulti(Dev, index, data, 1);
+}
+
+VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data)
+{
+	uint8_t buf[2];
+	VL53L0X_Error Status = VL53L0X_ReadMulti(Dev, index, buf, 2);
+	*data = ((uint16_t)buf[0] << 8) | buf[1];
+	return Status;
+}
+
+VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data)
+{
+	uint8_t buf[4];
+	VL53L0X_Error Status = VL53L0X_ReadMulti(Dev, index, buf, 4);
+	*data = ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) |
+			((uint32_t)buf[2] << 8) | buf[3];
+	return Status;
+}
+
+VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index,
+	uint8_t AndData, uint8_t OrData)
+{
+	uint8_t data;
+	VL53L0X_Error Status = VL53L0X_RdByte(Dev, index, &data);
+	if (Status != VL53L0X_ERROR_NONE)
+		return Status;
+	data = (data & AndData) | OrData;
+	return VL53L0X_WrByte(Dev, index, data);
+}
+
+VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev)
+{
+	(void)Dev;
+	usleep(1000);
+	return VL53L0X_ERROR_NONE;
+}
+
+/* string/internal stubs — never called but needed for linking */
+extern "C" {
+
+VL53L0X_Error VL53L0X_get_device_info(VL53L0X_DEV Dev, VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
+{
+	(void)Dev; (void)pVL53L0X_DeviceInfo;
+	return VL53L0X_ERROR_NONE;
+}
+
+VL53L0X_Error VL53L0X_get_device_error_string(VL53L0X_DeviceError ErrorCode, char *pDeviceErrorString)
+{
+	(void)ErrorCode; (void)pDeviceErrorString;
+	return VL53L0X_ERROR_NONE;
+}
+
+VL53L0X_Error VL53L0X_get_range_status_string(uint8_t RangeStatus, char *pRangeStatusString)
+{
+	(void)RangeStatus; (void)pRangeStatusString;
+	return VL53L0X_ERROR_NONE;
+}
+
+VL53L0X_Error VL53L0X_get_pal_error_string(VL53L0X_Error PalErrorCode, char *pPalErrorString)
+{
+	(void)PalErrorCode; (void)pPalErrorString;
+	return VL53L0X_ERROR_NONE;
+}
+
+VL53L0X_Error VL53L0X_get_pal_state_string(VL53L0X_State PalStateCode, char *pPalStateString)
+{
+	(void)PalStateCode; (void)pPalStateString;
+	return VL53L0X_ERROR_NONE;
+}
+
+VL53L0X_Error VL53L0X_get_sequence_steps_info(VL53L0X_SequenceStepId SequenceStepId, char *pSequenceStepsString)
+{
+	(void)SequenceStepId; (void)pSequenceStepsString;
+	return VL53L0X_ERROR_NONE;
+}
+
+VL53L0X_Error VL53L0X_get_limit_check_info(VL53L0X_DEV Dev, uint16_t LimitCheckId, char *pLimitCheckString)
+{
+	(void)Dev; (void)LimitCheckId; (void)pLimitCheckString;
+	return VL53L0X_ERROR_NONE;
+}
+
+}