diff --git a/AGENTS.md b/AGENTS.md index d61aa1e..e6950e6 100644 --- a/AGENTS.md +++ b/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,30 +17,33 @@ 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) | -| `lib/` | Public headers only (no .cpp) | -| `main/` | Entry point → executable `smartcar_demo1` | -| `docs/` | Architecture docs (`ARCHITECTURE.md` is the design reference) | +| `src/` | Source (.cpp + .hpp) → compiled into `common_lib` (static lib) | +| `lib/` | Public headers (.h only) + `vl53l0x/` C API sources | +| `main/` | Entry points → `smartcar_demo1` (main) + `lidar_test` (standalone test) | +| `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 Controlled via `ctl.sh` (writes to text files, no CLI args): @@ -48,106 +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. **Note**: `start.sh` references the binary as `smartcar_demo` but the actual executable is `smartcar_demo1` — this is a known bug, use `ctl.sh` instead. +**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 -- `./speed` (double) — target speed (duty cycle %) -- `./deadband` — steering deadband (pixels) -- `./steer_gain` — steering gain multiplier -- `./center_bias` — midline offset correction -- `./foresee` — look-ahead row for steering - -### Debug / Display -- `./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) +Key groups: +- **Steering**: `speed`, `deadband`, `steer_gain`, `center_bias`, `foresee` +- **Debug**: `start` (0/1), `showImg` (0/1), `debug` (0/1), `destfps`, `saveImg` +- **Zebra**: `zebrasee` +- **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` +- **Curve slowdown**: `curve_slope` (default 0.4), `curve_min` (default 0.8) +- **Encoder brake**: `brake_scale`, `brake_max` +- **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` ## Architecture -### Per-frame pipeline in `CameraHandler()` (`src/camera.cpp:578`) +### Per-frame pipeline in `CameraHandler()` (`src/camera.cpp:755`) -CameraHandler 现已拆分为多个函数，主函数 ~40 行仅做编排： - -```cpp +``` CameraHandler() -├── capture_frame() // 1. MJPG 取帧 → 1/4解码 320×240 BGR +├── capture_frame() // 1. MJPG 取帧 → 1/4解码 320×240 BGR ├── save_image_if_requested() // 2. 保存帧 (debug) -├── image_main() // 3. 视觉巡线 (80×60 HSV-Otsu-FloodFill) -├── run_model_inference() // 4. Mild v12 每2帧推理 (160×120, 4类) -├── zebra_process() // 5a. 斑马线去抖 + Z_STOP(4s)/Z_COOLDOWN(5s) -├── traffic_light_process() // 5b. 红绿灯 TL_NORMAL→TL_STOP→TL_WAIT_GREEN -├── cone_detect_and_deform() // 5c. 锥桶检测 + 中线变形 + 保持衰减 -├── steering_update() // 6. 舵机比例控制 (deadband过滤) -├── motor_update() // 7. 电机开环PWM + 编码器刹车 + 弯道减速 -├── lcd_render() // 8. LCD RGB565渲染 (边界线+检测框+状态) -└── fps_log() // 9. 每15帧打印分步耗时 +├── image_main() // 3. 视觉巡线 (80×60 HSV-Otsu-FloodFill) +├── lidar_avoid_process() // 4. VL53L0X 激光避障 (每2帧) + 中线变形 +├── run_model_inference() // 5. Mild v12 每2帧推理 (160×120, 4类) +├── zebra_process() // 6a. 斑马线去抖 + Z_STOP/Z_COOLDOWN +├── traffic_light_process() // 6b. 红绿灯 TL_NORMAL→TL_STOP→TL_WAIT_GREEN +├── cone_detect_and_deform() // 6c. 锥桶检测 + 中线变形 + 保持衰减 +├── 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. -- **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`): 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. -- **Curve slowdown**: `speed *= (1.0 - |deviation| × 0.4)`, clamped to ≥ 60%. +### Motor control + +- **Open-loop**: `MotorController::updateduty(spd)` — direct PWM, no encoder feedback in normal driving. +- **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 (cone / red light / green light / zebra) + background -- **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) +- **Input**: 160×120 BGR, **Output**: 4 classes + background +- **Class indices**: 0=cone, 1=red light, 2=green light, 3=zebra +- **Per-class thresholds**: `{0.65, 0.75, 0.80, 0.82}` (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) - -### 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 +- **Inference engine**: `src/model_v10.{hpp,cpp}` (file names say v10, internally Mild v3 — historical naming) ## Style and conventions -- C++ files have **no copyright headers** — comments are ascii-box-style block comments when present -- Header guards use `#ifndef FILENAME_H_` / `#define FILENAME_H_` format (with some exceptions: `#pragma once` in model files) -- Global state uses `extern` globals (e.g., `g_cfg`, `g_steer_deviation`, `g_boxes`) -- `lib/` contains .h files only; `src/` contains .cpp files only -- No unit tests, no CI, no linting — this is embedded code tested on-device +- No copyright headers — comments use ascii-box-style block dividers +- Header guards: `#ifndef FILENAME_H_` / `#define FILENAME_H_` (some model files use `#pragma once`) +- Global state via `extern` globals (e.g., `g_cfg`, `g_steer_deviation`, `g_boxes`) +- `lib/` has `.h` headers; `src/` has `.cpp` implementations + `.hpp` model headers +- No unit tests, no CI, no linting — do not add comments to code unless asked diff --git a/CMakeLists.txt b/CMakeLists.txt index ca73f0b..df9e927 100644 --- a/CMakeLists.txt +++ b/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/ilikara/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}") diff --git a/ctl.sh b/ctl.sh index f1e08c0..5e4d78d 100644 --- a/ctl.sh +++ b/ctl.sh @@ -36,12 +36,14 @@ init_pins() { echo 8 > "$DIR/deadband" 2>/dev/null echo 1.5 > "$DIR/steer_gain" 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 diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md index 5b5bf62..36f6654 100644 --- a/docs/ARCHITECTURE.md +++ b/docs/ARCHITECTURE.md @@ -34,13 +34,12 @@ ``` main() ├── cfg_load_all() # 从工作目录文件读取全部配置 - ├── 补读 cone_avoid_gain/range/hold_frames - ├── CameraInit(0) # 打开摄像头, LCD, 模型, I2C, 计算巡线尺寸 + ├── CameraInit(0) # 打开摄像头, LCD, 模型, I2C, VL53L0X, 计算巡线尺寸 ├── ControlInit() # 初始化双电机 GPIO + PWM + 启动编码器线程 │ └── while(running): ├── CameraHandler() # ★ 逐帧执行 - └── target_speed = g_cfg.speed # (debug 模式下每 7 帧重载) + └── target_speed = g_cfg.speed # (debug 模式下每 30 帧重载) ``` `CameraInit` 现在只接受 `camera_id` 一个参数（之前有 dest_fps, width, height 三个遗留参数已移除）。 @@ -48,12 +47,12 @@ main() --- -## CameraHandler 11 步流水线 +## CameraHandler 10 步流水线 (`src/camera.cpp:755`) ``` -Step 1 capture_frame() 640×480 MJPEG → IMREAD_REDUCED_COLOR_4 - 取帧+解码 → raw_frame = 160×120 BGR - (回退: raw_mat 三通道 → 直用 640×480) +Step 1 capture_frame() 640×480 MJPEG → IMREAD_REDUCED_COLOR_4 + 取帧+解码 → raw_frame = 160×120 BGR + (回退: raw_mat 三通道 → 直用 640×480) Step 2 save_image_if_requested() debug=1 && saveImg 文件=1 → 存 ./image/XXXXX.jpg 保存图像 (条件) @@ -62,36 +61,43 @@ Step 3 image_main() raw_frame → resize(lt_w×lt_h) → HSV 双 视觉巡线 → floodFill(种子 (lt_w/2, lt_h-10), 半径5) → 逐行最长连续段搜索 → 丢线补全(row 10~59) 输出: left_line[], right_line[], mid_line[] + 随后: mid_line_raw = mid_line (原始中线快照) -Step 4 run_model_inference() 每 2 帧推理一次 +Step 4 lidar_avoid_process() 每 2 帧读距 → 距离→行映射 → 去抖确认 + 激光避障 确认后中线变形 (固定朝右) + 消失后保持衰减 + 方向判断基于 mid_line_raw, 变形修改 mid_line + +Step 5 run_model_inference() 每 2 帧推理一次 模型推理 raw_frame (160×120) → model_v10_detect() 4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线 - g_thresh = {0.90, 0.75, 0.80, 0.90} + g_thresh = {0.65, 0.75, 0.80, 0.82} → g_boxes[16], g_box_count -Step 5a zebra_process() 去抖计数 + ZNORMAL→ZSTOP(4s)→ZCOOLDOWN(5s) +Step 6a zebra_process() 去抖计数 + ZNORMAL→ZSTOP(4s)→ZCOOLDOWN(5s) 斑马线状态机 触发: 连续5帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee) 刹停时 I2C 语音播报 -Step 5b traffic_light_process() TLNORMAL→TLSTOP→TLWAIT_GREEN +Step 6b traffic_light_process() TLNORMAL→TLSTOP→TLWAIT_GREEN 红绿灯状态机 红灯≥3帧 → 停车; 红灯消失 → 等绿灯; 绿灯≥3帧 → 通行 -Step 5c cone_detect_and_deform() 去抖确认 + 中线变形 + 消失后保持衰减 - 锥桶检测 & 中线变形 cone_speed 减速 + cone_hold_frames 保持 +Step 6c cone_detect_and_deform() 去抖确认 + 中线变形 + 消失后保持衰减 + 回弹 + 锥桶检测 & 中线变形 cone_hold_frames 保持 → cone_return_frames 回弹 -Step 6 steering_update() foresee/2 → check_row → mid_line[row]×2 - newWidth/2 +Step 7 steering_update() foresee/2 → check_row → mid_line[row]×2 - newWidth/2 舵机控制 deadband 过滤 → servo duty: 1500000 ± offset ns -Step 7 motor_update() 开环PWM + 编码器比例刹车 + 弯道减速 + 锥桶减速 - 电机控制 +Step 8 motor_update() 开环PWM + 编码器比例刹车 + 弯道减速 + 电机控制 (锥桶/挡板减速代码存在但已注释) -Step 8 lcd_render() track→BGR→ROI + 边界线(红/绿/蓝) + 检测框 + 状态指示 +Step 9 lcd_render() track→BGR→ROI + 边界线(红/绿/蓝) + 检测框 + 状态指示 LCD 渲染 RGB565 → /dev/fb0 mmap -Step 9 fps_log() 每 15 帧输出总帧率 + 分步耗时 ms +Step 10 fps_log() 分步耗时日志 FPS 统计 ``` +另：每 150 帧调用 `malloc_trim(0)` 回收空闲内存（嵌入式优化）。 + --- ## 视觉巡线深度展开 (`image_main`) @@ -183,9 +189,10 @@ ControlUpdate(speed, zebra_or_tl_block): motor[i]->updateduty(0) ← 已静止，不刹车 return - curve = 1.0 - |g_steer_deviation| × 0.4 ← 弯道减速 - curve = max(curve, 0.6) ← 最低 ≥ 60% + 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 方向) @@ -196,8 +203,8 @@ ControlUpdate(speed, zebra_or_tl_block): 刹车力度与当前车速成正比（`brake_scale`），有上限（`brake_max` ns）。 速度低于 0.5 pps 时不再施加刹车。 -**motor_update 额外逻辑：** `motor_update()` 在调用 `ControlUpdate` 之前，若锥桶已触发（`cone_is_slow()`）， -会将 speed 乘以 `g_cfg.cone_speed`（默认 0.5），实现锥桶路段减速。 +**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），函数调用完全相同。 @@ -347,28 +354,43 @@ lcd_render(): |---|---|---|---|---| | `./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 | 3 | 3 | 锥桶连续确认帧数 | +| `./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 | 30 | 30 | 锥桶消失后保持变形帧数 | +| `./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` 写入的值覆盖代码默认值，是实际运行参数。 @@ -380,12 +402,12 @@ lcd_render(): | 文件 | 功能 | 排除原因 | |---|---|---| -| `vl53l0x.cpp` | 激光测距模块 | 硬件未连接 | | `zebra_detect.cpp` | 经典斑马线检测 | 已被 Mild 模型替代 | | `PIDController.cpp` | 位置式/增量式 PID | 电机开环直驱，无调用点 | | `serial.cpp` | VOFA 串口可视化 | 未使用 | -**注意：** `PIDController.h` 仍在 `lib/` 中，`MotorController` 仅提供 `updateduty()`（开环占空比）， +**注意：** `vl53l0x.cpp` 已恢复编译（激光测距已集成至 `lidar_avoid_process()`）。 +`PIDController.h` 仍在 `lib/` 中，`MotorController` 仅提供 `updateduty()`（开环占空比）， 不提供 `updateSpeed()` 方法。 --- @@ -396,7 +418,7 @@ lcd_render(): - **输入**：160×120 BGR - **输出**：4 类 + 背景 → 9 通道：0=锥桶, 1=红灯, 2=绿灯, 3=斑马线 - **架构**：3→9→9→13→13→19→19→19→44→44→64→64 → head(64→64,dw) → out(64→9) -- **阈值**：`{0.90, 0.75, 0.80, 0.90}`（锥桶/红灯/绿灯/斑马线） +- **阈值**：`{0.65, 0.75, 0.80, 0.82}`（锥桶/红灯/绿灯/斑马线） - **权重文件**：`mild_v12.bin`（105KB 自定义二进制格式） - **推理频率**：每 2 帧一次（跳帧节省 CPU） diff --git a/docs/CONE_DESIGN.md b/docs/CONE_DESIGN.md index d8ca5a0..0987423 100644 --- a/docs/CONE_DESIGN.md +++ b/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) ``` -**关键差异：** 去抖只决定"是否触发变形"，变形本身没有持续时间概念。 -- 锥桶在 → 变形在（每帧重新计算 mid_line 变形） -- 锥桶消失 → 下帧 image_main() 重新计算 mid_line → 变形自动消失 -- 不去抖时的抖动：锥桶在边界附近闪烁会使 mid_line 来回跳，用去抖平滑 +**cone_min_frames 默认值为 1（单帧即确认）。** 原因：视觉巡线（`image_main()` 的 floodFill） +对赛道上的锥桶响应极快——锥桶进入视野后 1~2 帧内车就开始转弯，锥桶随即离开视野。 +模型检测的窗口天然只有 1~2 帧，多帧去抖门槛（如 3）永远达不到。 +边界过滤（row 有效、赛道内、conf ≥ cone_thresh）已经提供了足够的误触发保护。 --- -## 7. 舵机与电机 +## 7. 保持衰减 + 回弹 -**舵机：** 零改动。Step 6 读取 `mid_line[foresee/2]`，看到变形后的中线值，自然输出偏转方向。 +锥桶消失后不会立刻恢复正常巡线，而是经历三个阶段： -**电机：** 推入期间减速： ``` -if (g_cone_confirmed && g_zstate != Z_STOP) { - effective_speed = target_speed * cone_speed_factor; -} else { - effective_speed = target_speed; +确认 (g_cone_confirmed && cone_seen) + → g_cone_hold_ctr = 1, g_cone_return_ctr = 0 + → 冻结位置: hold_src_row/col = cone_row/col + → 记录回弹方向: 锥桶在中线左侧 → 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 行感受越强) | - -### 8.2 热更新参数 - -| 文件 | 类型 | 默认 | 含义 | -|---|---|---|---| -| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 | -| `./cone_min_frames` | int | 3 | 连续确认帧数 | +| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 (当前减速代码已注释) | +| `./cone_min_frames` | int | 1 | 确认帧数 (1=单帧即确认) | | `./cone_margin` | int | 0 | 0=中心点过滤, 1=框边缘过滤 | | `./cone_thresh` | double | 0.80 | 锥桶置信度阈值 (业务层, 不改 NMS) | - -### 8.3 读取实现 - -**main.cpp 启动时（一次性）：** -```cpp -double avoid_gain_val = readDoubleFromFile("./cone_avoid_gain"); -int avoid_range_val = (int)readDoubleFromFile("./cone_avoid_range"); -if (avoid_gain_val <= 0) avoid_gain_val = 0.3; -if (avoid_range_val <= 0) avoid_range_val = 30; -g_cfg.cone_avoid_gain = avoid_gain_val; -g_cfg.cone_avoid_range = avoid_range_val; -``` - -**global.cpp (cfg_load_all，热更新)：** -```cpp -g_cfg.cone_speed = readDoubleFromFile(cone_speed_file); -g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file); -g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file); -g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_file); -``` - -**ctl.sh：** -```sh -echo 0.3 > "$DIR/cone_avoid_gain" 2>/dev/null -echo 30 > "$DIR/cone_avoid_range" 2>/dev/null -echo 0.5 > "$DIR/cone_speed" 2>/dev/null -echo 3 > "$DIR/cone_min_frames" 2>/dev/null -echo 0 > "$DIR/cone_margin" 2>/dev/null -echo 0.80 > "$DIR/cone_thresh" 2>/dev/null -``` +| `./cone_hold_frames` | int | 15 | 消失后保持变形帧数 | +| `./cone_return_gain` | double | 1.0 | 回弹力度 (1.0=等同避让力度) | +| `./cone_return_frames` | int | 30 | 回弹持续帧数 | --- -## 9. 代码修改清单 +## 10. 代码位置 ``` +lib/image_cv.h: + └── extern mid_line_raw (原始中线快照声明) + +src/image_cv.cpp: + └── std::vector mid_line_raw (定义) + lib/global.h: - ├── CfgCache 新增: cone_avoid_gain, cone_avoid_range, - │ cone_speed, cone_min_frames, cone_margin, cone_thresh - └── 新增文件名常量 + └── CfgCache 包含: cone_avoid_gain, cone_avoid_range, cone_speed, + 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) - -main/main.cpp: - └── CameraInit 之前: 读取 cone_avoid_gain, cone_avoid_range 写入 g_cfg + └── cfg_load_all() 统一读取所有锥桶参数 src/camera.cpp: - ├── #define CONE_CLASS 0 - ├── 新增静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed - ├── Step 5 之后: 插入锥桶检测+边界过滤+去抖 - ├── Step 5.5: 中线变形 (修改 mid_line[row] for row in [10, cone_row]) - ├── Step 7 电机: g_cone_confirmed → effective_speed *= cone_speed_factor - └── Step 8 LCD: 锥桶框改橙色 + ├── CameraHandler(): image_main() 后执行 mid_line_raw = mid_line (快照) + ├── 静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed, + │ g_cone_hold_ctr, g_cone_hold_src_row/col, + │ g_cone_return_ctr, g_cone_return_dir + ├── cone_is_slow(): 判断是否处于避让/保持/回弹任一阶段 + ├── 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 回弹 → 正常 | diff --git a/docs/ENCODER_BRAKE.md b/docs/ENCODER_BRAKE.md index 3a68b33..8088f41 100644 --- a/docs/ENCODER_BRAKE.md +++ b/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。 - -## 3. 刹车状态机 +`ControlInit()` 启动独立线程 `encoder_thread()`（`src/control.cpp:30`），持续运行直到 `ControlExit()`。 ``` - ┌──────────┐ - 正常行驶 ───→ │ IDLE │ - └────┬─────┘ - │ zebra_block==true - ▼ - ┌──────────┐ - │ BRAKING │ ← 施加 -30% 占空比 - │ │ 每帧读 GPIO 67 - └────┬─────┘ - ┌───────────┴───────────┐ - │ 脉冲变化 │ 脉冲不变 - ▼ ▼ - 轮子在转 → 继续刹车 g_brake_still++ - 重置计数器 │ - 连续 5 帧无变化？ - 或超时 60 帧？ - │ - ▼ - ┌──────────┐ - │ HOLDING │ ← duty=0，保持静止 - └──────────┘ - │ - zebra_block==false - │ - ▼ - ┌──────────┐ - │ IDLE │ - └──────────┘ +encoder_thread(): + 高频轮询 GPIO67 (LSB 边沿) + GPIO72 (方向) + 轮询间隔: 500µs sleep (~2kHz) + + 每 100ms 窗口: + g_enc_speed = pulse_cnt / elapsed_time (脉冲/秒, 原子变量) + g_enc_dir = GPIO72 当前值 (原子变量) + pulse_cnt 归零, 开始新窗口 ``` -## 4. 参数 +通过 sysfs 文件描述符轮询（`GPIO::readValue()`），`lseek` + `read`。不依赖中断。 -定义在 `src/control.cpp`： +## 3. 触发条件 -| 常量 | 值 | 含义 | -|---|---|---| -| `BRAKE_STILL_THRESH` | 5 | 连续无脉冲帧数阈值，超过即判停 | -| `BRAKE_TIMEOUT` | 60 | 最长刹车帧数（~2s @30fps），强制释放 | -| `BRAKE_DUTY` | -30 | 刹车占空比（-100~100，负值=反转） | +`ControlUpdate(speed, block)` 中 `block == true` 时进入刹车流程。 -## 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） -- 锥桶避障不使用编码器 -- `g_cfg.start == 0` 时刹车状态机重置 +| 文件 | 类型 | 默认 | 含义 | +|---|---|---|---| +| `./brake_scale` | double | 10 | 速度(pps) → 刹车占空比(ns) 缩放系数 | +| `./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) - └── if (zebra_block) → 刹车状态机 - └── encoderLSB.readValue() ← GPIO 67 + ├── zebra_process() → zebra_block + ├── traffic_light_process() → tl_block + └── 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 ``` diff --git a/docs/LIDAR_AVOID.md b/docs/LIDAR_AVOID.md index 81e7d62..b7698ae 100644 --- a/docs/LIDAR_AVOID.md +++ b/docs/LIDAR_AVOID.md @@ -1,232 +1,130 @@ # 激光雷达挡板避障设计 -## 问题 +## 概述 -VL53L0X 是单点 ToF 激光测距传感器，仅返回沿光束方向的一个距离值（mm），没有扇形扫描能力。 -当激光测到近处有物体时，无法直接从距离值判断它是： +VL53L0X 单点 ToF 激光测距传感器，返回沿光束方向的距离值（mm）。 +当距离低于阈值时触发绕行：中线变形推向右侧，消失后保持衰减。 -- A) **挡板/障碍物** — 横在赛道上的物体，需要绕行 -- B) **赛道边墙** — 弯道处车头对准了侧墙，这是正常行驶状态 - -**解决思路：用视觉巡线的边界线（left_line / right_line）来区分 A 和 B。** +**当前实现：** 纯距离触发 + 预触发去抖，不检查视觉边界线。绕行方向固定朝右。 --- -## 关键约束：挡板会导致丢线 +## VL53L0X 驱动集成 -挡板立在赛道上时，不仅触发激光近距读数，还会**遮挡摄像头视野中的赛道边界**， -导致从挡板所在位置开始边界线丢失（left_line / right_line = -1）。 +驱动已编译（`src/vl53l0x.cpp` + `lib/vl53l0x/` C API），在 `CameraInit()` 中初始化： ``` -Camera 俯视视角 (图像坐标): - row 0 (远) : ░░░░░░░ ← 被挡板挡住，丢线 - row 15 : ░░░░░░░ ← 挡板上方，丢线 - row 25 : ░░░░░░░ ← 挡板顶部附近，丢线 - row 30 : ███████ ← ★ 挡板所在行，边界线丢失 - row 35 : ■■■■■■■ ← 挡板下方，赛道可见，边界线有效 - row 59 (近) : ■■■■■■■ ← 车前方，赛道清晰 +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，在视觉巡线之后、模型推理之前： ``` - Camera + Laser - ｜ (高度 H, 俯角 θ) - ｜╲ - ｜ ╲ laser beam - ｜ ╲ - ground ──────────────┴────███████──── 挡板 at distance d_mm - (挡板后方赛道被遮挡) +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 之上) ``` -- 激光光束沿车体正前方（图像中轴线） -- 距离 d_mm 越小 → 物体越近 → 映射到图像中越靠下的行（row 大） -- 距离 d_mm 越大 → 物体越远 → 映射到图像中越靠上的行（row 小） +lidar 先于 cone 修改 mid_line，优先级更高。cone 的 clamp 确保不超出边界。 --- ## 核心算法 -### 第一步：读取激光距离 +### 1. 读取激光距离（每 2 帧一次） ``` -d_mm = vl53l0x.readRange().RangeMilliMeter +lidar_avoid_process(): + if !g_lidar_ok || !g_cfg.lidar_enable → return + if ++lidar_skip < 2 → return // 每 2 帧一读 (~15Hz) + lidar_skip = 0 -if d_mm >= LIDAR_THRESHOLD_MM: - return CLEAR // 远处无障碍，不做任何处理 + data = g_lidar_sensor.readResult() // 非阻塞读取上次测量结果 + d_mm = data.RangeMilliMeter + g_lidar_sensor.startMeasure() // 立即启动下次测量 (后台 ~20ms) ``` -`LIDAR_THRESHOLD_MM` 是触发阈值。只有距离小于此值才进入判定。建议默认 ~300mm。 - -### 第二步：距离 → 图像行映射 +### 2. 预触发去抖 ``` -// 线性模型: -// row = lt_h-1 (底行, 最近) 对应 D_NEAR -// row = 10 (最远有效行) 对应 D_FAR -// clamp 到 [10, lt_h-1] + if d_mm == 0: // 读失败, 跳过 + skip -row = lt_h - 1 - (d_mm - D_NEAR) / (D_FAR - D_NEAR) * (lt_h - 11) -row = clamp(row, 10, lt_h - 1) + 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）才真正确认。 -| 参数 | 含义 | 建议初值 | -|------|------|----------| -| `D_NEAR` | row = lt_h-1 对应的物理距离 | 50 mm | -| `D_FAR` | row = 10 对应的物理距离 | 1200 mm | - -标定方法：在赛道前方 300mm、600mm、900mm 处各放一个挡板，记录图像中挡板出现的 row，线性拟合。 - -### 第三步：用边线判定障碍物（核心） +### 3. 距离 → 图像行映射 ``` -// 设 row = distance_to_row(d_mm)，即激光测距对应的图像行 - -// ── 3a. 检查"挡板下方"（更近处）：赛道应该仍可见 ── -near_valid = false -near_ref_row = -1 -for r = min(row + NEAR_START, lt_h-1) down to max(row + NEAR_END, lt_h-1): - if left_line[r] != -1 && right_line[r] != -1: - near_valid = true - near_ref_row = r // 记录有效行，后续绕行时判断宽侧 - break - -// ── 3b. 检查"挡板位置及上方"（更远处）：边界应该已丢失 ── -far_lost = false -for r = row down to max(row - FAR_SPAN, 10): - if left_line[r] == -1 || right_line[r] == -1: - far_lost = true - break - -// ── 3c. 联合判定 ── -if near_valid && far_lost: - → OBSTACLE_CONFIRMED - 记录: g_lidar_obstacle_row = row - g_lidar_ref_row = near_ref_row // 用于判断绕行方向 -else: - → CLEAR + 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. 触发判定 ``` -场景 A: 挡板挡路 → 触发绕行 - d_mm = 300mm → row = 35 - row 36~39 (近处): ✓ 赛道可见 - row 30~35 (挡板处): ✗ 丢线 (挡板遮挡) - → near_valid=true, far_lost=true → ★ 触发 + g_lidar_frames++ + g_lidar_obstacle_row = row -场景 B: 正常直道，无障碍 - d_mm = 1200mm → row = 10, 且 d_mm > THRESHOLD - → 不进入判定，直接 CLEAR - -场景 C: 弯道，激光打到边墙 - d_mm = 200mm → row = 40 - row 41~44 (近处): ✓ 赛道可见 - row 37~40 (弯道处): ✓ 赛道也可能可见（边墙不一定导致丢线） - → near_valid=true, far_lost=false → CLEAR + 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 OBSTACLE_CONFIRMED: - g_lidar_frames++ -else: - g_lidar_frames = max(0, g_lidar_frames - 1) - -g_lidar_confirmed = (g_lidar_frames >= LIDAR_MIN_FRAMES) + 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 ~ 障碍行施加变形： ``` -// 在 near_ref_row（挡板下方最近的有效行）中判断左右空间 -left_space = mid_line[near_ref_row] - left_line[near_ref_row] -right_space = right_line[near_ref_row] - mid_line[near_ref_row] + decay = (确认中) 1.0 : (1.0 - hold_ctr / hold_frames) // 保持期线性衰减 -// 往宽侧推 -dir = (left_space > right_space) ? +1.0 : -1.0 -``` - -### 中线变形 - -对从 row=10 到挡板位置 row 的所有行施加变形，变形量从远到近线性斜坡上升： - -``` -// gain = lidar_avoid_gain (推离力度, 归一化) -// range = lidar_avoid_range (斜坡陡峭度, 行数) -// half_w = lt_w / 2 - -for r = 10 to g_lidar_obstacle_row: - t = clamp((r - 10) / range, 0.0, 1.0) // 斜率上升 - push = t * gain * half_w * dir - mid_line[r] = clamp(mid_line[r] + push, - left_line[r] + 2.0, - right_line[r] - 2.0) -``` - -**变形示意图：** - -``` - row 10 ───────●──────────────────────────○ 赛道中心线 - row 20 ────────●─────────────────────────○ (往右绕行) - row 30 ───────────●──────────────────────○ - row 35 ──────────────●───────────────────○ ← 挡板位置 - row 40 ────────────────████████████──────── ← 挡板 (丢线) - row 59 ─■────────●───■■■■■■■■■■■■■■───●───■ ← 车底 (近处可见) - left mid 方向盘往右打 right -``` - -### 速度联动 - -绕行时降速以保证安全： - -``` -if g_lidar_confirmed || g_lidar_hold_ctr > 0: - spd *= lidar_speed // 默认 0.4，即降至 40% 速度 -``` - -### 消失后保持衰减 - -挡板不再被检测到后，变形不会立即消失，而是线性衰减（类似锥桶 hold 逻辑）： - -``` -// 确认状态 -if g_lidar_confirmed: - g_lidar_hold_ctr = 0 - g_lidar_hold_src_row = g_lidar_obstacle_row - g_lidar_hold_dir = dir - -// 衰减状态（挡板消失后） -else if g_lidar_hold_src_row > 0 && g_lidar_hold_ctr < lidar_hold_frames: - g_lidar_hold_ctr++ - decay = 1.0 - (double)g_lidar_hold_ctr / lidar_hold_frames // 线性衰减到 0 - - for r = 10 to g_lidar_hold_src_row: - t = clamp((r - 10) / range, 0.0, 1.0) - push = t * gain * half_w * g_lidar_hold_dir * decay - mid_line[r] = clamp(mid_line[r] + push, left+2, right-2) + 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) ``` --- @@ -237,107 +135,53 @@ else if g_lidar_hold_src_row > 0 && g_lidar_hold_ctr < lidar_hold_frames: ┌────────────────────────┐ │ │ ▼ │ - ┌──────┐ 确认 ┌──────┐ 消失 ┌──────────┐ 保持结束 - │NORMAL│ ─────► │AVOID │ ─────► │HOLD_DECAY│ ────────► NORMAL - │ │ │ │ │ (衰减) │ - └──────┘ └──────┘ └──────────┘ - ▲ │ - └────────────────────────────────┘ 测距恢复正常 + ┌──────┐ 确认 ┌──────┐ 消失 ┌──────────┐ 保持结束 + │NORMAL│ ─────► │AVOID │ ─────► │HOLD_DECAY│ ────────► NORMAL + │ │ │ │ │ (衰减) │ + └──────┘ └──────┘ └──────────┘ + ▲ │ + └────────────────────────────────┘ 测距恢复正常 -NORMAL: 正常行驶，激光测距 > THRESHOLD -AVOID: 挡板确认 → 中线变形绕行 + 减速 × lidar_speed +NORMAL: 正常行驶，激光测距 > lidar_pre +AVOID: 挡板确认 → 中线变形绕行 (固定朝右) HOLD_DECAY: 挡板消失 → 变形量线性衰减 (lidar_hold_frames 帧内归零) 衰减期间若再次检测到挡板 → 立即切回 AVOID ``` -| 状态 | 电机 | 舵机 | 说明 | -|------|------|------|------| -| NORMAL | 正常速度 | 正常巡线 | 无变形 | -| AVOID | speed × lidar_speed | 中线偏转绕行 | 基于 left/right 空间判断方向 | -| HOLD_DECAY | speed × lidar_speed | 变形量线性衰减 | 确保车完全通过后再恢复 | - --- -## 与现有流水线的集成 +## 速度联动 -`lidar_avoid_process()` 放在 cone_detect_and_deform 之前执行（两者都修改 mid_line， -lidar 优先级更高）： - -``` -// CameraHandler 中 Step 5: -bool zebra_block = zebra_process(); -bool tl_block = traffic_light_process(); -lidar_avoid_process(); // ★ 新增：直接修改 mid_line -cone_detect_and_deform(); - -steering_update(); -motor_update(zebra_block, tl_block); -``` - -### 中线修改的优先级 - -lidar 和 cone 都会修改 `mid_line[]`。由于 lidar 绕行是结构性避障（避开整个挡板）， -应**先执行 lidar 变形，再执行 cone 变形**。 -cone 的 clamp 到 `[left+2, right-2]` 会确保不超出 lidar 变形后的安全区间。 - -### motor_update 速度联动 - -```cpp -// motor_update 中新增： -if (g_lidar_confirmed || g_lidar_hold_ctr > 0) { - spd *= g_cfg.lidar_speed; -} -``` - -不通过 `block` 参数刹车（绕行不需要停车），而是降速 + 变形。 - ---- - -## 复用现有 VL53L0X 驱动 - -已有驱动（当前被 `CMakeLists.txt` 排除编译）： - -```cpp -// lib/vl53l0x.h -VL53L0X sensor; -sensor.init(); // open /dev/stmvl53l0x_ranging -VL53L0X_RangingMeasurementData_t data; -sensor.readRange(data); // data.RangeMilliMeter -sensor.stop(); -``` - -**集成时需做的事：** -1. 从 `CMakeLists.txt` 的 EXCLUDE 列表中移除 `vl53l0x.cpp` -2. 在 `CameraInit()` 中调用 `sensor.init()`（硬件未连接时优雅降级） -3. 在 `cameraDeInit()` 中调用 `sensor.stop()` +`motor_update()` 中包含挡板减速代码（`lidar_is_active()` → `speed × lidar_speed`）， +但当前**已注释**。绕行仅靠中线变形，不减速。如需启用，取消注释即可。 --- ## 配置参数 -| 文件 | 类型 | 建议默认 | 含义 | -|------|------|----------|------| -| `./lidar_thresh` | int | 300 | 障碍判定距离阈值 (mm)，低于此值触发判定 | +| 文件 | 类型 | 默认 | 含义 | +|------|------|------|------| +| `./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_near_start` | int | 1 | near_valid 检测起始偏移 (row + N) | -| `./lidar_near_end` | int | 4 | near_valid 检测终止偏移 | -| `./lidar_far_span` | int | 6 | far_lost 检测跨度 (从 row 往上检查 N 行) | | `./lidar_min_frames` | int | 3 | 连续确认帧数 | | `./lidar_avoid_gain` | double | 0.4 | 绕行中线推离力度 (归一化) | | `./lidar_avoid_range` | int | 30 | 绕行斜坡陡峭度 (行数) | -| `./lidar_speed` | double | 0.4 | 绕行时速度倍率 | +| `./lidar_speed` | double | 0.4 | 绕行时速度倍率 (当前未生效) | | `./lidar_hold_frames` | int | 30 | 挡板消失后变形保持帧数 | -| `./lidar_enable` | int(0/1) | 1 | 激光避障总开关（0=禁用） | +| `./lidar_near_start` | int | 1 | (已加载但未使用，预留) | +| `./lidar_near_end` | int | 4 | (已加载但未使用，预留) | +| `./lidar_far_span` | int | 6 | (已加载但未使用，预留) | --- ## 边界情况 & 注意事项 -1. **激光硬件未连接** — `sensor.init()` 失败时，`lidar_avoid_process()` 直接返回 false，不阻塞正常行驶。 -2. **弯道边墙误判** — 急弯处赛道边墙可能导致激光近距 + 边线丢失同时出现。依赖 `lidar_thresh` 和 `lidar_min_frames` 调参抑制。误判时车会短暂绕向一侧，弯道通过后立即恢复。 -3. **左右空间相等** — `left_space == right_space` 时默认 `dir = -1.0`（往右绕），可通过配置 `lidar_default_dir` 调整。 -4. **上坡/下坡** — 车辆俯仰变化会影响距离→行的映射精度。建议在平坦路段标定。 -5. **多传感器优先级** — lidar 先于 cone 修改 mid_line。zebra/tl 的 block 刹车优先级最高（红灯/斑马线停车不绕行）。 -6. **首次集成建议** — 先调通数据采集：打印 `d_mm`、对应 `row`、以及 `near_ref_row` 处的 `left/right/mid` 值，跑几圈确认标定参数无误后再开启绕行。 -7. **挡板材质** — 深色挡板不会被 HSV-Otsu 判为赛道（白色 255），但仍会遮挡背景赛道导致丢线。判定逻辑依赖的是**丢线**而非**像素颜色**。 +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 刹车优先级最高（红灯/斑马线停车不绕行）。 diff --git a/docs/TRAFFIC_LIGHT.md b/docs/TRAFFIC_LIGHT.md index de87184..42b33f7 100644 --- a/docs/TRAFFIC_LIGHT.md +++ b/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) - Step 4: 模型推理 → g_boxes[], g_box_count - Step 5: 斑马线状态机 - Step 5.5: 红绿灯状态机 - ├─ 遍历 g_boxes, 统计 red_seen/green_seen - ├─ 去抖计数 (3帧确认, 衰减-1/帧) - └─ 状态跃迁 - Step 5.6: 锥桶检测 (g_zstate!=Z_STOP && g_tl_state==TL_NORMAL 时才运行) - Step 7: 电机控制 - ControlUpdate(spd, g_zstate==Z_STOP || tl_block) +CameraHandler (src/camera.cpp:755) + Step 5: 模型推理 → g_boxes[], g_box_count + Step 6a: 斑马线状态机 + Step 6b: 红绿灯状态机 + ├─ 遍历 g_boxes, 统计 red_seen/green_seen + ├─ 去抖计数 (3帧确认, 衰减-1/帧) + └─ 状态跃迁 + Step 6c: 锥桶检测 (g_zstate!=Z_STOP && g_tl_state==TL_NORMAL 时才运行) + Step 8: 电机控制 + motor_update(zebra_block, tl_block) + → ControlUpdate(final_spd, zebra_block || tl_block) control.cpp: - ControlUpdate(speed, zebra_block) - └─ zebra_block → 编码器线程读速度 → 比例刹车 + ControlUpdate(speed, block) + └─ block → 编码器线程读速度 → 比例刹车 ``` ## 6. 状态变量 diff --git a/lib/global.h b/lib/global.h index b5e7639..9320caa 100644 --- a/lib/global.h +++ b/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,6 +29,8 @@ 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"; @@ -56,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; // 舵机增益 @@ -67,10 +73,12 @@ 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) diff --git a/lib/image_cv.h b/lib/image_cv.h index 7f0ea1f..e96c58e 100644 --- a/lib/image_cv.h +++ b/lib/image_cv.h @@ -12,5 +12,7 @@ extern cv::Mat track; extern std::vector left_line; extern std::vector right_line; extern std::vector mid_line; +extern std::vector mid_line_raw; +extern int g_lost_rows; extern int line_tracking_height, line_tracking_width; diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 3144813..5aa5d3a 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -3,4 +3,7 @@ add_executable(smartcar_demo1 main.cpp) target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS}) add_executable(lidar_test lidar_test.cpp) -target_link_libraries(lidar_test common_lib) \ No newline at end of file +target_link_libraries(lidar_test common_lib) + +add_executable(remote_control remote_control.cpp) +target_link_libraries(remote_control common_lib) \ No newline at end of file diff --git a/main/remote_control.cpp b/main/remote_control.cpp new file mode 100644 index 0000000..84a1d94 --- /dev/null +++ b/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 +#include +#include +#include + +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; +} diff --git a/src/camera.cpp b/src/camera.cpp index 6686947..25cf023 100644 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -1,3 +1,27 @@ +/* + * camera.cpp — 每帧感知-决策-执行流水线 + * + * 入口: CameraHandler() 由 main.cpp 主循环每帧调用一次。 + * + * 10 步流水线: + * 1. capture_frame MJPEG 取帧 → 1/4 解码 160×120 BGR + * 2. save_image_if_requested debug 存图 + * 3. image_main 视觉巡线 → left/right/mid_line[] + * 4. lidar_avoid_process VL53L0X 激光避障 → 中线变形 (已禁用) + * 5. zebra/tl/cone 场景识别状态机 (用上一帧 g_boxes[]) + * 6. steering_update 舵机比例控制 + * 7. motor_update 电机开环 + 编码器刹车 + 弯道减速 + * 8. run_model_inference Mild v12 每2帧推理 → g_boxes[] (放最后, 不阻塞舵机) + * 9. lcd_render LCD RGB565 渲染 + * 10. fps_log 分步耗时日志 + * + * 硬件依赖: + * - 摄像头: /dev/video0 (640×480 MJPEG, V4L2) + * - LCD: /dev/fb0 (RGB565, mmap) + * - 舵机: pwmchip1/pwm0 (3ms 周期, 1.2~1.8ms 占空比) + * - 语音: /dev/i2c-2 从地址 0x34 (斑马线播报) + * - 激光: VL53L0X via /dev/stmvl53l0x_ranging + */ #include "camera.h" #include "model_v10.hpp" #include "vl53l0x.h" @@ -18,38 +42,54 @@ #include // ═══════════════════════════════════════════════════════════ -// 模型检测类别 +// 模型检测类别索引 +// Mild v12 输出 4 类 + 背景, 对应 g_thresh[] 阈值 // ═══════════════════════════════════════════════════════════ enum { MD_CONE = 0, MD_RED = 1, MD_GREEN = 2, MD_ZEBRA = 3 }; // ═══════════════════════════════════════════════════════════ -// 摄像头 & 帧缓冲全局 +// 摄像头 & 帧缓冲全局变量 // ═══════════════════════════════════════════════════════════ -cv::VideoCapture cap; -cv::Mat raw_mat, decoded_frame; -static int g_decode_mode = -1; +cv::VideoCapture cap; // V4L2 摄像头句柄 +cv::Mat raw_mat, decoded_frame; // raw_mat=V4L2原始帧, decoded_frame=MJPEG解码结果 +static int g_decode_mode = -1; // -1=未检测, 1=MJPEG 1/4解码, 0=全尺寸回退 -int screenWidth, screenHeight, newWidth, newHeight; -int fb; -uint16_t *fb_buffer; -PwmController servo(1, 0); +int screenWidth, screenHeight; // LCD 屏幕物理分辨率 (从 /dev/fb0 读取) +int newWidth, newHeight; // 自适应显示分辨率 = 摄像头分辨率 × 缩放因子 +int fb; // /dev/fb0 文件描述符 +uint16_t *fb_buffer; // LCD 帧缓冲 mmap 地址 +PwmController servo(1, 0); // 舵机: pwmchip1/pwm0 +// 巡线空间到显示空间的缩放因子 +// line_tracking_width = newWidth / calc_scale +// line_tracking_height = newHeight / calc_scale static constexpr int calc_scale = 2; // ═══════════════════════════════════════════════════════════ -// 模型相关静态 +// 模型推理相关 +// +// g_thresh: 四类各自置信度阈值 (锥桶/红灯/绿灯/斑马线) +// g_boxes: 检测结果缓冲区 (最多 16 个框) +// g_box_count: 当前帧有效检测框数 // ═══════════════════════════════════════════════════════════ -static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.82f}; +static float g_thresh[4] = {0.80f, 0.75f, 0.80f, 0.87f}; static DetectBoxV10 g_boxes[16]; static int g_box_count = 0; // ═══════════════════════════════════════════════════════════ // 斑马线状态机 +// +// 状态: Z_NORMAL → Z_STOP(4s) → Z_COOLDOWN(5s) → Z_NORMAL +// 触发条件: 连续 5 帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee) +// +// g_zc_frames: 去抖连续帧计数 (未检测时每帧 -2 衰减) +// g_zc_min_cy: 本轮追踪中斑马线出现时的最小 cy (越小=越远) +// g_ztime: 进入 Z_STOP / Z_COOLDOWN 的时间戳 // ═══════════════════════════════════════════════════════════ -static constexpr int ZEBRA_MIN_FRAMES = 5; -static constexpr int ZEBRA_FAR_CY = 50; +static constexpr int ZEBRA_MIN_FRAMES = 5; // 连续确认帧数 +static constexpr int ZEBRA_FAR_CY = 80; // 远处 cy 阈值: 斑马线必须曾出现在 cy≤80 static int g_zc_frames = 0; -static int g_zc_min_cy = 120; +static int g_zc_min_cy = 120; // 初始设远 (120=从未检测到) enum ZState { Z_NORMAL, Z_STOP, Z_COOLDOWN }; static ZState g_zstate = Z_NORMAL; @@ -57,6 +97,12 @@ static time_t g_ztime = 0; // ═══════════════════════════════════════════════════════════ // 红绿灯状态机 +// +// 状态: TL_NORMAL → TL_STOP → TL_WAIT_GREEN → TL_NORMAL +// 红灯≥3帧确认停车, 红灯消失→等绿灯, 绿灯≥3帧确认通行 +// +// g_tl_red_frames: 红灯连续帧计数 (未检测时 -1 衰减) +// g_tl_green_frames: 绿灯连续帧计数 // ═══════════════════════════════════════════════════════════ enum TLState { TL_NORMAL, TL_STOP, TL_WAIT_GREEN }; static TLState g_tl_state = TL_NORMAL; @@ -64,7 +110,17 @@ static int g_tl_red_frames = 0; static int g_tl_green_frames = 0; // ═══════════════════════════════════════════════════════════ -// 锥桶检测 & 保持 +// 锥桶检测 & 中线变形状态 +// +// 生命周期: 确认(avoid) → 保持衰减(hold) → 回弹(return) → 空闲 +// +// g_cone_frames: 去抖连续帧计数 (未检测时 -1 衰减) +// g_cone_last_row/col: 上帧锥桶位置 (用于位置容差判定) +// g_cone_confirmed: 是否通过去抖确认 +// g_cone_hold_ctr: 保持衰减计数器 (确认时=1, 每帧++) +// g_cone_hold_src_row/col: 保持阶段使用的锥桶位置 (确认时冻结) +// g_cone_return_ctr: 回弹计数器 (保持结束后=1, 每帧++) +// g_cone_return_dir: 回弹方向 (+1=推右, -1=推左, 与避让方向相反) // ═══════════════════════════════════════════════════════════ static int g_cone_frames = 0; static int g_cone_last_row = -1; @@ -77,7 +133,19 @@ static int g_cone_return_ctr = 0; static double g_cone_return_dir = 0; // ═══════════════════════════════════════════════════════════ -// 激光雷达挡板避障 (主循环每 5 帧同步读取一次) +// 激光雷达挡板避障 +// +// 每 2 帧非阻塞读取 VL53L0X 距离, 纯距离触发(不检查视觉边线)。 +// 绕行方向固定朝右 (g_lidar_hold_dir = 1.0)。 +// +// g_lidar_sensor: VL53L0X 驱动实例 +// g_lidar_ok: 硬件是否初始化成功 +// g_lidar_frames: 去抖连续帧计数 +// g_lidar_confirmed: 是否通过去抖确认 +// g_lidar_obstacle_row: 障碍物对应的巡线空间行号 +// g_lidar_hold_src_row: 保持阶段使用的障碍物行号 (确认时冻结) +// g_lidar_hold_ctr: 保持衰减计数器 +// g_lidar_hold_dir: 绕行方向 (固定 1.0 = 朝右) // ═══════════════════════════════════════════════════════════ static VL53L0X g_lidar_sensor; static bool g_lidar_ok = false; @@ -89,20 +157,25 @@ static int g_lidar_hold_ctr = 0; static double g_lidar_hold_dir = 0; // ═══════════════════════════════════════════════════════════ -// LCD & FPS +// LCD 开关 & 舵机偏差 // ═══════════════════════════════════════════════════════════ -static bool g_lcd_on = true; -double g_steer_deviation = 0; +static bool g_lcd_on = true; // LCD 渲染开关 (每 10 帧从 g_cfg.showImg 刷新) +double g_steer_deviation = 0; // 归一化舵机偏差 [-1, 1], 供 motor_update 弯道减速用 // ═══════════════════════════════════════════════════════════ -// I2C 音频 (斑马线语音播报) +// I2C 音频 — 斑马线语音播报 +// +// 硬件: I2C-2 总线, 从地址 0x34, SMBus 块写 +// 协议: command=0x6E, data={0xFF, 0x10} → 触发语音 +// 调用时机: 斑马线状态机进入 Z_STOP 时 // ═══════════════════════════════════════════════════════════ static int i2c_audio_fd = -1; +// 打开 I2C 音频设备 (懒初始化, 只打开一次) static bool i2c_audio_open() { - if (i2c_audio_fd >= 0) return true; + if (i2c_audio_fd >= 0) return true; // 已打开 i2c_audio_fd = open("/dev/i2c-2", O_RDWR); if (i2c_audio_fd < 0) { fprintf(stderr, "[ZEBRA] 无法打开 I2C-2: %s\n", strerror(errno)); @@ -116,11 +189,13 @@ static bool i2c_audio_open() return true; } +// 触发斑马线语音播报: SMBus I2C 块写 → 音频模块 static void play_zebra_audio() { if (!i2c_audio_open()) { printf("[ZEBRA] 语音失败: I2C 未打开\n"); return; } - ioctl(i2c_audio_fd, I2C_SLAVE, 0x34); + ioctl(i2c_audio_fd, I2C_SLAVE, 0x34); // 确保从地址 + // SMBus I2C 块写: command=0x6E, data=[0xFF, 0x10] union i2c_smbus_data d; struct i2c_smbus_ioctl_data a; __u8 v[] = {0xFF, 0x10}; @@ -138,14 +213,25 @@ static void play_zebra_audio() // ═══════════════════════════════════════════════════════════ -// CameraInit / cameraDeInit +// CameraInit — 系统初始化 +// +// 初始化顺序: +// 1. 舵机 → 中位 +// 2. LCD 帧缓冲 → mmap +// 3. 摄像头 → 640×480 MJPEG, CONVERT_RGB=0 +// 4. 自适应分辨率计算 → newWidth/newHeight, line_tracking_* +// 5. Mild v12 模型加载 +// 6. I2C 音频设备 +// 7. VL53L0X 激光 (可选, lidar_enable 控制) // ═══════════════════════════════════════════════════════════ int CameraInit(int camera_id) { - servo.setPeriod(3000000); - servo.setDutyCycle(1500000); + // ── 1. 舵机初始化: 3ms 周期, 1.5ms 中位 ── + servo.setPeriod(3000000); // 3,000,000 ns = 3ms + servo.setDutyCycle(1500000); // 1,500,000 ns = 1.5ms (中位) servo.enable(); + // ── 2. LCD 帧缓冲初始化 ── fb = open("/dev/fb0", O_RDWR); if (fb == -1) { std::cerr << "无法打开帧缓冲区设备" << std::endl; return -1; } @@ -160,12 +246,15 @@ int CameraInit(int camera_id) std::cerr << "无法映射帧缓冲区到内存" << std::endl; close(fb); return -1; } + // ── 3. 摄像头初始化 ── + // CONVERT_RGB=0: 让 V4L2 返回原始 MJPEG 字节流 (单通道) + // 后续用 imdecode(IMREAD_REDUCED_COLOR_4) 做 1/4 解码 → 160×120 cap.open(0, cv::CAP_V4L2); - if (!cap.isOpened()) cap.open(0); + if (!cap.isOpened()) cap.open(0); // V4L2 失败则尝试默认后端 cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); cap.set(cv::CAP_PROP_FRAME_WIDTH, 640); cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480); - cap.set(cv::CAP_PROP_CONVERT_RGB, 0); + cap.set(cv::CAP_PROP_CONVERT_RGB, 0); // 关键: 不让 V4L2 转 RGB if (!cap.isOpened()) { printf("无法打开摄像头\n"); munmap(fb_buffer, fb_size); close(fb); return -1; @@ -175,9 +264,11 @@ int CameraInit(int camera_id) int camH = cap.get(cv::CAP_PROP_FRAME_HEIGHT); printf("摄像头分辨率: %d x %d\n", camW, camH); + // ── 4. 自适应分辨率: 在 LCD 屏幕上等比缩放 ── + // newWidth × newHeight = 摄像头画面等比放入 LCD 后的像素尺寸 double ws = (double)screenWidth / camW; double hs = (double)screenHeight / camH; - double s = std::min(ws, hs); + double s = std::min(ws, hs); // 取小边缩放因子, 保证不超出屏幕 newWidth = (int)(camW * s); newHeight = (int)(camH * s); printf("自适应分辨率: %d x %d\n", newWidth, newHeight); @@ -185,55 +276,65 @@ int CameraInit(int camera_id) double fps = cap.get(cv::CAP_PROP_FPS); printf("Camera fps: %f\n", fps); + // 巡线空间 = 显示空间 / calc_scale (典型 80×60) line_tracking_width = newWidth / calc_scale; line_tracking_height = newHeight / calc_scale; + // ── 5. 加载 Mild v12 模型 ── if (!model_v10_init("./mild_v12.bin")) printf("[MODEL] 警告: 模型加载失败\n"); else printf("[MODEL] Mild v12 模型已加载\n"); + // ── 6. I2C 音频设备 ── i2c_audio_open(); - // 只在启用时才初始化激光,避免驱动内核定时器拖慢 CPU - if (g_cfg.lidar_enable) { - g_lidar_ok = g_lidar_sensor.init(); - if (g_lidar_ok) { - printf("[LIDAR] VL53L0X 已初始化\n"); - g_lidar_sensor.startMeasure(); - } else - printf("[LIDAR] VL53L0X 未连接, 避障禁用\n"); - } else { - printf("[LIDAR] 已禁用\n"); - } + // ── 7. VL53L0X 激光测距 (可选) ── + // 仅在 lidar_enable=1 时初始化, 避免驱动内核定时器拖慢 CPU + // if (g_cfg.lidar_enable) { + // g_lidar_ok = g_lidar_sensor.init(); + // if (g_lidar_ok) { + // printf("[LIDAR] VL53L0X 已初始化\n"); + // g_lidar_sensor.startMeasure(); // 启动首次后台测量 (~20ms) + // } else + // printf("[LIDAR] VL53L0X 未连接, 避障禁用\n"); + // } else { + // printf("[LIDAR] 已禁用\n"); + // } return 0; } +// ═══════════════════════════════════════════════════════════ +// cameraDeInit — 释放所有资源 +// ═══════════════════════════════════════════════════════════ void cameraDeInit(void) { - cap.release(); + cap.release(); // 释放摄像头 struct fb_var_screeninfo vinfo; if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) != -1) { size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8; - munmap(fb_buffer, fb_size); + munmap(fb_buffer, fb_size); // 解除 LCD mmap } - close(fb); - if (i2c_audio_fd >= 0) close(i2c_audio_fd); - if (g_lidar_ok) g_lidar_sensor.stop(); - model_v10_deinit(); + close(fb); // 关闭 LCD fd + if (i2c_audio_fd >= 0) close(i2c_audio_fd); // 关闭 I2C 音频 + if (g_lidar_ok) g_lidar_sensor.stop(); // 停止 VL53L0X + model_v10_deinit(); // 释放模型内存 } // ═══════════════════════════════════════════════════════════ -// 图像保存 (debug 模式用) +// Step 2: 图像保存 (debug 模式) +// +// 条件: debug=1 且 ./saveImg 文件内容为 1 +// 保存路径: ./image/image_XXXXX.jpg (自增编号) // ═══════════════════════════════════════════════════════════ static int saved_frame_count = 0; static void save_image_if_requested() { if (!g_cfg.debug) return; - if (!readFlag(saveImg_file)) return; + if (!readFlag(saveImg_file)) return; // 每帧读 ./saveImg 文件 std::ostringstream filename; filename << "./image/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg"; if (cv::imwrite(filename.str(), raw_frame)) { @@ -244,15 +345,25 @@ static void save_image_if_requested() // ═══════════════════════════════════════════════════════════ -// 帧捕获 + 解码 +// Step 1: 帧捕获 + MJPEG 解码 +// +// 正常模式 (CONVERT_RGB=0 生效): +// V4L2 返回单通道 MJPEG → imdecode(IMREAD_REDUCED_COLOR_4) +// → 1/4 解码 → raw_frame = 160×120 BGR +// +// 回退模式 (CONVERT_RGB=0 未生效): +// V4L2 返回三通道 BGR → 直用 → raw_frame = 640×480 BGR +// +// 首帧自动检测: raw_mat.channels()==1 → 正常模式, 否则回退 // ═══════════════════════════════════════════════════════════ static int capture_frame(struct timespec *t0, struct timespec *t1) { - clock_gettime(CLOCK_MONOTONIC, t0); + clock_gettime(CLOCK_MONOTONIC, t0); // 计时: 取帧开始 cap.read(raw_mat); - clock_gettime(CLOCK_MONOTONIC, t1); + clock_gettime(CLOCK_MONOTONIC, t1); // 计时: 取帧结束 if (raw_mat.empty()) return -1; + // 首帧自动检测解码模式 if (g_decode_mode == -1) { g_decode_mode = (raw_mat.channels() == 1) ? 1 : 0; printf("[CAM] CONVERT_RGB=0 %s, mode=%d\n", @@ -261,22 +372,29 @@ static int capture_frame(struct timespec *t0, struct timespec *t1) } if (g_decode_mode == 1) { + // 正常: MJPEG 字节流 → 1/4 解码 → 160×120 BGR cv::imdecode(raw_mat, cv::IMREAD_REDUCED_COLOR_4, &decoded_frame); if (decoded_frame.empty()) return -1; raw_frame = decoded_frame; } else { + // 回退: 直接使用 640×480 BGR (性能较差) raw_frame = raw_mat; } return 0; } // ═══════════════════════════════════════════════════════════ -// 模型推理 (每 2 帧一次) +// Step 5: 模型推理 (每 2 帧一次) +// +// 输入: raw_frame (160×120 BGR) +// 输出: g_boxes[0..g_box_count-1], 每个框含 cx,cy,w,h,cls,conf +// 阈值: g_thresh = {0.80, 0.75, 0.80, 0.87} (锥桶/红灯/绿灯/斑马线) +// 跳帧: 每 2 帧推理一次, 奇数帧保留上次结果 // ═══════════════════════════════════════════════════════════ static void run_model_inference() { static int infer_skip = 0; - if (++infer_skip < 2) return; + if (++infer_skip < 2) return; // 跳帧 infer_skip = 0; g_box_count = 0; if (model_v10_ready()) { @@ -287,7 +405,24 @@ static void run_model_inference() // ═══════════════════════════════════════════════════════════ -// 斑马线检测 + 状态机 +// Step 6a: 斑马线检测 + 状态机 +// +// 检测逻辑: +// 遍历 g_boxes 找 cls=3 (斑马线), 只取第一个 (break) +// 记录 cy, 更新 g_zc_min_cy (本轮最远出现位置) +// 未检测到时 g_zc_frames 每帧 -2 衰减, 归零则重置 min_cy +// +// 触发条件 (仅 Z_NORMAL 时): +// 1. zebra_near: 当前 cy > g_cfg.zebrasee (足够近) +// 2. g_zc_frames >= 5 (连续确认) +// 3. g_zc_min_cy <= 50 (曾从远处检测到, 排除误触发) +// → 播报语音 → Z_STOP +// +// 状态转移: +// Z_NORMAL → Z_STOP (停车 4 秒) → Z_COOLDOWN (冷却 5 秒) → Z_NORMAL +// 冷却期间不检测斑马线, 防止重复触发 +// +// 返回值: true = Z_STOP (通知 motor_update 刹车) // ═══════════════════════════════════════════════════════════ static bool zebra_process() { @@ -296,42 +431,45 @@ static bool zebra_process() int zebra_cy = 0; float zebra_cf = 0; + // 遍历检测框, 找第一个斑马线 for (int i = 0; i < g_box_count; ++i) { if (g_boxes[i].cls != MD_ZEBRA) continue; zebra_cy = (int)g_boxes[i].cy; zebra_cf = g_boxes[i].conf; zebra_seen = true; - zebra_near = (zebra_cy > g_cfg.zebrasee); + zebra_near = (zebra_cy > g_cfg.zebrasee); // cy > 阈值 = 足够近 if (g_zstate == Z_NORMAL) { - if (zebra_cy < g_zc_min_cy) g_zc_min_cy = zebra_cy; + if (zebra_cy < g_zc_min_cy) g_zc_min_cy = zebra_cy; // 追踪最远 cy g_zc_frames++; } - break; + break; // 只取第一个斑马线框 } + // 未检测到 → 衰减去抖计数 if (!zebra_seen) { - if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 2); - if (g_zc_frames == 0) g_zc_min_cy = 120; + if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 2); // 每帧 -2 快速衰减 + if (g_zc_frames == 0) g_zc_min_cy = 120; // 完全衰减 → 重置远处追踪 } time_t now = time(nullptr); switch (g_zstate) { case Z_NORMAL: + // 触发: 近 + 连续帧够 + 曾从远处出现 if (zebra_near && g_zc_frames >= ZEBRA_MIN_FRAMES && g_zc_min_cy <= ZEBRA_FAR_CY) { - play_zebra_audio(); + play_zebra_audio(); // I2C 语音播报 g_zstate = Z_STOP; g_ztime = now; if (g_cfg.debug) printf("[ZEBRA] cy=%d cf=%.2f 停车4s 冷却5s\n", zebra_cy, zebra_cf); - g_zc_frames = 0; g_zc_min_cy = 120; + g_zc_frames = 0; g_zc_min_cy = 120; // 重置, 为下次检测做准备 } break; case Z_STOP: - if (now - g_ztime >= 4) { + if (now - g_ztime >= 4) { // 停车 4 秒 g_zstate = Z_COOLDOWN; g_ztime = now; if (g_cfg.debug) printf("[ZEBRA] 起步\n"); } break; case Z_COOLDOWN: - if (now - g_ztime >= 5) { + if (now - g_ztime >= 5) { // 冷却 5 秒 g_zstate = Z_NORMAL; if (g_cfg.debug) printf("[ZEBRA] 恢复\n"); } @@ -341,16 +479,27 @@ static bool zebra_process() } // ═══════════════════════════════════════════════════════════ -// 红绿灯状态机 +// Step 6b: 红绿灯状态机 +// +// 去抖: 红灯/绿灯各自独立计数, 连续 3 帧确认, 未检测时 -1 衰减 +// +// 状态转移: +// TL_NORMAL → TL_STOP (红灯连续 ≥3 帧) +// TL_STOP → TL_WAIT_GREEN (红灯消失) +// TL_WAIT_GREEN → TL_NORMAL (绿灯连续 ≥3 帧) +// +// 返回值: true = 非 TL_NORMAL (通知 motor_update 刹车) // ═══════════════════════════════════════════════════════════ static bool traffic_light_process() { + // 遍历检测框, 统计红/绿灯是否出现 bool red_seen = false, green_seen = false; for (int i = 0; i < g_box_count; ++i) { if (g_boxes[i].cls == MD_RED) red_seen = true; if (g_boxes[i].cls == MD_GREEN) green_seen = true; } + // 去抖: 出现 +1, 消失 -1 if (red_seen) g_tl_red_frames++; else g_tl_red_frames = std::max(0, g_tl_red_frames - 1); if (green_seen) g_tl_green_frames++; @@ -358,21 +507,21 @@ static bool traffic_light_process() switch (g_tl_state) { case TL_NORMAL: - if (g_tl_red_frames >= 3) { + if (g_tl_red_frames >= 3) { // 红灯确认 → 停车 g_tl_state = TL_STOP; if (g_cfg.debug) printf("[TL] 红灯停车\n"); } break; case TL_STOP: - if (!red_seen) { + if (!red_seen) { // 红灯消失 → 等绿灯 g_tl_state = TL_WAIT_GREEN; if (g_cfg.debug) printf("[TL] 等待绿灯\n"); } break; case TL_WAIT_GREEN: - if (g_tl_green_frames >= 3) { + if (g_tl_green_frames >= 3) { // 绿灯确认 → 通行 g_tl_state = TL_NORMAL; - g_tl_red_frames = g_tl_green_frames = 0; + g_tl_red_frames = g_tl_green_frames = 0; // 清零, 重新开始 if (g_cfg.debug) printf("[TL] 绿灯通行\n"); } break; @@ -381,7 +530,17 @@ static bool traffic_light_process() } // ═══════════════════════════════════════════════════════════ -// 激光雷达挡板避障 +// Step 4: 激光雷达挡板避障 +// +// lidar_is_active(): 判断是否处于确认或保持衰减状态 +// +// lidar_avoid_process() 流程: +// 1. 每 2 帧非阻塞读取 VL53L0X (readResult + startMeasure) +// 2. 预触发去抖: d_mm >= lidar_pre → 帧数衰减; d_mm == 0 → 跳过 +// 3. 距离→行映射: 线性映射 [lidar_near, lidar_far] → [lt_h-1, 10] +// 4. 触发判定: d_mm < lidar_thresh && frames >= min_frames → 确认 +// 5. 保持衰减: 确认后 hold_ctr 递增, 超过 hold_frames 则失效 +// 6. 中线变形: 固定朝右推, 斜坡从 row=10 上升到障碍行, 衰减渐弱 // ═══════════════════════════════════════════════════════════ static bool lidar_is_active() { return g_lidar_confirmed || @@ -390,70 +549,77 @@ static bool lidar_is_active() { static void lidar_avoid_process() { - // 每 2 帧一读 (15Hz), 预触发 + // ── 跳帧 + 前置检查 ── static int lidar_skip = 0; - if (!g_lidar_ok || !g_cfg.lidar_enable) return; - if (++lidar_skip < 2) return; + if (!g_lidar_ok || !g_cfg.lidar_enable) return; // 硬件未就绪或已禁用 + if (++lidar_skip < 2) return; // 每 2 帧一次 (~15Hz) lidar_skip = 0; + // ── 非阻塞读取上次测量结果, 立即启动下次测量 ── VL53L0X_RangingMeasurementData_t data; - if (!g_lidar_sensor.readResult(data)) return; + if (!g_lidar_sensor.readResult(data)) return; // 读取失败 → 跳过 int d_mm = data.RangeMilliMeter; - g_lidar_sensor.startMeasure(); + g_lidar_sensor.startMeasure(); // 后台测量 ~20ms - // 超出预触发窗口 → 减帧(不归零), 传感器0不计数 + // ── 预触发去抖窗口 ── int lt_h = line_tracking_height; bool skip = false; if (d_mm == 0) { - skip = true; // 读失败, 本帧不动 + skip = true; // 传感器读数为 0 = 读失败 } else if (d_mm >= g_cfg.lidar_pre) { - if (g_lidar_frames > 0) g_lidar_frames--; + if (g_lidar_frames > 0) g_lidar_frames--; // 超出窗口 → 衰减 (不归零) skip = true; } if (!skip) { - // 距离 → 图像行 + // ── 距离 → 图像行映射 (线性) ── + // lidar_near(50mm) → row=lt_h-1 (最近), lidar_far(1200mm) → row=10 (最远) int row = lt_h - 1 - (d_mm - g_cfg.lidar_near) * (lt_h - 11) / (g_cfg.lidar_far - g_cfg.lidar_near); if (row < 10) row = 10; if (row >= lt_h) row = lt_h - 1; - // 纯距离触发, 不看赛道线 + // ── 纯距离触发, 不检查视觉边界线 ── g_lidar_frames++; g_lidar_obstacle_row = row; g_lidar_confirmed = (d_mm < g_cfg.lidar_thresh) && (g_lidar_frames >= g_cfg.lidar_min_frames); static int dbg = 0; - if (g_cfg.debug && ++dbg >= 5) { dbg = 0; + if (g_cfg.debug && ++dbg >= 30) { dbg = 0; printf("[LIDAR] d=%d row=%d frames=%d confirm=%d\n", d_mm, row, g_lidar_frames, g_lidar_confirmed); } + // ── 确认 → 记录保持状态 ── 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_hold_dir = 1.0; // 固定朝右绕行 if (g_cfg.debug) printf("[LIDAR] 触发 d=%d row=%d dir=右\n", d_mm, g_lidar_obstacle_row); - g_lidar_frames = 0; + g_lidar_frames = 0; // 重置计数 } } + // ── 保持计数器递增 (确认中和消失后都递增) ── hold_decay: if (g_lidar_hold_src_row > 0 && g_lidar_hold_ctr <= g_cfg.lidar_hold_frames) g_lidar_hold_ctr++; - if (!lidar_is_active()) return; + if (!lidar_is_active()) return; // 非活跃 → 不变形 - // 中线变形 — 固定朝右 + // ── 中线变形: 固定朝右推 ── + // 从 row=10 到障碍行, 斜坡线性上升; 障碍行以下维持最大推力 + // 消失后 decay 线性衰减到 0 int src_row = g_lidar_hold_src_row; - double rng = (double)g_cfg.lidar_avoid_range; + double rng = (double)g_cfg.lidar_avoid_range; // 斜坡陡峭度 double half_w = line_tracking_width / 2.0; - double dir = g_lidar_hold_dir; + double dir = g_lidar_hold_dir; // 1.0 = 朝右 double decay = g_lidar_confirmed ? 1.0 : (1.0 - (double)g_lidar_hold_ctr / g_cfg.lidar_hold_frames); for (int i = 10; i < lt_h; ++i) { + // 障碍行及以上: 斜坡上升; 障碍行以下: 满推力 double t = (i <= src_row) ? std::clamp((i - 10) / rng, 0.0, 1.0) : 1.0; double push = t * g_cfg.lidar_avoid_gain * half_w * dir * decay; double nm = std::clamp(mid_line[i] + push, @@ -464,19 +630,34 @@ hold_decay: } // ═══════════════════════════════════════════════════════════ -// 锥桶检测 + 中线变形 +// Step 6c: 锥桶检测 + 中线变形 +// +// cone_is_slow(): 判断是否处于避让/保持/回弹任一阶段 +// (供 motor_update 减速用, 但减速代码当前已注释) +// +// cone_detect_and_deform() 流程: +// 1. 仅在 Z_NORMAL && TL_NORMAL 时运行 +// 2. 遍历 g_boxes 找 cls=0 (锥桶), 置信度 ≥ cone_thresh +// 3. 模型坐标→巡线空间, 边界过滤 (row 有效、赛道内) +// 4. cone_margin: 0=用中心点判断, 1=用框左右边缘判断 +// 5. 取 cy 最大(最近)的锥桶 +// 6. 去抖: 位置容差内连续帧计数, 突变则重计 +// 7. 确认 → 保持衰减 → 回弹, 三阶段生命周期 +// 8. 避让阶段: 推离锥桶方向的中线变形 (row 10 ~ 锥桶行) +// 9. 回弹阶段: 朝锥桶方向反推 (帮助车回到中央) // ═══════════════════════════════════════════════════════════ static bool cone_is_slow() { return g_cone_confirmed || (g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) || - (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_hold_frames / 3); + (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_return_frames); } static void cone_detect_and_deform() { + // 斑马线停车或红绿灯非正常时, 不做锥桶检测 if (g_zstate == Z_STOP || g_tl_state != TL_NORMAL) return; - // ── 找最近锥桶 ── + // ── 1. 找最近锥桶 (cy 最大 = 最近) ── bool cone_seen = false; int cone_row = -1, cone_col = -1; @@ -484,81 +665,126 @@ static void cone_detect_and_deform() if (g_boxes[i].cls != MD_CONE) continue; if (g_boxes[i].conf < g_cfg.cone_thresh) continue; + // 模型空间 → 巡线空间坐标映射 int cl = g_boxes[i].cx * line_tracking_width / raw_frame.cols; int rl = g_boxes[i].cy * line_tracking_height / raw_frame.rows; + // 边界过滤: row 0~9 无可靠巡线数据 if (rl < 10 || rl >= line_tracking_height) continue; - if (left_line[rl] == -1 || right_line[rl] == -1) continue; + if (left_line[rl] == -1 || right_line[rl] == -1) continue; // 丢线行 + // 赛道内判断 if (g_cfg.cone_margin) { + // margin=1: 用框的左右边缘判断是否在赛道内 int bl = (g_boxes[i].cx - g_boxes[i].w/2) * line_tracking_width / raw_frame.cols; int br = (g_boxes[i].cx + g_boxes[i].w/2) * line_tracking_width / raw_frame.cols; - if (br < left_line[rl] || bl > right_line[rl]) continue; + if (br < left_line[rl] || bl > right_line[rl]) continue; // 框完全在赛道外 } else { - if (cl < left_line[rl] || cl > right_line[rl]) continue; + // margin=0: 用中心点判断 + if (cl < left_line[rl] || cl > right_line[rl]) continue; // 中心在赛道外 } + // 取 cy 最大的 (最近的锥桶) if (rl > cone_row) { cone_row = rl; cone_col = cl; } cone_seen = true; } - // ── 去抖确认 ── + // ── 2. 去抖确认 ── { + // 位置容差: 动态计算, 典型 row_tol=5, col_tol=10 int row_tol = std::max(2, line_tracking_height / 12); int col_tol = std::max(3, line_tracking_width / 8); if (cone_seen) { if (g_cone_frames == 0) { + // 首次出现 → 记录位置, 开始计数 g_cone_last_row = cone_row; g_cone_last_col = cone_col; g_cone_frames = 1; } else if (std::abs(cone_row - g_cone_last_row) <= row_tol && std::abs(cone_col - g_cone_last_col) <= col_tol) { + // 位置接近 → 继续计数 g_cone_frames++; g_cone_last_row = cone_row; g_cone_last_col = cone_col; } else { + // 位置突变 → 视为新锥桶, 重新开始计数 g_cone_frames = 1; g_cone_last_row = cone_row; g_cone_last_col = cone_col; } } else { + // 未检测到 → 衰减 if (g_cone_frames > 0) g_cone_frames = std::max(0, g_cone_frames - 1); } } g_cone_confirmed = (g_cone_frames >= g_cfg.cone_min_frames); - // ── 保持/衰减 ── + static int cdbg = 0; + if (g_cfg.debug && ++cdbg >= 15) { cdbg = 0; + printf("[CONE] frames=%d confirm=%d hold=%d return=%d\n", + g_cone_frames, g_cone_confirmed, g_cone_hold_ctr, g_cone_return_ctr); + } + + // ── 3. 三阶段生命周期: 确认 → 保持衰减 → 回弹 ── if (g_cone_confirmed) { - g_cone_hold_ctr = 0; - g_cone_hold_src_row = cone_row; - g_cone_hold_src_col = cone_col; - g_cone_return_ctr = 0; - } else if (g_cone_hold_src_row > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) { - g_cone_hold_ctr++; - if (g_cone_hold_ctr > g_cfg.cone_hold_frames) { - double ms = mid_line[g_cone_hold_src_row]; - g_cone_return_dir = (g_cone_hold_src_col < ms) ? -1.0 : 1.0; - g_cone_return_ctr = 1; + g_cone_hold_ctr = 1; + g_cone_return_ctr = 0; + // 只在锥桶实际可见时更新冻结位置和回弹方向, + // 防止 cone_row=-1 覆盖有效值 + mid_line[-1] 越界 + if (cone_seen) { + g_cone_hold_src_row = cone_row; + g_cone_hold_src_col = cone_col; + double ms = mid_line_raw[cone_row]; + g_cone_return_dir = (cone_col < ms) ? -1.0 : 1.0; } - } else if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_hold_frames / 3) { + } else if (g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) { + // 保持衰减阶段: 锥桶消失, 变形量逐帧衰减 + g_cone_hold_ctr++; + if (g_cone_hold_ctr > g_cfg.cone_hold_frames) + g_cone_return_ctr = 1; // 保持结束 → 启动回弹 + } else if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_return_frames) { + // 回弹阶段: 朝锥桶方向反推, 帮助车回到赛道中央 g_cone_return_ctr++; } - if (!cone_is_slow()) return; - if (g_zstate == Z_STOP || g_tl_state != TL_NORMAL) return; + // 回弹结束 → 清零所有状态, 避免计数器卡死 + if (g_cone_return_ctr > g_cfg.cone_return_frames) { + printf("[CONE] ★ RETURN END\n"); + g_cone_hold_src_row = -1; + g_cone_hold_ctr = 0; + g_cone_return_ctr = 0; + } - // ── 中线变形 ── - int src_row = g_cone_confirmed ? cone_row : g_cone_hold_src_row; - int src_col = g_cone_confirmed ? cone_col : g_cone_hold_src_col; + // 生命周期诊断 (debug 模式下每 5 帧打印) + // static int cone_ldbg = 0; + // if ((g_cone_confirmed || g_cone_hold_ctr > 0 || g_cone_return_ctr > 0) && ++cone_ldbg >= 5) { + // cone_ldbg = 0; + // printf("[CONE-DBG] seen=%d frm=%d conf=%d hold=%d/%d ret=%d/%d src_row=%d ret_dir=%.0f\n", + // cone_seen, g_cone_frames, g_cone_confirmed, + // g_cone_hold_ctr, g_cfg.cone_hold_frames, + // g_cone_return_ctr, g_cfg.cone_return_frames, + // g_cone_hold_src_row, g_cone_return_dir); + // } + + // 三个阶段都不活跃 → 不做变形 + if (!cone_is_slow()) return; + + // ── 4. 中线变形 ── + // confirmed+可见 → 实时位置; 否则(confirmed+不可见 / 保持 / 回弹) → 冻结位置 + int src_row = (g_cone_confirmed && cone_seen) ? cone_row : g_cone_hold_src_row; + int src_col = (g_cone_confirmed && cone_seen) ? cone_col : g_cone_hold_src_col; if (src_row <= 10) return; - double rng = (double)g_cfg.cone_avoid_range; + double rng = (double)g_cfg.cone_avoid_range; // 斜坡陡峭度 double half_w = line_tracking_width / 2.0; - double ms = mid_line[src_row]; + double ms = mid_line_raw[src_row]; + // 推离方向: 锥桶在原始中线左侧 → 推右(+1), 右侧 → 推左(-1) double dir = (src_col < ms) ? 1.0 : -1.0; + // 保持阶段衰减: 从 1.0 线性降到 0.0 double decay = g_cone_confirmed ? 1.0 : (1.0 - (double)g_cone_hold_ctr / g_cfg.cone_hold_frames); - // ── 避开阶段: 推离锥桶 ── + // ── 4a. 避开阶段: 推离锥桶 (row 10 ~ 锥桶行) ── if (g_cone_return_ctr == 0) { for (int row = 10; row <= src_row; ++row) { + // 斜坡: row=10 处 t=0(不推), 随行号增大 t→1(满推) double t = std::clamp(((row - 10) / rng), 0.0, 1.0); double push = t * g_cfg.cone_avoid_gain * half_w * dir * decay; double nm = std::clamp(mid_line[row] + push, @@ -568,16 +794,23 @@ static void cone_detect_and_deform() } } - // ── 回弹: 锥桶消失后朝锥桶方向推, 把车带回赛道中心 ── - if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_hold_frames / 3) { + // ── 4b. 回弹: 锥桶消失后朝锥桶方向反推 ── + if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_return_frames) { + static int rdbg = 0; + if (++rdbg >= 10) { rdbg = 0; + printf("[CONE] return frame=%d dir=%.0f gain=%.1f\n", + g_cone_return_ctr, g_cone_return_dir, g_cfg.cone_return_gain); + } int lt_h = line_tracking_height; - double ret_rng = rng * 1.2; + double ret_rng = rng * 1.2; // 回弹斜坡比避让略缓 double ret_hw = half_w; - double ret_dir = g_cone_return_dir; - double ret_dec = 1.0 - (double)g_cone_return_ctr / (g_cfg.cone_hold_frames / 3); + double ret_dir = g_cone_return_dir; // 与避让方向相反 + // 回弹衰减: 从 1.0 线性降到 0.0 + double ret_dec = 1.0 - (double)g_cone_return_ctr / g_cfg.cone_return_frames; + // 回弹覆盖全部有效行 (row 10 ~ lt_h-1), 不限于锥桶位置 for (int row = 10; row < lt_h; ++row) { double t = std::clamp((row - 10) / ret_rng, 0.0, 1.0); - double push = t * g_cfg.cone_avoid_gain * 0.5 * ret_hw * ret_dir * ret_dec; + double push = t * g_cfg.cone_return_gain * ret_hw * ret_dir * ret_dec; double nm = std::clamp(mid_line[row] + push, (double)left_line[row] + 2.0, (double)right_line[row] - 2.0); @@ -588,22 +821,68 @@ static void cone_detect_and_deform() // ═══════════════════════════════════════════════════════════ -// 舵机控制 +// 急弯检测 — 扫描左右边界纵向梯度 +// +// 5 行窗口内边界水平位移 > 15 像素 → 直角弯 +// ═══════════════════════════════════════════════════════════ +static bool g_sharp_turn = false; + +static void detect_sharp_turn() +{ + g_sharp_turn = false; + const int W = 3; // 窗口大小(行) + const int THR = 20; // 位移阈值(像素) + int lt_h = line_tracking_height; + for (int r = lt_h - 1; r >= 10 + W; --r) { + if (left_line[r] == -1 || left_line[r-W] == -1) continue; + if (std::abs(left_line[r] - left_line[r-W]) > THR) { g_sharp_turn = true; return; } + } + for (int r = lt_h - 1; r >= 10 + W; --r) { + if (right_line[r] == -1 || right_line[r-W] == -1) continue; + if (std::abs(right_line[r] - right_line[r-W]) > THR) { g_sharp_turn = true; return; } + } +} + +// ═══════════════════════════════════════════════════════════ +// Step 7: 舵机控制 +// +// 读取 mid_line[foresee/calc_scale] 处的中线位置, +// 转换为显示空间偏差 → 归一化 → 乘增益 → PWM 占空比。 +// +// 数学: +// deviation = mid_line[check_row] × calc_scale - newWidth/2 (像素) +// deviation -= center_bias (偏置修正) +// g_steer_deviation = deviation / (newWidth/2) (归一化 [-1,1]) +// +// |deviation| < deadband → 直行 (1,500,000 ns) +// 否则: offset = norm × steer_gain × 300,000 +// duty = clamp(1,500,000 + offset, 1,200,000, 1,800,000) +// +// 舵机量程: 1,200,000 ~ 1,800,000 ns, 中位 1,500,000 ns // ═══════════════════════════════════════════════════════════ static void steering_update() { if (!g_cfg.start) return; + // foresee (显示空间像素) → check_row (巡线空间行号) + // double scale = 1.0; + // if (g_sharp_turn) scale = g_cfg.sharp_turn_scale; + // else if (g_lost_rows > 15) scale = g_cfg.foresee_lost_scale; + // double eff_foresee = g_cfg.foresee * scale; int check_row = (int)g_cfg.foresee / calc_scale; if (check_row < 0 || check_row >= line_tracking_height) return; - if (mid_line[check_row] == -1) return; + if (mid_line[check_row] == -1) return; // 丢线行 → 跳过 + // 巡线空间中线 → 显示空间偏差 double deviation = mid_line[check_row] * calc_scale - newWidth / 2; - deviation -= g_cfg.center_bias; + deviation -= g_cfg.center_bias; // 机械偏置修正 + // 归一化到 [-1, 1], 供弯道减速使用 g_steer_deviation = deviation / (newWidth / 2.0); if (std::abs(deviation) < g_cfg.deadband) { + // 死区内 → 直行 servo.setDutyCycle(1500000); } else { + // 比例控制: 偏差 → 归一化 → × 增益 × 满偏占空比差 double offset = deviation / (newWidth / 2.0) * g_cfg.steer_gain * 300000; double duty_ns = std::clamp(1500000.0 + offset, 1200000.0, 1800000.0); servo.setDutyCycle((unsigned int)duty_ns); @@ -611,18 +890,33 @@ static void steering_update() } // ═══════════════════════════════════════════════════════════ -// 电机控制 +// Step 8: 电机控制 +// +// 弯道减速: +// factor = 1.0 - |g_steer_deviation| × curve_slope +// factor = max(factor, curve_min) +// final_spd = target_speed × factor +// +// 锥桶/挡板减速 (代码存在但已注释): +// cone_is_slow() → factor = min(factor, cone_speed) +// lidar_is_active() → factor = min(factor, lidar_speed) +// +// 调用 ControlUpdate(final_spd, block) 执行: +// block = zebra_block || tl_block → 编码器比例刹车 +// 否则 → 开环占空比驱动 // ═══════════════════════════════════════════════════════════ static void motor_update(bool zebra_block, bool tl_block) { double spd = target_speed; - // 弯道减速 + // 弯道减速: 舵机偏差越大 → 速度越低 double curve = 1.0 - std::abs(g_steer_deviation) * g_cfg.curve_slope; if (curve < g_cfg.curve_min) curve = g_cfg.curve_min; - // 取最低倍率: 弯道 / 锥桶 / 挡板 三者不叠加 + // 取最低倍率: 弯道 / 锥桶 / 挡板 三者不叠加, 取最小值 double factor = curve; + // if (g_lost_rows > 15) + // factor = std::min(factor, 0.6); // if (cone_is_slow() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) // factor = std::min(factor, g_cfg.cone_speed); // if (lidar_is_active() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) @@ -630,27 +924,39 @@ static void motor_update(bool zebra_block, bool tl_block) double final_spd = spd * factor; + // 每 30 帧打印一次速度调试信息 static int dbg = 0; if (++dbg >= 30) { dbg = 0; printf("[SPD] base=%.0f dev=%.2f curve=%.2f final=%.1f\n", spd, g_steer_deviation, curve, final_spd); } + // 传递给 control.cpp: 刹车信号 = 斑马线停车 OR 红绿灯停车 ControlUpdate(final_spd, zebra_block || tl_block); } // ═══════════════════════════════════════════════════════════ -// LCD 渲染 +// Step 9: LCD 渲染 +// +// 流程: +// 1. 每 10 帧从 g_cfg.showImg 刷新 g_lcd_on 开关 +// 2. track (128/0 灰度) → resize → GRAY2BGR → 居中拷贝到 lcd_fbImage +// 3. 绘制边界线: 红=左边界, 绿=右边界, 蓝=中线 +// 4. 绘制检测框: 橙=锥桶, 紫=斑马线 (跳过红绿灯框) +// 5. 绘制状态指示: L=激光避障, R=红灯, G=等绿灯, S=斑马线停, C=冷却, N=正常 +// 6. BGR → RGB565 → 写入 /dev/fb0 mmap // ═══════════════════════════════════════════════════════════ static cv::Mat lcd_fbImage, lcd_resized, lcd_colored; static void lcd_render() { + // 每 10 帧刷新 LCD 开关, 避免每帧读文件 static int lcd_check = 0; if (--lcd_check < 0) { g_lcd_on = g_cfg.showImg; lcd_check = 10; } if (!g_lcd_on) return; + // 创建黑底画布, 赛道灰度图居中显示 lcd_fbImage.create(screenHeight, screenWidth, CV_8UC3); lcd_fbImage.setTo(cv::Scalar(0, 0, 0)); cv::resize(track, lcd_resized, cv::Size(newWidth, newHeight)); @@ -659,20 +965,21 @@ static void lcd_render() cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight); lcd_colored.copyTo(lcd_fbImage(roi)); - // 边界线 + // ── 边界线: 逐行绘制 ── for (int y = 0; y < line_tracking_height; ++y) { int lx = (int)(left_line[y] * calc_scale), ly = y * calc_scale; int rx = (int)(right_line[y] * calc_scale); int mx = (int)(mid_line[y] * calc_scale); - cv::line(lcd_fbImage(roi), cv::Point(lx, ly), cv::Point(lx, ly), cv::Scalar(0, 0, 255), calc_scale); - cv::line(lcd_fbImage(roi), cv::Point(rx, ly), cv::Point(rx, ly), cv::Scalar(0, 255, 0), calc_scale); - cv::line(lcd_fbImage(roi), cv::Point(mx, ly), cv::Point(mx, ly), cv::Scalar(255, 0, 0), calc_scale); + cv::line(lcd_fbImage(roi), cv::Point(lx, ly), cv::Point(lx, ly), cv::Scalar(0, 0, 255), calc_scale); // 红=左 + cv::line(lcd_fbImage(roi), cv::Point(rx, ly), cv::Point(rx, ly), cv::Scalar(0, 255, 0), calc_scale); // 绿=右 + cv::line(lcd_fbImage(roi), cv::Point(mx, ly), cv::Point(mx, ly), cv::Scalar(255, 0, 0), calc_scale); // 蓝=中 } - // 检测框 (跳过红绿灯，不画框) + // ── 检测框 (跳过红绿灯, 不画框) ── float bx = (float)newWidth / raw_frame.cols, by = (float)newHeight / raw_frame.rows; for (int i = 0; i < g_box_count; ++i) { if (g_boxes[i].cls == MD_RED || g_boxes[i].cls == MD_GREEN) continue; + // 模型空间 → 显示空间 int x1 = (int)((g_boxes[i].cx - g_boxes[i].w/2) * bx); int y1 = (int)((g_boxes[i].cy - g_boxes[i].h/2) * by); int x2 = (int)((g_boxes[i].cx + g_boxes[i].w/2) * bx); @@ -682,19 +989,22 @@ static void lcd_render() x2 = std::max(0, std::min(newWidth - 1, x2)); y2 = std::max(0, std::min(newHeight - 1, y2)); + // 颜色: 锥桶=橙, 斑马线=紫, 其他=绿 cv::Scalar color(0, 255, 0); - if (g_boxes[i].cls == MD_ZEBRA) color = cv::Scalar(255, 0, 255); - else if (g_boxes[i].cls == MD_CONE) color = cv::Scalar(0, 165, 255); + if (g_boxes[i].cls == MD_ZEBRA) color = cv::Scalar(255, 0, 255); // 紫 + else if (g_boxes[i].cls == MD_CONE) color = cv::Scalar(0, 165, 255); // 橙 cv::rectangle(lcd_fbImage(roi), cv::Point(x1, y1), cv::Point(x2, y2), color, 2); + // 标注: 类别编号 + 置信度百分比 char lab[16]; snprintf(lab, 16, "%d %.0f", g_boxes[i].cls, g_boxes[i].conf * 100); cv::putText(lcd_fbImage(roi), lab, cv::Point(x1 + 2, y1 + 10), cv::FONT_HERSHEY_SIMPLEX, 0.3, color, 1); } - // 状态指示 + // ── 状态指示 (左下角) ── + // L=激光避障, R=红灯停, G=等绿灯, S=斑马线停, C=冷却, N=正常 char ztxt[8]; int zi = 0; - if (lidar_is_active()) ztxt[zi++] = 'L'; + // if (lidar_is_active()) ztxt[zi++] = 'L'; if (g_tl_state == TL_STOP) ztxt[zi++] = 'R'; else if (g_tl_state == TL_WAIT_GREEN) ztxt[zi++] = 'G'; if (g_zstate == Z_STOP) ztxt[zi++] = 'S'; @@ -704,11 +1014,22 @@ static void lcd_render() cv::putText(lcd_fbImage(roi), ztxt, cv::Point(2, newHeight - 4), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 255, 255), 1); + // BGR → RGB565 → /dev/fb0 convertMatToRGB565(lcd_fbImage, fb_buffer, screenWidth, screenHeight); } // ═══════════════════════════════════════════════════════════ -// FPS 统计 +// Step 10: FPS 统计 +// +// 每 15 帧输出一行: +// fps: 总帧率 +// cap: 取帧耗时(ms) +// vis: 视觉巡线耗时(ms) +// mdl: 模型推理耗时(ms) (管道末尾, 不阻塞舵机) +// ctl: 控制耗时(ms) (状态机+舵机+电机) +// lat: 端到端延时(ms) (取帧开始→舵机输出) +// +// ts[] 时间戳由 CameraHandler 在各步骤间采集 // ═══════════════════════════════════════════════════════════ static void fps_log(struct timespec *ts) { @@ -716,68 +1037,113 @@ static void fps_log(struct timespec *ts) static struct timespec tf0; if (fc == 0) clock_gettime(CLOCK_MONOTONIC, &tf0); fc++; - if (fc % 15 != 0) return; + if (fc % 15 != 0) return; // 每 15 帧输出一次 struct timespec tf1; clock_gettime(CLOCK_MONOTONIC, &tf1); double dt = (tf1.tv_sec - tf0.tv_sec) + (tf1.tv_nsec - tf0.tv_nsec) * 1e-9; - double ms_rd = (ts[1].tv_sec - ts[0].tv_sec) * 1000.0 + (ts[1].tv_nsec - ts[0].tv_nsec) * 1e-6; + double ms_cap = (ts[1].tv_sec - ts[0].tv_sec) * 1000.0 + (ts[1].tv_nsec - ts[0].tv_nsec) * 1e-6; double ms_vis = (ts[2].tv_sec - ts[1].tv_sec) * 1000.0 + (ts[2].tv_nsec - ts[1].tv_nsec) * 1e-6; - double ms_mdl = (ts[3].tv_sec - ts[2].tv_sec) * 1000.0 + (ts[3].tv_nsec - ts[2].tv_nsec) * 1e-6; - double ms_ctl = (ts[4].tv_sec - ts[3].tv_sec) * 1000.0 + (ts[4].tv_nsec - ts[3].tv_nsec) * 1e-6; + double ms_mdl = (ts[5].tv_sec - ts[4].tv_sec) * 1000.0 + (ts[5].tv_nsec - ts[4].tv_nsec) * 1e-6; + double ms_ctl = (ts[4].tv_sec - ts[2].tv_sec) * 1000.0 + (ts[4].tv_nsec - ts[2].tv_nsec) * 1e-6; + double ms_lat = (ts[3].tv_sec - ts[0].tv_sec) * 1000.0 + (ts[3].tv_nsec - ts[0].tv_nsec) * 1e-6; - printf("fps=%.1f | rd=%.0f vi=%.0f md=%.0f ct=%.0f ms\r", - fc / dt, ms_rd, ms_vis, ms_mdl, ms_ctl); + printf("fps=%.1f | cap=%.0f vis=%.0f mdl=%.0f ctl=%.0f lat=%.0f ms\r", + fc / dt, ms_cap, ms_vis, ms_mdl, ms_ctl, ms_lat); fflush(stdout); } // ═══════════════════════════════════════════════════════════ -// CameraHandler — 每帧主流程 +// CameraHandler — 每帧主流程 (10 步流水线) +// +// 由 main.cpp 主循环调用, 单线程顺序执行。 +// +// 附加逻辑: +// - debug 模式每 30 帧重载全部配置 (cfg_load_all) +// +// ts[6] 时间戳分布 (按执行顺序): +// ts[0]: capture_frame 取帧开始 +// ts[1]: capture_frame 取帧结束 +// ts[2]: image_main 视觉巡线结束 +// ts[3]: steering_update 舵机输出结束 (端到端延时终点) +// ts[4]: motor_update 电机控制结束 +// ts[5]: run_model_inference 模型推理结束 (管道末尾) // ═══════════════════════════════════════════════════════════ int CameraHandler(void) { + // debug 模式: 每 30 帧热重载配置文件 static int reloadCnt = 0; if (g_cfg.debug && ++reloadCnt >= 30) { reloadCnt = 0; cfg_load_all(); } + // 嵌入式优化: 每 150 帧回收 glibc malloc 缓存的空闲页 static int trimCnt = 0; - if (++trimCnt >= 150) { trimCnt = 0; malloc_trim(0); } + // if (++trimCnt >= 150) { trimCnt = 0; malloc_trim(0); } - struct timespec ts[5]; // [0]=capture_end, [1]=vision_end, [2]=model_end, [3]=control_end, [4]=fps + struct timespec ts[6]; - // 1. 取帧 + 解码 + // 1. 取帧 + MJPEG 解码 if (capture_frame(&ts[0], &ts[1]) < 0) return -1; // 2. 保存图像 (debug) save_image_if_requested(); - // 3. 视觉巡线 + // 3. 视觉巡线 → left/right/mid_line[] image_main(); + mid_line_raw = mid_line; clock_gettime(CLOCK_MONOTONIC, &ts[2]); - // 4. 避障激光 (预触发: 读数+起测 ~1ms, 测距 20ms 后台跑) - lidar_avoid_process(); + // 4. 激光避障 → 修改 mid_line[] (在模型推理之前, 优先级高于锥桶) + // lidar_avoid_process(); - // 5. 模型推理 - run_model_inference(); - clock_gettime(CLOCK_MONOTONIC, &ts[3]); + // 5. 场景识别状态机 (使用上一帧的模型检测结果 g_boxes[]) + bool zebra_block = zebra_process(); // 5a. 斑马线 + bool tl_block = traffic_light_process(); // 5b. 红绿灯 + cone_detect_and_deform(); // 5c. 锥桶 → 修改 mid_line[] - // 6. 场景识别 - bool zebra_block = zebra_process(); - bool tl_block = traffic_light_process(); - cone_detect_and_deform(); + // detect_sharp_turn(); // 急弯检测 - // 7. 舵机 + // 6. 舵机控制 (读取变形后的 mid_line) steering_update(); + clock_gettime(CLOCK_MONOTONIC, &ts[3]); // 舵机输出结束 = 端到端延时终点 + // ── 临时测试: 锥桶确认后 1s 强制右转 0.5s ── + // { + // static int cone_test_timer = -1; + // static bool cone_test_need_clr = false; + // + // if (cone_test_need_clr && !g_cone_confirmed) + // cone_test_need_clr = false; + // + // if (g_cone_confirmed && cone_test_timer < 0 && !cone_test_need_clr) { + // cone_test_timer = 0; + // printf("[CONE-TEST] 锥桶确认, 开始计时\n"); + // } + // if (cone_test_timer >= 0) { + // cone_test_timer++; + // if (cone_test_timer == 30) + // printf("[CONE-TEST] 强制右转开始\n"); + // if (cone_test_timer >= 30 && cone_test_timer < 38) + // servo.setDutyCycle(1700000); + // if (cone_test_timer >= 38) { + // printf("[CONE-TEST] 强制右转结束\n"); + // cone_test_timer = -1; + // cone_test_need_clr = true; + // } + // } + // } - // 8. 电机 + // 7. 电机控制 (弯道减速 + 刹车) motor_update(zebra_block, tl_block); clock_gettime(CLOCK_MONOTONIC, &ts[4]); - // 9. LCD + // 8. 模型推理 → g_boxes[] (放到最后, 供下一帧状态机使用, 不阻塞舵机) + run_model_inference(); + clock_gettime(CLOCK_MONOTONIC, &ts[5]); + + // 9. LCD 渲染 lcd_render(); - // 10. FPS + // 10. FPS 日志 fps_log(ts); return 0; diff --git a/src/control.cpp b/src/control.cpp index 7dbc68d..dd5a5c7 100644 --- a/src/control.cpp +++ b/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,17 +17,27 @@ #include #include +// ═══════════════════════════════════════════════════════════ +// 全局 & 静态变量 +// ═══════════════════════════════════════════════════════════ MotorController *motorController[2] = {nullptr, nullptr}; -GPIO mortorEN(73); +GPIO mortorEN(73); // 电机使能引脚 -static GPIO encoderLSB(67); -static GPIO encoderDIR(72); +static GPIO encoderLSB(67); // 编码器 LSB 脉冲 +static GPIO encoderDIR(72); // 编码器方向 static std::thread g_enc_thread; static std::atomic g_enc_running{false}; -static std::atomic g_enc_speed{0.0}; -static std::atomic g_enc_dir{0}; +static std::atomic g_enc_speed{0.0}; // 编码器速度 (脉冲/秒) +static std::atomic 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); @@ -36,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) { @@ -82,55 +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(); - int cur_dir = g_enc_dir.load(); + double cur_speed = g_enc_speed.load(); // 原子读: 编码器速度 (pps) + 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; } + // ── 模式 3: 正常行驶 → 开环占空比 ── for (int i = 0; i < 2; ++i) 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); // 使能拉低 } diff --git a/src/global.cpp b/src/global.cpp index 2def5f7..e26891a 100644 --- a/src/global.cpp +++ b/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); @@ -39,6 +41,8 @@ void cfg_load_all() 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); diff --git a/src/image_cv.cpp b/src/image_cv.cpp index 4c1fe65..3906a58 100644 --- a/src/image_cv.cpp +++ b/src/image_cv.cpp @@ -30,6 +30,9 @@ cv::Mat track; std::vector left_line; std::vector right_line; std::vector mid_line; +std::vector mid_line_raw; + +int g_lost_rows = 0; int line_tracking_width; // 处理宽度 = 80 int line_tracking_height; // 处理高度 = 60 @@ -105,7 +108,8 @@ cv::Mat find_road(cv::Mat &frame) cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel); static cv::Mat mask; - mask = cv::Mat::zeros(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1); + if (mask.empty()) mask.create(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1); + mask.setTo(0); // 3. 种子点位置 // X = 图像水平中心 (line_tracking_width/2) @@ -130,7 +134,8 @@ cv::Mat find_road(cv::Mat &frame) nullptr, loDiff, upDiff, 8); static cv::Mat outputImage; - outputImage = cv::Mat::zeros(line_tracking_height, line_tracking_width, CV_8UC1); + if (outputImage.empty()) outputImage.create(line_tracking_height, line_tracking_width, CV_8UC1); + outputImage.setTo(0); mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage); return outputImage; @@ -244,11 +249,13 @@ void image_main() // ── 6. 中线计算 + 丢线补全 ───── // 从倒数第二行开始向上，底行(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]; @@ -278,6 +285,7 @@ void image_main() 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;