替换 Mild v12 模型 + 同事编码器刹车红绿灯状态机合并
This commit is contained in:
spdis
@@ -1,172 +0,0 @@
|
||||
# 2K0300智能车项目代码文档
|
||||
|
||||
## 项目概述
|
||||
该项目是一个智能车控制系统，包含摄像头图像处理、电机控制、编码器反馈、PID 控制、帧缓冲区显示等功能。项目使用 C++ 编写，依赖 OpenCV 库进行图像处理，并通过 CMake 进行构建。
|
||||
|
||||
## 目录结构
|
||||
- **CMakeLists.txt**: 项目的构建配置文件。
|
||||
- **cross.cmake**: 交叉编译工具链配置文件。
|
||||
- **src/**: 库文件目录，包含摄像头、电机控制、PID 控制等模块的实现。
|
||||
- **lib/**: 库文件目录，包含摄像头、电机控制、PID 控制等模块的头文件。
|
||||
- **main/**: 主程序目录。
|
||||
- **xx_demo/**: 示例程序目录，包含多个演示程序。
|
||||
|
||||
---
|
||||
|
||||
## 主要模块介绍
|
||||
|
||||
### 1. 摄像头模块 (`camera.cpp`, `camera.h`)
|
||||
摄像头模块负责初始化摄像头设备、捕获图像并进行处理。主要功能包括：
|
||||
- **摄像头初始化**: 打开摄像头设备，设置分辨率、帧率等参数。
|
||||
- **图像捕获**: 通过 OpenCV 捕获摄像头图像，并进行缩放、二值化等处理。
|
||||
- **帧缓冲区显示**: 将处理后的图像显示到帧缓冲区设备（如 LCD 屏幕）。
|
||||
- **图像保存**: 支持将捕获的图像保存为文件。
|
||||
|
||||
#### 关键函数：
|
||||
- `CameraInit(uint8_t camera_id, double dest_fps, int width, int height)`: 初始化摄像头设备。
|
||||
- `CameraHandler(void)`: 处理摄像头捕获的图像。
|
||||
- `streamCapture(void)`: 持续捕获摄像头图像，存储到全局变量`cv::Mat pubframe`中，`std::mutex frameMutex`为互斥锁。
|
||||
- `cameraDeInit(void)`: 释放摄像头资源。
|
||||
|
||||
---
|
||||
|
||||
### 2. 电机控制模块 (`MotorController.cpp`, `MotorController.h`)
|
||||
电机控制模块负责控制智能车的电机，支持 PWM 调速和方向控制。主要功能包括：
|
||||
- **电机初始化**: 初始化 PWM 控制器和 GPIO 引脚。
|
||||
- **速度控制**: 通过 PID 控制器调节电机速度。
|
||||
- **方向控制**: 控制电机的正反转。
|
||||
|
||||
#### 关键函数：
|
||||
- `MotorController(int pwmchip, int pwmnum, int gpioNum, unsigned int period_ns, double kp, double ki, double kd, double targetSpeed, int encoder_pwmNum, int encoder_gpioNum, int encoder_dir_)`: 构造函数，初始化电机控制器，`encoder_dir_`取值为1/-1，在更新速度时自动乘算到编码器速度上。
|
||||
- `updateSpeed(void)`: 从编码器更新当前速度。
|
||||
- `updateTarget(int speed)`: 设置目标速度。
|
||||
- `updateduty(double dutyCycle)`: 设置 PWM 占空比，为负值时反转。
|
||||
|
||||
---
|
||||
|
||||
### 3. PID 控制模块 (`PIDController.cpp`, `PIDController.h`)
|
||||
PID 控制模块实现了位置式和增量式 PID 控制器，用于电机速度和舵机角度的精确控制。主要功能包括：
|
||||
- **PID 参数设置**: 支持动态调整比例、积分、微分参数。
|
||||
- **控制模式切换**: 支持位置式和增量式 PID 控制。
|
||||
- **输出限幅**: 防止输出值超出设定范围。
|
||||
- **积分限幅**：放置积分项超出设定范围。
|
||||
|
||||
#### 关键函数：
|
||||
- `PIDController(double kp, double ki, double kd, double target, PIDMode mode, double output_limit, double integral_limit)`: 构造函数，初始化 PID 参数。
|
||||
- `update(double measured_value)`: 输入测量值，更新 PID 控制器输出。
|
||||
- `setTarget(double target)`：设置PID控制器目标值。
|
||||
- `setPID(double kp, double ki, double kd)`: 设置 PID 参数。
|
||||
- `setMode(PIDMode mode)`: 设置控制模式（位置式或增量式）。
|
||||
|
||||
---
|
||||
|
||||
### 4. 编码器模块 (`encoder.cpp`, `encoder.h`)
|
||||
编码器模块用于读取电机编码器的脉冲信号，计算电机转速。主要功能包括：
|
||||
- **编码器初始化**: 映射寄存器，初始化编码器设备。
|
||||
- **脉冲计数**: 读取编码器脉冲信号，计算转速。
|
||||
- **方向检测**: 通过 GPIO 读取电机方向。
|
||||
|
||||
#### 关键函数：
|
||||
- `ENCODER(int pwmNum, int gpioNum)`: 构造函数，初始化编码器。
|
||||
- `pulse_counter_update(void)`: 更新脉冲计数并计算转速（单位：rps）。
|
||||
|
||||
---
|
||||
|
||||
### 5. 帧缓冲区模块 (`frame_buffer.cpp`, `frame_buffer.h`)
|
||||
帧缓冲区模块负责将 OpenCV 图像转换为 RGB565 格式并显示到帧缓冲区设备（如 LCD 屏幕）。主要功能包括：
|
||||
- **图像格式转换**: 将 OpenCV 的 RGB888 图像转换为 RGB565 格式。
|
||||
- **帧缓冲区写入**: 将转换后的图像写入帧缓冲区设备。
|
||||
|
||||
#### 关键函数：
|
||||
- `convertMatToRGB565(const cv::Mat &frame, uint16_t *buffer, int width, int height)`: 将 OpenCV 图像转换为 RGB565 格式，写入到buffer中。
|
||||
- `convertRGBToRGB565(uint8_t r, uint8_t g, uint8_t b)`: 将 RGB888 颜色值转换为 RGB565 格式。
|
||||
|
||||
---
|
||||
|
||||
### 6. GPIO 模块 (`GPIO.cpp`, `GPIO.h`)
|
||||
GPIO 模块用于控制 GPIO 引脚，支持方向设置、值读取和写入。主要功能包括：
|
||||
- **GPIO 初始化**: 导出 GPIO 引脚并设置方向。
|
||||
- **值读写**: 读取或写入 GPIO 引脚的值。
|
||||
|
||||
#### 关键函数：
|
||||
- `GPIO(int gpioNum)`: 构造函数，初始化 GPIO 引脚。
|
||||
- `setDirection(const std::string &direction)`: 设置 GPIO 引脚方向。
|
||||
- `setValue(bool value)`: 设置 GPIO 引脚值。
|
||||
- `readValue(void)`: 读取 GPIO 引脚值。
|
||||
|
||||
---
|
||||
|
||||
### 7. PWM 控制模块 (`PwmController.cpp`, `PwmController.h`)
|
||||
PWM 控制模块用于生成 PWM 信号，控制电机速度和舵机角度。主要功能包括：
|
||||
- **PWM 初始化**: 初始化 PWM 设备。
|
||||
- **周期和占空比设置**: 设置 PWM 信号的周期和占空比。
|
||||
- **PWM 启用/禁用**: 控制 PWM 信号的输出。
|
||||
|
||||
#### 关键函数：
|
||||
- `PwmController(int pwmchip, int pwmnum)`: 构造函数，初始化 PWM 设备。
|
||||
- `setPeriod(unsigned int period_ns)`: 设置 PWM 周期。
|
||||
- `setDutyCycle(unsigned int duty_cycle_ns)`: 设置 PWM 占空比。
|
||||
- `enable(void)`: 启用 PWM 输出。
|
||||
- `disable(void)`: 禁用 PWM 输出。
|
||||
|
||||
---
|
||||
|
||||
### 8. 图像处理模块 (`image_cv.cpp`, `image_cv.h`)
|
||||
图像处理模块负责对摄像头捕获的图像进行处理，提取赛道边缘和中线。主要功能包括：
|
||||
- **图像缩放**: 将图像缩放到指定大小。
|
||||
- **二值化处理**: 将图像转换为二值图像。
|
||||
- **边缘提取**: 提取赛道的左右边缘和中线。
|
||||
|
||||
#### 关键函数：
|
||||
- `image_main(void)`: 主图像处理函数，执行缩放、二值化和边缘提取。
|
||||
|
||||
---
|
||||
|
||||
### 9. 全局变量模块 (`global.cpp`, `global.h`)
|
||||
全局变量模块定义了项目中使用的全局变量和工具函数。主要功能包括：
|
||||
- **文件读取**: 从文件中读取双精度值和标志。
|
||||
- **全局变量**: 定义 PID 参数、目标速度等全局变量。
|
||||
|
||||
#### 关键函数：
|
||||
- `readDoubleFromFile(const std::string &filename)`: 从文件中读取双精度值。
|
||||
- `readFlag(const std::string &filename)`: 从文件中读取标志。
|
||||
|
||||
---
|
||||
|
||||
### 10. 定时器模块 (`Timer.cpp`, `Timer.h`)
|
||||
定时器模块用于创建定时任务，定期执行指定函数。主要功能包括：
|
||||
- **定时任务创建**: 创建定时任务并启动。
|
||||
- **定时任务停止**: 停止定时任务。
|
||||
|
||||
#### 关键函数：
|
||||
- `Timer(int interval_ms, std::function<void()> task)`: 构造函数，初始化定时器。
|
||||
- `start(void)`: 启动定时任务。
|
||||
- `stop(void)`: 停止定时任务。
|
||||
|
||||
---
|
||||
|
||||
## 示例程序
|
||||
|
||||
### 1. 编码器测试程序 (`encoder_demo.cpp`)
|
||||
该程序用于测试编码器模块，读取编码器脉冲信号并输出转速。
|
||||
|
||||
### 2. 帧缓冲区测试程序 (`framebuffer_demo.cpp`)
|
||||
该程序用于测试帧缓冲区模块，绘制图形并显示到帧缓冲区设备。
|
||||
|
||||
### 3. OpenCV 测试程序 (`opencv_demo1.cpp`, `opencv_demo2.cpp`, `opencv_demo3.cpp`)
|
||||
这些程序用于测试 OpenCV 图像处理功能，分别为显示摄像头图像、显示旋转立方体、目标匹配。
|
||||
- `opencv_demo3.cpp`在运行时依赖`opencv/share/opencv4/haarcascades/haarcascade_frontalface_default.xml`，需要在`opencv`文件夹同一目录下运行。
|
||||
|
||||
### 4. 遥控车演示程序 (`demo1.cpp`)
|
||||
该程序通过键盘控制智能车的运动，支持前进、后退、左转、右转。
|
||||
|
||||
---
|
||||
|
||||
## 构建项目
|
||||
使用 CMake 构建项目：
|
||||
```bash
|
||||
mkdir build
|
||||
cd build
|
||||
cmake ..
|
||||
make
|
||||
```
|
||||
@@ -0,0 +1,94 @@
|
||||
# AGENTS.md — SmartCar Demo
|
||||
|
||||
## Project overview
|
||||
|
||||
LoongArch64 embedded autonomous smart car using OpenCV + NanoDet V10 model.
|
||||
Runs on-device (Loongson board) with cross-compilation from x86 Linux.
|
||||
|
||||
## Build
|
||||
|
||||
```sh
|
||||
# On the build host (x86 Linux):
|
||||
mkdir build && cd build
|
||||
cmake ..
|
||||
make
|
||||
```
|
||||
|
||||
- **Cross-compilation target**: LoongArch64 (`-march=loongarch64`)
|
||||
- **Toolchain**: `/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6`
|
||||
- **C++ standard**: 17
|
||||
- **OpenCV path** (device-side): `/mnt/d/PPPProgram/smartcar/opencv_device/`
|
||||
- `cross.cmake` is included **before** `cmake_minimum_required` — this is intentional.
|
||||
|
||||
## 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) |
|
||||
| `build/` | CMake build output (gitignored) |
|
||||
|
||||
**Executable name**: `smartcar_demo1` (the CMake project name `smartcar_demo2` is different — don't assume they match).
|
||||
|
||||
## Excluded modules
|
||||
|
||||
These source files exist but are **excluded from build**:
|
||||
|
||||
- `vl53l0x.cpp` — laser ranging module, hardware not connected
|
||||
- `zebra_detect.cpp` — classic zebra-crossing detection, replaced by NanoDet model
|
||||
- `encoder.cpp` — referenced in exclusion list but file does **not** exist in repo
|
||||
|
||||
## Device-side operation
|
||||
|
||||
The onboard binary is controlled via text files in the working directory — no CLI args.
|
||||
|
||||
```sh
|
||||
# On device:
|
||||
sh ctl.sh init # initialize GPIO/PWM pins
|
||||
sh ctl.sh start # start the demo
|
||||
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. `start.sh` uses `LD_PRELOAD=./gpio_fix_final.so` (GPIO workaround).
|
||||
|
||||
## Configuration system
|
||||
|
||||
All config is via single-value text files in the working directory:
|
||||
|
||||
- `./speed` (double) — target speed
|
||||
- `./start` (0/1) — motor enable switch
|
||||
- `./debug` (0/1) — when 1, reloads all config files every 7 frames
|
||||
- `./showImg` (0/1) — LCD display toggle
|
||||
- `./deadband`, `./steer_gain`, `./center_bias` — steering tuning
|
||||
- `./foresee` — look-ahead row for steering
|
||||
- `./zebrasee` — zebra crossing near-threshold
|
||||
- `./destfps` — target framerate
|
||||
- `./kp`, `./ki`, `./kd` — PID gains (currently **not used**; motor is open-loop)
|
||||
- `./mortor_kp`, `./mortor_ki`, `./mortor_kd` — motor encoder PID (spelling `mortor` is intentional)
|
||||
|
||||
## Architecture
|
||||
|
||||
9-step per-frame pipeline in `CameraHandler()` (`src/camera.cpp`):
|
||||
1. Capture frame (640×480 MJPEG → BGR)
|
||||
2. Optionally save image
|
||||
3. Vision line tracking (80×60 downscale → HSV+Otsu → floodFill → mid_line[60])
|
||||
4. Model inference every 2nd frame (NanoDet V10, 4 classes: cone/red/green/zebra)
|
||||
5. Zebra crossing state machine (NORMAL→STOP(4s)→COOLDOWN(5s))
|
||||
6. Steering servo control (PWM, deadband-filtered)
|
||||
7. Motor control (open-loop duty cycle, curve-based slowdown in turns)
|
||||
8. LCD rendering (`/dev/fb0` mmap)
|
||||
9. FPS logging
|
||||
|
||||
**Key detail**: Motor control is **open-loop** — `MotorController::updateSpeed()` and encoder PID exist in code but are never called from the main loop. Only `updateduty()` (direct PWM) is used.
|
||||
|
||||
Currently **cones (cls=0), red lights (cls=1), and green lights (cls=2)** are detected by the model but ignored in decision logic.
|
||||
|
||||
## Style and conventions
|
||||
|
||||
- C++ files have **no copyright headers** and **minimal comments** — comments are ascii-box-style block comments when present
|
||||
- Header guards use the form `#ifndef FILENAME_H_` / `#define FILENAME_H_`
|
||||
- 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
|
||||
@@ -1,256 +0,0 @@
|
||||
# SmartCar Demo — 使用手册 & 踩坑记录
|
||||
|
||||
## 项目概述
|
||||
|
||||
龙芯 2K0300 智能车卖家 Demo，简易架构：`sysfs` 直接控制 GPIO/PWM + mmap 读编码器 + OpenCV 巡线。
|
||||
|
||||
**赛道**：蓝底黄线边界，灰度路面
|
||||
**驱动**：前轮舵机转向 + 后轮双电机差速辅助
|
||||
|
||||
---
|
||||
|
||||
## 硬件信息
|
||||
|
||||
| 部件 | 型号/规格 | 说明 |
|
||||
|------|----------|------|
|
||||
| 主控 | 龙芯 2K0300 (LA264, 1.2GHz) | LoongArch64 |
|
||||
| 摄像头 | USB UVC | 320×240 MJPG, `/dev/video0` |
|
||||
| 显示器 | SPI LCD | `/dev/fb0`, RGB565, 160×128 (实际) |
|
||||
| 舵机 | 标准 PWM 舵机 | pwmchip1/pwm0, 3ms 周期, 1.52ms 中位 |
|
||||
| 电机驱动 | DRV8701E (双路) | pwmchip8/pwm1→右, pwmchip8/pwm2→左 |
|
||||
| 编码器 | 硬件脉冲计数 | `/dev/mem` mmap 基址 `0x1611B000` |
|
||||
| IMU | JY62 六轴陀螺仪 | `/dev/ttyS1`, 115200bps |
|
||||
| ToF 测距 | VL53L0X | `/dev/stmvl53l0x_ranging` |
|
||||
| 音箱 | wonderEcho | I2C-2, 地址 0x6E (有应答, 无驱动) |
|
||||
|
||||
### 关键引脚映射
|
||||
|
||||
| 功能 | GPIO / PWM | 备注 |
|
||||
|------|-----------|------|
|
||||
| 电机使能 | GPIO73 | 高有效, 1=启动, 0=紧急制动 |
|
||||
| 左电机 IN1 (正转) | GPIO12 + pwmchip8/pwm2 | DRV8701 IN/IN 模式 |
|
||||
| 左电机 IN2 (反转) | GPIO13 | **必须锁 0** |
|
||||
| 右电机 PWM | pwmchip8/pwm1 | 方向脚 GPIO67 是哑脚 |
|
||||
| 左编码器 | 通道0 + GPIO75 | 方向检测 |
|
||||
| 右编码器 | 通道3 + GPIO72 | 方向检测, 极性 -1 |
|
||||
| 舵机 | pwmchip1/pwm0 | 3ms, 1.31~1.73ms (±7 刻度) |
|
||||
|
||||
---
|
||||
|
||||
## 构建环境
|
||||
|
||||
### 交叉编译工具链
|
||||
|
||||
```
|
||||
路径: /opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6
|
||||
编译器: loongarch64-linux-gnu-g++ (GCC 8.3)
|
||||
架构: -march=loongarch64
|
||||
```
|
||||
|
||||
### OpenCV
|
||||
|
||||
```
|
||||
路径: /home/ilikara/loongson/opencv-4.11.0/loongson
|
||||
版本: 4.11.0 (必须匹配设备上的 libopencv)
|
||||
```
|
||||
|
||||
### 编译
|
||||
|
||||
```bash
|
||||
cd smartcar1
|
||||
mkdir -p build && cd build
|
||||
cmake .. # CROSS_COMPILE=1 在 cross.cmake 中设置
|
||||
make -j$(nproc)
|
||||
```
|
||||
|
||||
产物: `build/main/smartcar_demo` (约 90KB)
|
||||
|
||||
**Windows 上不可编译！** 必须在 Linux 下用交叉工具链。可在 WSL Ubuntu 中用 `g++ -std=c++17 -fsyntax-only` 做部分语法检查。
|
||||
|
||||
---
|
||||
|
||||
## 部署与启动
|
||||
|
||||
### 板端目录结构
|
||||
|
||||
```
|
||||
/home/root/smartcar/
|
||||
├── smartcar_demo # 主程序
|
||||
├── ctl.sh # 启停脚本
|
||||
├── gpio_fix_final.so # GPIO74→73 重定向 (LD_PRELOAD)
|
||||
├── start.sh # 推荐启动脚本 (已内置 LD_PRELOAD)
|
||||
├── showImg # LCD 预览开关
|
||||
├── start # 车控启停
|
||||
├── speed # 目标速度 (占空比)
|
||||
├── deadband # 舵机死区
|
||||
├── steer_gain # 转向增益
|
||||
├── kp / ki / kd # 舵机 PID
|
||||
├── mortor_kp / mortor_ki / mortor_kd # 电机 PID
|
||||
└── foresee # 前瞻行号
|
||||
```
|
||||
|
||||
### 推荐启动
|
||||
|
||||
```bash
|
||||
# 方式 1: 使用包装脚本 (已固化 LD_PRELOAD)
|
||||
sh /home/root/smartcar/start.sh
|
||||
|
||||
# 方式 2: 手动
|
||||
LD_PRELOAD=/home/root/smartcar/gpio_fix_final.so /home/root/smartcar/smartcar_demo
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 运行时参数热调
|
||||
|
||||
**无需重新编译**，直接 `echo` 写文件即可实时生效：
|
||||
|
||||
```bash
|
||||
echo 11 > /home/root/smartcar/speed # 目标占空比 (0~100)
|
||||
echo 8 > /home/root/smartcar/deadband # 舵机死区 (像素)
|
||||
echo 1.5 > /home/root/smartcar/steer_gain # 转向增益
|
||||
echo 3.5 > /home/root/smartcar/kp # 舵机 P
|
||||
echo 0.3 > /home/root/smartcar/ki # 舵机 I
|
||||
echo 2.0 > /home/root/smartcar/kd # 舵机 D
|
||||
echo 40 > /home/root/smartcar/foresee # 前瞻行 (0~120, 越大看越远)
|
||||
echo 1 > /home/root/smartcar/showImg # 1=LCD 预览, 0=关闭
|
||||
echo 1 > /home/root/smartcar/start # 1=启动, 0=停止
|
||||
echo 1 > /home/root/smartcar/saveImg # 保存当前帧到 ./image/
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 踩坑记录 (关键！)
|
||||
|
||||
### 1. GPIO74 与 LCD 驱动冲突 — 程序无法启动
|
||||
|
||||
**现象**: `std::runtime_error: Failed to open GPIO value file`
|
||||
|
||||
**原因**: Demo 二进制硬编码 GPIO_EN=74, 但 GPIO74 被 LCD 驱动 `fb_st7735r` 占用 (内核级, 无法释放)。
|
||||
|
||||
**解决**: 编译 `gpio_fix_final.so`, 用 LD_PRELOAD 劫持 `open()` 系统调用, 将对 gpio74 的访问重定向到 gpio73。启动时必须带 `LD_PRELOAD`。
|
||||
|
||||
**源代码修复**: `control.cpp` 中 `GPIO mortorEN(73)` — 如果从源码编译, 直接改成 GPIO73, 无需 LD_PRELOAD。
|
||||
|
||||
### 2. 右电机方向硬件锁死 — 差速失效
|
||||
|
||||
**现象**: 右轮只能单向旋转, 代码写 GPIO67 无效。
|
||||
|
||||
**原因**: DRV8701 的右路方向脚 (原本应对应某个 GPIO) 在 PCB 上直接拉高/拉低了。代码中的 GPIO67 是哑脚, 实际不控制任何硬件。
|
||||
|
||||
**解决**: 物理剪线交换右电机电源线 (M+/M- 对调), 代码中 GPIO67 作为占位符不动。这样一来, 代码输出正占空比时电机实际反向。
|
||||
|
||||
### 3. GPIO13 实测映射 — 文档与实物不符
|
||||
|
||||
**文档称**: GPIO12=左IN1, GPIO13=右IN1。
|
||||
**实测**: GPIO13 控制的是**左电机 IN2 (反转)**, 不是右电机方向!
|
||||
|
||||
**影响**: 若 GPIO13=1, 左电机 IN1=1 IN2=1 → 制动状态, 左轮抱死。
|
||||
|
||||
**解决**: 代码中显式 `leftIn2.setValue(0)` 把 GPIO13 锁 0, 确保左电机只有正转 (IN1=1, IN2=0)。
|
||||
|
||||
### 4. 摄像头分辨率不稳定
|
||||
|
||||
**现象**: 摄像头偶尔返回 640×480 而非 320×240。
|
||||
|
||||
**原因**: 某些 UVC 摄像头不响应 `CAP_PROP_FRAME_WIDTH/HEIGHT` 设置, 按默认格式输出。
|
||||
|
||||
**解决**:
|
||||
```cpp
|
||||
cap.open(0, cv::CAP_V4L2); // 强制 V4L2 后端
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, 320); // 设宽
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, 240); // 设高
|
||||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M','J','P','G')); // MJPG
|
||||
```
|
||||
即使摄像头不遵从, `resize()` 也能在软件层统一到 80×60。
|
||||
|
||||
### 5. 编码器必须 root 权限
|
||||
|
||||
**现象**: `ENCODER` 构造失败, mmap 报错。
|
||||
|
||||
**原因**: 编码器通过 `/dev/mem` 直接映射 PWM 硬件寄存器 (物理地址 `0x1611B000`), 需要 root。
|
||||
|
||||
**解决**: 必须以 root 运行程序。`sudo` 或直接以 root 登录。
|
||||
|
||||
### 6. IMU 端口纠正
|
||||
|
||||
**文档称**: IMU 在 `/dev/ttyS2`。
|
||||
**实测**: IMU 在 `/dev/ttyS1` (115200bps)。
|
||||
|
||||
### 7. 网络热插拔导致断连
|
||||
|
||||
**现象**: 拔掉网线再插上, SSH 连不上。
|
||||
|
||||
**原因**: `/etc/network/interfaces` 中静态 eth0 配置与 connman 冲突。
|
||||
|
||||
**解决**: 删除 `/etc/network/interfaces` 中的 eth0 段, 让 connman 全权管理有线网络。修改后即使先开机后插网线也能自动获取 IP。
|
||||
|
||||
### 8. GCC 14.2.0 工具链 ABI 不兼容
|
||||
|
||||
**现象**: 用新版 GCC 14.2.0 (当前 WSL 环境) 编译的程序在设备上运行报 `GLIBCXX_3.4.30 not found`。
|
||||
|
||||
**原因**: GCC 14.2.0 的 libstdc++ ABI 与设备上的 glibc 2.28 / libstdc++ 6.0.25 不兼容。
|
||||
|
||||
**解决**:
|
||||
- **必须使用原始 GCC 8.3 rc1.6 工具链**。路径: `/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6`
|
||||
- 可用 C 编译器 (`gcc` 而非 `g++`) + 显式 `-march=la64v1.0` + 指定 `-Wl,-dynamic-linker=...` 编译纯 C 程序
|
||||
|
||||
### 9. 视觉管线分辨率选择
|
||||
|
||||
**Demo 内部处理**: 摄像头 320×240 → 缩放至显示分辨率 (160×120) → 再除以 `calc_scale=2` → 最终 **80×60** 做 HSV 二值化 + 洪泛填充 + 搜线。
|
||||
|
||||
**关键**: 80×60 是最优平衡点——再大 Otsu 不稳, 再小细节丢失。
|
||||
|
||||
### 10. 启动顺序 & 竞态
|
||||
|
||||
- GPIO 必须先 export + 设方向, 再启动程序
|
||||
- 舵机 PWM 必须先设 period + enable, 才能写 duty_cycle
|
||||
- 程序需要几秒初始化 IMU (校准 200 样本 × 5ms = 1 秒)
|
||||
|
||||
### 11. PWM 频率
|
||||
|
||||
| PWM | 频率 | 周期 | 用途 |
|
||||
|-----|------|------|------|
|
||||
| pwmchip8/pwm1,2 | 20kHz | 50000ns | 电机驱动 (DRV8701 需 ≥ 20kHz) |
|
||||
| pwmchip1/pwm0 | 333Hz | 3000000ns | 舵机 (标准 3ms 周期) |
|
||||
|
||||
### 12. 算力实测 (2K0300 CPU)
|
||||
|
||||
| 精度 | 算力 | 备注 |
|
||||
|------|------|------|
|
||||
| FP32 | 0.43 GFLOPS | 无 SIMD, 纯标量 |
|
||||
| INT8 | 0.44 GOPS | 几乎无加速, 与 FP32 持平 |
|
||||
| INT4 | 0.24 GOPS | 比 INT8 更慢 (位操作开销) |
|
||||
|
||||
**结论**: 2K0300 没有 SIMD/NPU, 纯 CPU 推理极弱。模型需 ≤15M MACs 才能跑 30+ FPS。只适合微型 CNN (2-3 层, 32×32 输入, 8-16 通道) 做简单分类。
|
||||
|
||||
---
|
||||
|
||||
## Demo 程序列表
|
||||
|
||||
| Demo | 功能 |
|
||||
|------|------|
|
||||
| `main/` | **完整自动驾驶巡线** — CameraTimer + MortorTimer + 文件热调 |
|
||||
| `demo1/` | 键盘 WASD 遥控 — 非阻塞终端 |
|
||||
| `image_test/` | 离线图像处理调试器 — 方向键翻页, 7 种显示模式 |
|
||||
| `udp_receive/` | UDP 遥控 (端口 8888) + Python 滑块 GUI |
|
||||
| `encoder_demo/` | 编码器原始读数测试 |
|
||||
| `jy62_demo/` | JY62 IMU 六轴数据读取 |
|
||||
| `key_demo/` | 9 键 GPIO 状态面板 |
|
||||
| `wonderEcho_demo/` | I2C smbus 读写实验 |
|
||||
| `opencv_demo1/` | USB 摄像头 → framebuffer 直显 + FPS |
|
||||
| `opencv_demo2/` | 3D 旋转立方体软件渲染 |
|
||||
| `opencv_demo3/` | Haar 级联人脸检测 |
|
||||
| `framebuffer_demo/` | 纯软件 8×8 字体 + 几何绘图 |
|
||||
| `vl53l0x_test/` | VL53L0X 测距传感器 (ST 官方) |
|
||||
|
||||
---
|
||||
|
||||
## 文件说明
|
||||
|
||||
| 文件 | 用途 |
|
||||
|------|------|
|
||||
| `CMakeLists.txt` | 构建配置: `src/` → 静态库, 各 demo 独立链接 |
|
||||
| `cross.cmake` | 交叉工具链切换 (`CROSS_COMPILE=1`) |
|
||||
| `lib/` | 头文件: GPIO, PWM, PID, Motor, Encoder, Camera, Timer, serial 等 |
|
||||
| `src/` | 实现文件: 每个 `.cpp` → 编译进 `libcommon_lib.a` |
|
||||
@@ -1,18 +0,0 @@
|
||||
#!/bin/bash
|
||||
export PATH=/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6/bin:/usr/bin:/bin
|
||||
CC=loongarch64-linux-gnu-g++
|
||||
SRC_FW=/mnt/d/PPPProgram/smartcar/smartcar_framework
|
||||
SRC_CAR=/mnt/d/PPPProgram/smartcar/smartcar2
|
||||
INC="-I$SRC_FW/model/v10/release -I$SRC_CAR/src -I/mnt/d/PPPProgram/smartcar/opencv_device/include/opencv4"
|
||||
LIB="-L/mnt/d/PPPProgram/smartcar/opencv_device/lib -lopencv_core -lopencv_imgproc -lopencv_imgcodecs -lpthread -lrt"
|
||||
FLAGS="-O3 -std=c++17 -march=loongarch64 -mtune=loongarch64 -ffast-math -funroll-loops -fomit-frame-pointer"
|
||||
|
||||
echo "=== NOSIMD ==="
|
||||
$CC $FLAGS $INC $SRC_FW/tools/v10_demo.cpp $SRC_FW/model/v10/release/model_v10.cpp -o /tmp/vd_nosimd $LIB
|
||||
echo "rc=$?"
|
||||
|
||||
echo "=== LSX ==="
|
||||
$CC $FLAGS -mlsx $INC $SRC_FW/tools/v10_demo.cpp $SRC_FW/model/v10/release/model_v10.cpp -o /tmp/vd_simd $LIB
|
||||
echo "rc=$?"
|
||||
|
||||
ls -la /tmp/vd_nosimd /tmp/vd_simd 2>/dev/null || echo "some binaries missing"
|
||||
@@ -15,14 +15,13 @@ init_pins() {
|
||||
echo out > /sys/class/gpio/gpio12/direction 2>/dev/null
|
||||
echo out > /sys/class/gpio/gpio13/direction 2>/dev/null
|
||||
echo out > /sys/class/gpio/gpio66/direction 2>/dev/null
|
||||
echo out > /sys/class/gpio/gpio67/direction 2>/dev/null
|
||||
echo in > /sys/class/gpio/gpio67/direction 2>/dev/null
|
||||
echo out > /sys/class/gpio/gpio75/direction 2>/dev/null
|
||||
echo in > /sys/class/gpio/gpio72/direction 2>/dev/null
|
||||
echo 1 > /sys/class/gpio/gpio73/value 2>/dev/null
|
||||
echo 0 > /sys/class/gpio/gpio12/value 2>/dev/null
|
||||
echo 0 > /sys/class/gpio/gpio13/value 2>/dev/null
|
||||
echo 0 > /sys/class/gpio/gpio66/value 2>/dev/null
|
||||
echo 0 > /sys/class/gpio/gpio67/value 2>/dev/null
|
||||
|
||||
for ch in 1 2; do
|
||||
echo $ch > /sys/class/pwm/pwmchip${PWMCHIP}/export 2>/dev/null
|
||||
@@ -47,6 +46,18 @@ init_pins() {
|
||||
echo 0 > "$DIR/mortor_kd" 2>/dev/null
|
||||
echo 40 > "$DIR/foresee" 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 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
|
||||
|
||||
echo 10 > "$DIR/brake_scale" 2>/dev/null
|
||||
echo 10000 > "$DIR/brake_max" 2>/dev/null
|
||||
|
||||
echo "[demo] 引脚初始化完成"
|
||||
}
|
||||
|
||||
|
||||
@@ -1,5 +0,0 @@
|
||||
# demo1
|
||||
add_executable(demo1 demo1.cpp)
|
||||
|
||||
# 链接 OpenCV 库
|
||||
target_link_libraries(demo1 common_lib ${OpenCV_LIBS})
|
||||
-125
@@ -1,125 +0,0 @@
|
||||
/*
|
||||
* @Author: ilikara 3435193369@qq.com
|
||||
* @Date: 2025-01-07 02:21:05
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-03-29 01:36:06
|
||||
* @FilePath: /smartcar/demo1/demo1.cpp
|
||||
* @Description: 遥控车Demo，通过键盘控制小车的运动，WASD分别控制前后左右，按下Q退出。由于SSH可能会只捕获到一个按键，所以设定按下WS时清除转向，不按WS时不清除前进后退。
|
||||
*/
|
||||
#include <iostream>
|
||||
#include <unordered_set>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "MotorController.h"
|
||||
#include "PwmController.h"
|
||||
#include "GPIO.h"
|
||||
|
||||
GPIO mortorEN(73);
|
||||
MotorController *motorController[2] = {nullptr, nullptr};
|
||||
PwmController servo(1, 0);
|
||||
|
||||
void Init()
|
||||
{
|
||||
mortorEN.setDirection("out");
|
||||
mortorEN.setValue(1);
|
||||
const int pwmchip[2] = {8, 8};
|
||||
const int pwmnum[2] = {2, 1};
|
||||
const int gpioNum[2] = {12, 13};
|
||||
const int encoder_pwmchip[2] = {0, 3};
|
||||
const int encoder_gpioNum[2] = {75, 72};
|
||||
const int encoder_dir[2] = {1, -1};
|
||||
const unsigned int period_ns = 50000; // 20 kHz
|
||||
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
motorController[i] = new MotorController(pwmchip[i], pwmnum[i], gpioNum[i], period_ns,
|
||||
0, 0, 0, 0,
|
||||
encoder_pwmchip[i], encoder_gpioNum[i], encoder_dir[i]);
|
||||
}
|
||||
|
||||
servo.setPeriod(3000000);
|
||||
servo.setDutyCycle(1500000);
|
||||
servo.enable();
|
||||
}
|
||||
|
||||
// 设置非阻塞输入
|
||||
void setNonBlockingInput(bool enable)
|
||||
{
|
||||
termios tty;
|
||||
tcgetattr(STDIN_FILENO, &tty);
|
||||
if (enable)
|
||||
{
|
||||
tty.c_lflag &= ~ICANON; // 禁用规范模式
|
||||
tty.c_lflag &= ~ECHO; // 禁用回显
|
||||
}
|
||||
else
|
||||
{
|
||||
tty.c_lflag |= ICANON;
|
||||
tty.c_lflag |= ECHO;
|
||||
}
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &tty);
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
std::unordered_set<char> pressedKeys;
|
||||
|
||||
setNonBlockingInput(true);
|
||||
std::cout << "Press keys (WASD). Press 'q' to quit." << std::endl;
|
||||
|
||||
Init();
|
||||
|
||||
while (true)
|
||||
{
|
||||
char ch = getchar();
|
||||
|
||||
if (ch == 'q')
|
||||
{ // 按 'q' 退出
|
||||
break;
|
||||
}
|
||||
|
||||
// 添加按下的键到集合中，并响应按下的键
|
||||
if (ch == 'w' || ch == 'W')
|
||||
{
|
||||
pressedKeys.insert('W');
|
||||
motorController[0]->updateduty(17);
|
||||
motorController[1]->updateduty(17);
|
||||
servo.setDutyCycle(1520000);
|
||||
}
|
||||
else if (ch == 'a' || ch == 'A')
|
||||
{
|
||||
pressedKeys.insert('A');
|
||||
servo.setDutyCycle(1360000);
|
||||
}
|
||||
else if (ch == 's' || ch == 'S')
|
||||
{
|
||||
pressedKeys.insert('S');
|
||||
motorController[0]->updateduty(-17);
|
||||
motorController[1]->updateduty(-17);
|
||||
servo.setDutyCycle(1520000);
|
||||
}
|
||||
else if (ch == 'd' || ch == 'D')
|
||||
{
|
||||
pressedKeys.insert('D');
|
||||
servo.setDutyCycle(1680000);
|
||||
}
|
||||
|
||||
// 显示当前按下的键
|
||||
std::cout << "Pressed Keys: ";
|
||||
for (char key : pressedKeys)
|
||||
{
|
||||
std::cout << key << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
// 清空按下的键集合
|
||||
pressedKeys.clear();
|
||||
|
||||
usleep(10000); // 10ms
|
||||
}
|
||||
|
||||
setNonBlockingInput(false);
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,302 @@
|
||||
# SmartCar 决策总架构：从视频帧到执行器
|
||||
|
||||
## 坐标系统一览
|
||||
|
||||
```
|
||||
Camera 输出 640×480 MJPEG
|
||||
│
|
||||
CONVERT_RGB=0 + IMREAD_REDUCED_COLOR_4
|
||||
→ 1/4 MJPEG 解码 → raw_frame = 160×120
|
||||
│
|
||||
┌───────────┤
|
||||
▼ ▼ (raw_frame.cols/rows 决定分母)
|
||||
巡线空间 显示空间 模型空间
|
||||
(line_tracking (newWidth × newHeight) 160×120 (正常)
|
||||
_width × calc_scale = 2 640×480 (回退模式)
|
||||
line_tracking newWidth = lt_w × 2
|
||||
_height) newHeight = lt_h × 2
|
||||
典型值 80×60
|
||||
```
|
||||
|
||||
**关键换算关系（正常模式 raw_frame = 160×120）：**
|
||||
- 模型空间 → 巡线空间：`col_lt = cx * line_tracking_width / 160`，`row_lt = cy * line_tracking_height / 120`
|
||||
- 巡线空间 → 显示空间：`pixel = value * calc_scale`（calc_scale = 2）
|
||||
- newWidth / newHeight 取决于屏幕物理分辨率，由 `CameraInit` 动态计算
|
||||
- line_tracking_width / height = newWidth/calc_scale, newHeight/calc_scale
|
||||
|
||||
**⚠ 回退模式：** 当 CONVERT_RGB=0 未生效时，raw_frame = 640×480，模型框坐标也在 640×480 空间。
|
||||
此时 LCD 渲染仍硬编码除以 160/120（会出错），但正常运行时不会进入此模式。
|
||||
|
||||
---
|
||||
|
||||
## 主循环 (main.cpp → CameraHandler)
|
||||
|
||||
```
|
||||
main()
|
||||
├── cfg_load_all() # 从工作目录文件读取全部配置
|
||||
├── CameraInit(0, dest_fps, 320, 240) # 打开摄像头, LCD, 模型, I2C
|
||||
├── ControlInit() # 初始化双电机 GPIO + PWM
|
||||
│
|
||||
└── while(running):
|
||||
└── CameraHandler() # ★ 以下逐帧执行
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## CameraHandler 9 步流水线
|
||||
|
||||
```
|
||||
┌─────────────────────┐
|
||||
Step 1 │ cap.read(raw_mat) │ 640×480 MJPEG 原始字节流
|
||||
取帧+解码 │ raw_mat 单通道(字节) │ → IMREAD_REDUCED_COLOR_4
|
||||
│ → cv::imdecode │ → raw_frame = 160×120 BGR
|
||||
│ raw_frame = decoded │ (回退: raw_mat 三通道→直用 640×480)
|
||||
└────────┬────────────┘
|
||||
│
|
||||
Step 2 ┌────────▼────────────┐
|
||||
保存图像 (条件) │ g_cfg.debug==1 │ 写入 ./image/image_XXXXX.jpg
|
||||
│ && saveImg==1 → │ (双重条件，缺一不可)
|
||||
│ saveCameraImage() │
|
||||
└────────┬────────────┘
|
||||
│
|
||||
Step 3 ┌────────▼────────────┐
|
||||
视觉巡线 │ image_main() │ raw_frame → resize(lt_w×lt_h)
|
||||
(每帧都跑) │ │ → HSV双通道Otsu → floodFill
|
||||
│ 输出: left_line[] │ → 逐行最长连续段搜索
|
||||
│ right_line[] │ → 丢线补全(仅 row 10~59)
|
||||
│ mid_line[] │ → track (赛道蒙版 128/0)
|
||||
└────────┬────────────┘
|
||||
│
|
||||
Step 4 ┌────────▼────────────┐
|
||||
模型推理 │ model_v10_detect() │ 每 2 帧推理一次
|
||||
(每 2 帧) │ raw_frame (160×120) │ 4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线
|
||||
│ → g_boxes[16] │ g_thresh = [0.80,0.80,0.80,0.75]
|
||||
│ → g_box_count │ 框坐标 ∈ raw_frame 空间
|
||||
└────────┬────────────┘
|
||||
│
|
||||
Step 5 ┌────────▼────────────┐
|
||||
斑马线状态机 │ 遍历 g_boxes │ 仅处理 cls=3，取第一个斑马线框
|
||||
│ 去抖计数+远近判断 │ NORMAL→STOP(4s)→COOLDOWN(5s)
|
||||
│ │ I2C 语音播报 @ 0x34
|
||||
└────────┬────────────┘
|
||||
│
|
||||
Step 6 ┌────────▼────────────┐
|
||||
舵机控制 │ if g_cfg.start: │
|
||||
│ foresee→check_row │ 偏差 = mid_line[row]×2 - newWidth/2
|
||||
│ mid_line[check_row]│ → g_steer_deviation ∈ [-1, 1]
|
||||
│ → deviation │ → servo duty: 1500000 ± offset
|
||||
│ → deadband 过滤 │ clamp [1.2M, 1.8M] ns
|
||||
│ → servo.setDuty()│ mid_val==255 → 跳过(无效行)
|
||||
└────────┬────────────┘
|
||||
│
|
||||
Step 7 ┌────────▼────────────┐
|
||||
电机控制 │ ControlUpdate( │
|
||||
(开环) │ target_speed, │ if zebra_STOP: duty=0 (刹停)+GPIO73=0
|
||||
│ g_zstate==Z_STOP) │ else: speed × curve
|
||||
│ │ curve = 1 - |deviation|×0.4
|
||||
│ │ curve ∈ [0.6, 1.0]
|
||||
│ │ 左右电机同速(无差速)
|
||||
└────────┬────────────┘
|
||||
│
|
||||
Step 8 ┌────────▼────────────┐
|
||||
LCD 渲染 │ if g_lcd_on: │ g_lcd_on = g_cfg.showImg 的缓存
|
||||
│ track→BGR→ROI │ (每10帧刷新一次缓存)
|
||||
│ + 边界线(红) │ 检测框坐标 bx=newWidth/160 缩放
|
||||
│ + 中线(蓝) │ 状态指示 N(绿)/S(红)/C(黄)
|
||||
│ + 检测框标注 │ → RGB565 → /dev/fb0 mmap
|
||||
│ (L1条状:跳过红灯) │
|
||||
└────────┬────────────┘
|
||||
│
|
||||
Step 9 ┌────────▼────────────┐
|
||||
FPS 统计 │ 每 15 帧输出 │ stdout: "fps=XX.X|rd=X vi=X md=X ct=X ms"
|
||||
│ 分步计时+总帧率 │ 15帧平均
|
||||
└─────────────────────┘
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 视觉巡线深度展开 (image_main)
|
||||
|
||||
```
|
||||
raw_frame (160×120 或 640×480 BGR)
|
||||
│
|
||||
├── cv::resize → (line_tracking_w × line_tracking_h) 典型 80×60
|
||||
│
|
||||
├── image_binerize():
|
||||
│ BGR→HSV → H通道Otsu(THRESH_BINARY_INV) → S通道Otsu → bitwise_or
|
||||
│ 赛道=255(白), 背景=0(黑)
|
||||
│ 原理：蓝底赛道H偏聚集(100~130)、S高；灰路面S低
|
||||
│ bitwise_or 取并集，H或S任一判为赛道即保留
|
||||
│
|
||||
├── find_road():
|
||||
│ MORPH_OPEN(2×2 CROSS) → 种子点(lt_w/2, lt_h-10)
|
||||
│ → 种子点处画实心圆(半径10, 255) 防种子落在黑色区域
|
||||
│ → floodFill(loDiff=20, upDiff=20, 8邻域, newVal=128)
|
||||
│ → mask 提取 ROI → track (赛道内部=128, 外部=0)
|
||||
│
|
||||
├── 逐行最长连续段搜索:
|
||||
│ uchar(*IMG)[lt_w] = track.data
|
||||
│ 每行扫描，找最长连续非零段
|
||||
│ 有多个白段时取最后一个(≥取等，偏右)
|
||||
│ → left_line[row], right_line[row]
|
||||
│ → 全零行: left=right=-1
|
||||
│
|
||||
└── 中线 + 丢线补全 (row = lt_h-1 → 10):
|
||||
有边界: mid = (left+right)/2, int 整除
|
||||
丢线: mid[row] = mid[row+1] (继承下行)
|
||||
若 mid > lt_w/2 → left=mid, right=lt_w-1 (偏右)
|
||||
若 mid ≤ lt_w/2 → left=0, right=mid (偏左)
|
||||
```
|
||||
|
||||
**边界线区间有效性：** row 10~59 有可靠数据；row 0~9 的 mid_line 保持初始值 -1（不可靠，不要写入 box 过滤逻辑）。
|
||||
|
||||
---
|
||||
|
||||
## 舵机控制数学
|
||||
|
||||
```
|
||||
foresee = g_cfg.foresee ← 前瞻行索引（像素，显示空间），默认 40
|
||||
check_row = foresee / calc_scale ← 转为巡线空间行号（calc_scale=2, int 整除）
|
||||
mid_val = mid_line[check_row] ← 该行中线列坐标 (0~79)
|
||||
|
||||
如果 mid_val == 255 → 跳过（丢线补全也写不到 255，仅作防御）
|
||||
否则：
|
||||
deviation = mid_val × 2 - newWidth / 2 ← 转为显示空间偏差(px)
|
||||
deviation -= g_cfg.center_bias ← 中心偏置修正
|
||||
g_steer_deviation = deviation / (newWidth/2) ← 归一化到 [-1, 1]
|
||||
|
||||
if |deviation| < deadband:
|
||||
servo = 1500000 ns (直行)
|
||||
else:
|
||||
norm = deviation / (newWidth/2)
|
||||
offset = norm × steer_gain × 300000
|
||||
duty = clamp(1500000 + offset, 1200000, 1800000)
|
||||
servo.setDutyCycle(duty)
|
||||
```
|
||||
|
||||
**舵机量程：** 1,200,000 ~ 1,800,000 ns，中位 1,500,000 ns，±300,000 ns 对应满偏。
|
||||
**deadband 单位：** 显示空间像素（deviation 的单位）。
|
||||
|
||||
---
|
||||
|
||||
## 电机控制数学
|
||||
|
||||
```
|
||||
ControlUpdate(speed, zebra_block):
|
||||
|
||||
if zebra_block || !g_cfg.start:
|
||||
motor[0].updateduty(0) → 两电机停转
|
||||
motor[1].updateduty(0)
|
||||
if !g_cfg.start: mortorEN.setValue(0) → GPIO73 拉低
|
||||
return
|
||||
|
||||
curve = 1.0 - |g_steer_deviation| × 0.4
|
||||
curve = max(curve, 0.6) ← 最低不低于 60% 速度
|
||||
spd = speed × curve
|
||||
|
||||
motor[0].updateduty(spd) → 左电机 (pwmchip8/pwm2, gpio12 方向)
|
||||
motor[1].updateduty(spd) → 右电机 (pwmchip8/pwm1, gpio13 方向)
|
||||
mortorEN.setValue(1) → GPIO73 拉高使能
|
||||
```
|
||||
|
||||
**motor[0] vs motor[1]：** 左/右区别仅在于 gpioNum（12 vs 13）和 pwm通道（2 vs 1），函数调用完全相同。
|
||||
|
||||
**updateduty(duty) 内部：**
|
||||
- `pwm_period * |duty| / 100` → 转占空比 ns 写入 sysfs
|
||||
- duty > 0 → GPIO 方向 = 1（正转），duty ≤ 0 → GPIO 方向 = 0（反转）
|
||||
|
||||
**速度控制是开环的：** `MotorController` 仅包含 `updateduty()` 方法，无编码器反馈。
|
||||
`PIDController` 类存在但从未被实例化或调用。
|
||||
MotorController.h 中也无 `updateSpeed()` 方法（之前的文档中误记了此函数）。
|
||||
|
||||
---
|
||||
|
||||
## 斑马线状态机
|
||||
|
||||
```
|
||||
检测到 cls=3 → 取第一个斑马线框 → 去抖计数+追踪最远cy
|
||||
|
||||
ZNORMAL 期间:
|
||||
zebra_seen: g_zc_frames++, g_zc_min_cy = min(g_zc_min_cy, zebra_cy)
|
||||
未 seen: g_zc_frames = max(0, g_zc_frames - 2)
|
||||
g_zc_frames==0 → g_zc_min_cy=120 (重置)
|
||||
|
||||
触发条件 (仅 ZNORMAL):
|
||||
zebra_near (cy > g_cfg.zebrasee)
|
||||
&& g_zc_frames >= ZEBRA_MIN_FRAMES (5)
|
||||
&& g_zc_min_cy <= ZEBRA_FAR_CY (50)
|
||||
→ play_zebra_audio() → ZSTOP
|
||||
|
||||
┌──────┐ 条件: 连续5帧 + 起源于远(cy_min≤50) + 当前近(cy>zebrasee)
|
||||
│NORMAL│ ─────────────────────────────────────► ┌──────┐
|
||||
│ │ ◄───────────────────────────────────── │ STOP │
|
||||
└──────┘ 冷却5秒结束 │ │
|
||||
▲ └──┬───┘
|
||||
│ 4秒后 │
|
||||
│ ┌──────────┐ ◄─────────────────────┘
|
||||
└───────────│ COOLDOWN │
|
||||
└──────────┘
|
||||
|
||||
NORMAL: 允许通行，检测斑马线
|
||||
STOP: 刹停 4 秒，g_zstate==Z_STOP 传给 ControlUpdate 第二个参数
|
||||
COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触发）
|
||||
仅跳过检测，不影响其他功能（舵机/巡线正常）
|
||||
```
|
||||
|
||||
**关键变量：**
|
||||
- `g_zc_min_cy`：NORMAL 期间追踪斑马线出现时的最小 cy（越远值越小）。未检测到斑马线时衰减减2/帧，归零后重置为 120。
|
||||
- `ZEBRA_FAR_CY = 50`：斑马线的 cy 必须曾在 ≤50 处出现过（即"从远处来"）
|
||||
- `zebrasee`（默认 60）：当前斑马线 cy > 此值视为"足够近"，触发停车
|
||||
- 去抖衰减速度：未检测到时每次 `-2`（比锥桶设计中的 `-1` 更快下降）
|
||||
- Box 遍历在找到第一个 cls=3 后 `break`，忽略同帧其他斑马线框
|
||||
|
||||
---
|
||||
|
||||
## 配置系统
|
||||
|
||||
所有配置通过工作目录下的纯文本文件读写。文件不存在时 readDoubleFromFile 返回 0：
|
||||
|
||||
| 文件 | 类型 | 代码默认 | ctl.sh 写入 | 含义 |
|
||||
|---|---|---|---|---|
|
||||
| `./speed` | double | 60 | 11 | 目标速度 (% 占空比) |
|
||||
| `./start` | int(0/1) | 0 | 0 | 使能开关，1=电机运行 |
|
||||
| `./debug` | int(0/1) | 0 | (不写入) | 每7帧重载配置+允许存图 |
|
||||
| `./showImg` | int(0/1) | 0 | (不写入) | LCD 显示（每10帧轮询→g_lcd_on） |
|
||||
| `./foresee` | double | 80 | 40 | 舵机前瞻行 (显示空间像素) |
|
||||
| `./deadband` | double | 5 | 8 | 舵机死区 (显示空间像素) |
|
||||
| `./steer_gain` | double | 1.0 | 1.5 | 舵机增益 |
|
||||
| `./center_bias` | double | 0 | (不写入) | 中线偏置修正 |
|
||||
| `./kp`,`./ki`,`./kd` | double | - | 3.5/0.3/2.0 | PID 参数 (当前未使用) |
|
||||
| `./mortor_kp/ki/kd` | double | - | 0.6/0.2/0 | 电机编码器 PID (当前未使用) |
|
||||
| `./zebrasee` | double | 60 | (不写入) | 斑马线近界阈值 (模型空间cy) |
|
||||
| `./destfps` | double | 30* | (不写入) | 目标帧率 (*代码兜底值) |
|
||||
| `./saveImg` | int(0/1) | - | (不写入) | 保存帧图像 (需 debug=1) |
|
||||
|
||||
**debug 模式：** `./debug` = 1 时，主循环每7帧调用一次 `cfg_load_all()` 重读全部配置，支持热更新调参。同时允许 `saveImg` 保存图像。
|
||||
|
||||
**注意**：`global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。
|
||||
|
||||
---
|
||||
|
||||
## 代码中未使用的模块
|
||||
|
||||
以下 `.cpp` 文件已编译进 `common_lib` 但主循环中从未调用：
|
||||
|
||||
| 文件 | 功能 | 状态 |
|
||||
|---|---|---|
|
||||
| `PIDController.cpp` | 位置式/增量式 PID | 类存在但无实例化，无调用 |
|
||||
| `serial.cpp` | VOFA 串口可视化 (vofa_justfloat/vofa_image) | 已实现但无调用 |
|
||||
| `Timer.cpp` | 定时器线程 | 已实现但无调用（Video 依赖它但 Video 也未用） |
|
||||
| `video.cpp` | 视频文件流读取 | 已实现但无调用 |
|
||||
|
||||
---
|
||||
|
||||
## 当前已知缺陷 / 未利用能力
|
||||
|
||||
1. **cls=0 锥桶** — 模型已检测但被忽略（CONE_DESIGN.md 设计了方案）
|
||||
2. **cls=1 红灯 / cls=2 绿灯** — 检测框跳过不画（LCD 渲染中 `if g_boxes[i].cls == 1 || g_boxes[i].cls == 2: continue`），无决策
|
||||
3. **电机开环** — 无编码器反馈，PID 类已实现但无调用点，`MotorController` 无 `updateSpeed()` 方法
|
||||
4. **舵机死区** — 用 `abs(deviation) < deadband` 比较像素值，deadband 单位是显示空间像素
|
||||
5. **中线 row 范围** — 丢线补全仅覆盖 row 10~59，row 0~9 的 mid_line 保持 -1 不可靠
|
||||
6. **mid_val == 255 防御** — 代码中写死但 255 永远不会出现在 mid_line 中（当前实现下）
|
||||
7. **LCD 坐标缩放硬编码** — 检测框绘制用 `newWidth/160.0f`，假设 raw_frame 宽=160。回退模式下 raw_frame=640 时出错
|
||||
@@ -0,0 +1,283 @@
|
||||
# 锥桶避障功能 — 中线变形方案
|
||||
|
||||
## 1. 核心思路
|
||||
|
||||
**不改舵机代码，直接修改 `mid_line[]` 数组。** 在锥桶附近把中线"推开"一个凸起，后续巡线逻辑（deviation → deadband → servo）原样工作，车自然跟着变形后的中线绕开锥桶。
|
||||
|
||||
不使用任何推入计时器、归还逻辑、EMA 平滑——锥桶消失后，下一帧 `image_main()` 重新计算 `mid_line`，自动恢复。
|
||||
|
||||
---
|
||||
|
||||
## 2. 坐标与边界过滤
|
||||
|
||||
```
|
||||
模型空间 (raw_frame):
|
||||
正常模式: 160×120 (MJPEG 1/4解码)
|
||||
回退模式: 640×480
|
||||
|
||||
巡线空间映射:
|
||||
cone_col = box.cx * line_tracking_width / raw_frame.cols (典型 0~79)
|
||||
cone_row = box.cy * line_tracking_height / raw_frame.rows (典型 0~59)
|
||||
|
||||
换算速查 (正常模式 160×120 → 典型 80×60):
|
||||
cone_col = cx / 2
|
||||
cone_row = cy / 2
|
||||
```
|
||||
|
||||
边界过滤 (cls=0, conf ≥ cone_thresh):
|
||||
```
|
||||
row_lt < 10 → 丢弃 (row 0~9 无可靠巡线数据)
|
||||
row_lt >= line_tracking_height → 丢弃
|
||||
left_line[row_lt] == -1 → 丢弃 (该行丢线)
|
||||
right_line[row_lt] == -1 → 丢弃
|
||||
col_lt < left_line[row_lt] → 丢弃 (赛道外)
|
||||
col_lt > right_line[row_lt] → 丢弃 (赛道外)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. 中线变形
|
||||
|
||||
### 3.1 方向
|
||||
|
||||
```
|
||||
mid_at_cone = mid_line[cone_row]
|
||||
|
||||
cone_col < mid_at_cone → 锥在左 → push_mid 向右 (+)
|
||||
cone_col > mid_at_cone → 锥在右 → push_mid 向左 (-)
|
||||
|
||||
direction = (cone_col < mid_at_cone) ? +1 : -1
|
||||
```
|
||||
|
||||
### 3.2 变形区域
|
||||
|
||||
从 `start_row = max(10, cone_row - range)` 到 `cone_row`，共 (cone_row - start_row) 行：
|
||||
|
||||
```
|
||||
row 0 ───────────────────────────────── 远
|
||||
│ (不变形)
|
||||
row 10 ─────────────────────────────────
|
||||
│ start_row ────────────── t=0
|
||||
│ 线性斜坡
|
||||
│ foresee 行在这里看到部分偏移
|
||||
│
|
||||
│ cone_row ────────────── t=1
|
||||
row 59 ───────────────────────────────── 近 (车前方)
|
||||
```
|
||||
|
||||
### 3.3 数学
|
||||
|
||||
```
|
||||
for row in [start_row, cone_row]:
|
||||
t = (row - start_row) / (cone_row - start_row) // [0, 1]
|
||||
|
||||
push = t * cone_avoid_gain * (line_tracking_width / 2) * direction
|
||||
|
||||
mid_line[row] = original_mid_line[row] + push
|
||||
mid_line[row] = clamp(mid_line[row],
|
||||
left_line[row] + 2,
|
||||
right_line[row] - 2)
|
||||
```
|
||||
|
||||
**参数：**
|
||||
- `cone_avoid_gain` (double, 0.3)：归一化推离量。1.0 = 推到半幅赛道宽。默认 0.3 表示推到赛道 30% 宽度处。
|
||||
- `cone_avoid_range` (int, 30)：斜坡覆盖行数。必须足够大使 foresee 行感受到偏移。
|
||||
|
||||
---
|
||||
|
||||
## 4. 数字算例
|
||||
|
||||
**场景：** 直道，锥桶在赛道左侧。`lt_w=80, lt_h=60, cone_avoid_gain=0.3, cone_avoid_range=30`
|
||||
|
||||
```
|
||||
原始数据:
|
||||
cone_col=25, cone_row=50, mid_at_cone=40 (锥在左侧15列)
|
||||
direction = +1 (推中线向右)
|
||||
start_row = max(10, 50-30) = 20
|
||||
half_w = 40
|
||||
|
||||
变形计算:
|
||||
push_max = 0.3 * 40 * 1 = 12 (行50处的最大位移, 巡线空间列)
|
||||
|
||||
row=20 (start, t=0): mid_line = 40 + 0 = 40 → deviation = 40*2-80 = 0 → 舵机中位
|
||||
row=25 (t=5/30=0.17): mid_line = 40 + 2.04 = 42 → deviation = 42*2-80 = +4 → 略右
|
||||
row=35 (t=15/30=0.5): mid_line = 40 + 6.0 = 46 → deviation = 46*2-80 = +12 → 明显右转
|
||||
row=50 (t=1.0): mid_line = 40 + 12 = 52 → deviation = 52*2-80 = +24 → 大幅右转
|
||||
|
||||
foresee=40 (显示空间) → check_row=40/2=20 (巡线空间):
|
||||
mid_line[20] 未被变形 (start_row=20, t=0)
|
||||
→ 舵机看不到! 需要减小 range 或增大 foresee
|
||||
```
|
||||
|
||||
**问题暴露：** `foresee=40` → `check_row=20`，但 `start_row=20` 时 t=0 无偏移。
|
||||
|
||||
**修正：** 确保变形起点在 `check_row` 以上。两种方式：
|
||||
1. 增大 `cone_avoid_range`（如 40），让 start_row 更低
|
||||
2. 让斜坡向上延伸超过 start_row（即从 row=10 就开始）
|
||||
|
||||
**采用方案 2：** 变形始终从 `row=10` 开始，`range` 参数控制斜坡的"陡峭度"而非范围。
|
||||
|
||||
---
|
||||
|
||||
## 5. 修正后的变形公式
|
||||
|
||||
```
|
||||
range = cone_avoid_range // 陡峭度参数 (默认 30)
|
||||
total_rows = cone_row - 10 // 实际跨度
|
||||
|
||||
for row in [10, cone_row]:
|
||||
t_raw = (row - 10.0) / total_rows // 线性 [0, 1]
|
||||
t = clamp(t_raw * (total_rows / range), // 用 range 缩放斜率
|
||||
0.0, 1.0)
|
||||
|
||||
push = t * cone_avoid_gain * (line_tracking_width / 2) * direction
|
||||
mid_line[row] = clamp(original_mid + push,
|
||||
left_line[row] + 2,
|
||||
right_line[row] - 2)
|
||||
```
|
||||
|
||||
**效果（cone_avoid_range=30, cone_row=50, total_rows=40）：**
|
||||
|
||||
```
|
||||
total_rows/range = 40/30 = 1.33 ← 斜率因子 >1, 斜坡更陡
|
||||
|
||||
row=10: t_raw=0, t=0*1.33=0 → push=0
|
||||
row=20: t_raw=0.25, t=0.25*1.33=0.33 → push=0.33*12=4.0 → deviation=+8 (刚好过死区)
|
||||
row=35: t_raw=0.625,t=0.83 → push=10 → deviation=+20
|
||||
row=50: t_raw=1.0, t=1.0 → push=12 → deviation=+24
|
||||
```
|
||||
|
||||
foresee 行 (row=20) 收到 33% 的偏移，产生 +8px 偏差 → 刚好越过 deadband(8px) → 舵机开始右转。
|
||||
|
||||
**调参规则：**
|
||||
- 增大 `cone_avoid_range` → 斜坡更缓 → foresee 行偏移更小 → 转向更柔和
|
||||
- 减小 `cone_avoid_range` → 斜坡更陡 → foresee 行偏移更大 → 转向更猛
|
||||
- 增大 `cone_avoid_gain` → 整体偏移量增大 → 转向更猛
|
||||
|
||||
---
|
||||
|
||||
## 6. 去抖机制
|
||||
|
||||
与之前一致，不改变：
|
||||
|
||||
```
|
||||
#define CONE_MIN_FRAMES 3
|
||||
#define CONE_COUNT_DECAY 1
|
||||
|
||||
g_cone_frames: 连续检测计数
|
||||
g_cone_last_row / g_cone_last_col: 位置跟踪 (容差 ±5行/±10列)
|
||||
g_cone_confirmed = (g_cone_frames >= CONE_MIN_FRAMES)
|
||||
```
|
||||
|
||||
**关键差异：** 去抖只决定"是否触发变形"，变形本身没有持续时间概念。
|
||||
- 锥桶在 → 变形在（每帧重新计算 mid_line 变形）
|
||||
- 锥桶消失 → 下帧 image_main() 重新计算 mid_line → 变形自动消失
|
||||
- 不去抖时的抖动：锥桶在边界附近闪烁会使 mid_line 来回跳，用去抖平滑
|
||||
|
||||
---
|
||||
|
||||
## 7. 舵机与电机
|
||||
|
||||
**舵机：** 零改动。Step 6 读取 `mid_line[foresee/2]`，看到变形后的中线值，自然输出偏转方向。
|
||||
|
||||
**电机：** 推入期间减速：
|
||||
```
|
||||
if (g_cone_confirmed && g_zstate != Z_STOP) {
|
||||
effective_speed = target_speed * cone_speed_factor;
|
||||
} else {
|
||||
effective_speed = target_speed;
|
||||
}
|
||||
ControlUpdate(effective_speed, g_zstate == Z_STOP);
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 8. 参数配置
|
||||
|
||||
### 8.1 启动时一次性读取（运行中不重新读文件）
|
||||
|
||||
| 文件 | 类型 | 默认 | 含义 |
|
||||
|---|---|---|---|
|
||||
| `./cone_avoid_gain` | double | 0.3 | 归一化推离量 (0=不推, 1=推到赛道边缘) |
|
||||
| `./cone_avoid_range` | int | 30 | 斜坡陡峭度 (越小越陡，foresee 行感受越强) |
|
||||
|
||||
### 8.2 热更新参数
|
||||
|
||||
| 文件 | 类型 | 默认 | 含义 |
|
||||
|---|---|---|---|
|
||||
| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 |
|
||||
| `./cone_min_frames` | int | 3 | 连续确认帧数 |
|
||||
| `./cone_margin` | int | 0 | 0=中心点过滤, 1=框边缘过滤 |
|
||||
| `./cone_thresh` | double | 0.80 | 锥桶置信度阈值 (业务层, 不改 NMS) |
|
||||
|
||||
### 8.3 读取实现
|
||||
|
||||
**main.cpp 启动时（一次性）：**
|
||||
```cpp
|
||||
double avoid_gain_val = readDoubleFromFile("./cone_avoid_gain");
|
||||
int avoid_range_val = (int)readDoubleFromFile("./cone_avoid_range");
|
||||
if (avoid_gain_val <= 0) avoid_gain_val = 0.3;
|
||||
if (avoid_range_val <= 0) avoid_range_val = 30;
|
||||
g_cfg.cone_avoid_gain = avoid_gain_val;
|
||||
g_cfg.cone_avoid_range = avoid_range_val;
|
||||
```
|
||||
|
||||
**global.cpp (cfg_load_all，热更新)：**
|
||||
```cpp
|
||||
g_cfg.cone_speed = readDoubleFromFile(cone_speed_file);
|
||||
g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file);
|
||||
g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file);
|
||||
g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_file);
|
||||
```
|
||||
|
||||
**ctl.sh：**
|
||||
```sh
|
||||
echo 0.3 > "$DIR/cone_avoid_gain" 2>/dev/null
|
||||
echo 30 > "$DIR/cone_avoid_range" 2>/dev/null
|
||||
echo 0.5 > "$DIR/cone_speed" 2>/dev/null
|
||||
echo 3 > "$DIR/cone_min_frames" 2>/dev/null
|
||||
echo 0 > "$DIR/cone_margin" 2>/dev/null
|
||||
echo 0.80 > "$DIR/cone_thresh" 2>/dev/null
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 9. 代码修改清单
|
||||
|
||||
```
|
||||
lib/global.h:
|
||||
├── CfgCache 新增: cone_avoid_gain, cone_avoid_range,
|
||||
│ cone_speed, cone_min_frames, cone_margin, cone_thresh
|
||||
└── 新增文件名常量
|
||||
|
||||
src/global.cpp:
|
||||
└── cfg_load_all() 追加热更新参数 (不含 avoid_gain/avoid_range)
|
||||
|
||||
main/main.cpp:
|
||||
└── CameraInit 之前: 读取 cone_avoid_gain, cone_avoid_range 写入 g_cfg
|
||||
|
||||
src/camera.cpp:
|
||||
├── #define CONE_CLASS 0
|
||||
├── 新增静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed
|
||||
├── Step 5 之后: 插入锥桶检测+边界过滤+去抖
|
||||
├── Step 5.5: 中线变形 (修改 mid_line[row] for row in [10, cone_row])
|
||||
├── Step 7 电机: g_cone_confirmed → effective_speed *= cone_speed_factor
|
||||
└── Step 8 LCD: 锥桶框改橙色
|
||||
|
||||
ctl.sh:
|
||||
└── init_pins() 追加 6 个新文件默认值
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 10. 方案对比
|
||||
|
||||
| | 旧方案（舵机推入） | 新方案（中线变形） |
|
||||
|---|---|---|
|
||||
| 舵机代码 | 需改动 | 零改动 |
|
||||
| deadband | 需绕过 | 自 然生效 |
|
||||
| steer_gain | 需单独 cone_gain | 共用 |
|
||||
| 归还机制 | 需要计时器+衰减 | 自动 (image_main 重算) |
|
||||
| 平滑性 | 需 EMA | 斜坡自带平滑 |
|
||||
| 参数数量 | 7 | 6 |
|
||||
| 锥桶消失恢复 | 需衰减逻辑 | 下一帧自动 |
|
||||
@@ -0,0 +1,95 @@
|
||||
# 编码器刹车 — 设计与实现
|
||||
|
||||
## 1. 硬件
|
||||
|
||||
编码器2，引脚：
|
||||
|
||||
| 信号 | 引脚 | 模式 |
|
||||
|---|---|---|
|
||||
| LSB 脉冲 | GPIO 67 | 输入 |
|
||||
| DIR 方向 | GPIO 72 | 输入 |
|
||||
|
||||
两引脚均属 gpiochip64（GPA64，base=64，16 pins）。
|
||||
|
||||
## 2. 触发条件
|
||||
|
||||
`ControlUpdate(speed, zebra_block)` 中 `zebra_block == true` 时进入刹车流程。`zebra_block` 由 `CameraHandler` Step 5 斑马线状态机传入——当 `g_zstate == Z_STOP` 时为 true。
|
||||
|
||||
## 3. 刹车状态机
|
||||
|
||||
```
|
||||
┌──────────┐
|
||||
正常行驶 ───→ │ IDLE │
|
||||
└────┬─────┘
|
||||
│ zebra_block==true
|
||||
▼
|
||||
┌──────────┐
|
||||
│ BRAKING │ ← 施加 -30% 占空比
|
||||
│ │ 每帧读 GPIO 67
|
||||
└────┬─────┘
|
||||
┌───────────┴───────────┐
|
||||
│ 脉冲变化 │ 脉冲不变
|
||||
▼ ▼
|
||||
轮子在转 → 继续刹车 g_brake_still++
|
||||
重置计数器 │
|
||||
连续 5 帧无变化？
|
||||
或超时 60 帧？
|
||||
│
|
||||
▼
|
||||
┌──────────┐
|
||||
│ HOLDING │ ← duty=0，保持静止
|
||||
└──────────┘
|
||||
│
|
||||
zebra_block==false
|
||||
│
|
||||
▼
|
||||
┌──────────┐
|
||||
│ IDLE │
|
||||
└──────────┘
|
||||
```
|
||||
|
||||
## 4. 参数
|
||||
|
||||
定义在 `src/control.cpp`：
|
||||
|
||||
| 常量 | 值 | 含义 |
|
||||
|---|---|---|
|
||||
| `BRAKE_STILL_THRESH` | 5 | 连续无脉冲帧数阈值，超过即判停 |
|
||||
| `BRAKE_TIMEOUT` | 60 | 最长刹车帧数（~2s @30fps），强制释放 |
|
||||
| `BRAKE_DUTY` | -30 | 刹车占空比（-100~100，负值=反转） |
|
||||
|
||||
## 5. 读取方式
|
||||
|
||||
GPIO 67 通过 sysfs 文件描述符轮询（`GPIO::readValue()`），每帧 `lseek` + `read` 一次。不依赖中断。
|
||||
|
||||
**为什么轮询够用：** 30fps 帧率 = 33ms 间隔。编码器假设 100+ 脉冲/转，即使 1 转/秒的极慢速度也有 3+ 脉冲/帧间隔，不会漏判。
|
||||
|
||||
## 6. 刹车强度
|
||||
|
||||
```
|
||||
motor[i]->updateduty(-30)
|
||||
```
|
||||
|
||||
`MotorController::updateduty()` 内：
|
||||
- 占空比：`period * |duty| / 100 = 50000 * 30 / 100 = 15000 ns`
|
||||
- 方向：`duty < 0` → `directionGPIO.setValue(false)` → 反转
|
||||
|
||||
即 30% 占空比的反向驱动，足够刹停但不会让车向后猛冲。
|
||||
|
||||
## 7. 编码器生效范围
|
||||
|
||||
- **仅在斑马线 STOP 时**使用编码器
|
||||
- 正常行驶时编码器处于"打开但未使用"状态（fd 保持 open，但不 read）
|
||||
- 锥桶避障不使用编码器
|
||||
- `g_cfg.start == 0` 时刹车状态机重置
|
||||
|
||||
## 8. 关键代码路径
|
||||
|
||||
```
|
||||
main.cpp
|
||||
└── while(running)
|
||||
└── CameraHandler()
|
||||
└── ControlUpdate(target_speed, g_zstate == Z_STOP)
|
||||
└── if (zebra_block) → 刹车状态机
|
||||
└── encoderLSB.readValue() ← GPIO 67
|
||||
```
|
||||
@@ -0,0 +1,103 @@
|
||||
# 红绿灯识别 — 设计与实现
|
||||
|
||||
## 1. 模型检测
|
||||
|
||||
NanoDet V10 模型 4 类输出中：
|
||||
- cls=1: 红灯
|
||||
- cls=2: 绿灯
|
||||
|
||||
模型置信度阈值由 `g_thresh[1]` 和 `g_thresh[2]` 控制（`src/camera.cpp:25`），当前分别为 0.80。
|
||||
|
||||
## 2. 状态机
|
||||
|
||||
```
|
||||
┌──────────────────────────────────────────┐
|
||||
│ │
|
||||
▼ │
|
||||
┌──────────┐ 红灯连续3帧 ┌──────────┐ │
|
||||
正常行驶 │ TL_NORMAL │ ─────────────→ │ TL_STOP │ │
|
||||
│ │ │ 刹车ON │ │
|
||||
└──────────┘ └────┬─────┘ │
|
||||
▲ │ 红灯消失 │
|
||||
│ ▼ │
|
||||
│ ┌──────────────┐ │
|
||||
│ 绿灯连续3帧 │ TL_WAIT_GREEN │ │
|
||||
└──────────────────│ 刹车ON │ │
|
||||
└──────────────┘ │
|
||||
```
|
||||
|
||||
| 状态 | 刹车 | 锥桶 | 触发条件 |
|
||||
|---|---|---|---|
|
||||
| TL_NORMAL | 无 | 正常 | — |
|
||||
| TL_STOP | ON | 抑制 | 红灯连续 3 帧确认 |
|
||||
| TL_WAIT_GREEN | ON | 抑制 | 红灯从视野消失 |
|
||||
|
||||
**去抖：** 红灯 3 帧确认触发，衰减 `-1`/帧；绿灯 3 帧确认触发，衰减 `-1`/帧。
|
||||
|
||||
**为什么红灯消失后还要等绿灯：** 防止红灯误检消失后误触发通行。确保绿灯确实出现在视野中才放行。
|
||||
|
||||
## 3. 刹车机制
|
||||
|
||||
和斑马线完全一样，复用编码器比例刹车：
|
||||
|
||||
```
|
||||
ControlUpdate(spd, g_zstate == Z_STOP || tl_block)
|
||||
|
||||
刹车信号 = 斑马线STOP || 红灯STOP || 等待绿灯
|
||||
```
|
||||
|
||||
刹车参数（热更新）：
|
||||
|
||||
| 文件 | 默认 | 含义 |
|
||||
|---|---|---|
|
||||
| `./brake_scale` | 10 | 速度(pps) → 刹车占空比(ns) |
|
||||
| `./brake_max` | 10000 | 最大刹车占空比(ns) |
|
||||
|
||||
## 4. 与其他模块的交互
|
||||
|
||||
```
|
||||
决策优先级:
|
||||
Z_STOP (斑马线) > TL_STOP/WAIT_GREEN (红绿灯) > cone (锥桶) > 正常巡线
|
||||
|
||||
锥桶抑制: g_zstate != Z_STOP && g_tl_state == TL_NORMAL 时才运行
|
||||
电机刹车: zebra_STOP || tl_block 任一为 true 即刹车
|
||||
LCD 状态: "R"=红灯停车 "G"=等绿灯, 拼接斑马线状态
|
||||
```
|
||||
|
||||
## 5. 关键代码路径
|
||||
|
||||
```
|
||||
CameraHandler (src/camera.cpp)
|
||||
Step 4: 模型推理 → g_boxes[], g_box_count
|
||||
Step 5: 斑马线状态机
|
||||
Step 5.5: 红绿灯状态机
|
||||
├─ 遍历 g_boxes, 统计 red_seen/green_seen
|
||||
├─ 去抖计数 (3帧确认, 衰减-1/帧)
|
||||
└─ 状态跃迁
|
||||
Step 5.6: 锥桶检测 (g_zstate!=Z_STOP && g_tl_state==TL_NORMAL 时才运行)
|
||||
Step 7: 电机控制
|
||||
ControlUpdate(spd, g_zstate==Z_STOP || tl_block)
|
||||
|
||||
control.cpp:
|
||||
ControlUpdate(speed, zebra_block)
|
||||
└─ zebra_block → 编码器线程读速度 → 比例刹车
|
||||
```
|
||||
|
||||
## 6. 状态变量
|
||||
|
||||
定义在 `src/camera.cpp`：
|
||||
|
||||
```cpp
|
||||
enum TLState { TL_NORMAL, TL_STOP, TL_WAIT_GREEN };
|
||||
static TLState g_tl_state = TL_NORMAL;
|
||||
static int g_tl_red_frames = 0; // 红灯连续计数
|
||||
static int g_tl_green_frames = 0; // 绿灯连续计数
|
||||
```
|
||||
|
||||
## 7. LCD 显示
|
||||
|
||||
状态字拼接：`[红绿灯][斑马线]`
|
||||
- `RN` — 红灯停车 + 正常巡线
|
||||
- `GN` — 等绿灯 + 正常巡线
|
||||
- `RS` — 红灯 + 斑马线同时 (极少见)
|
||||
- `NN` — 全部正常
|
||||
Binary file not shown.
@@ -0,0 +1,122 @@
|
||||
/*
|
||||
* encoder_brake_demo — 编码器刹车测试 v2
|
||||
*
|
||||
* 编码器2 (gpio67脉冲/gpio72方向) → 控制后面两个轮子同时反转
|
||||
* 左后轮: pwmchip8/pwm2, gpio12
|
||||
* 右后轮: pwmchip8/pwm1, gpio13
|
||||
* 使能: gpio73
|
||||
*/
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <cmath>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <ctime>
|
||||
#include <csignal>
|
||||
|
||||
static volatile bool running = true;
|
||||
void sig_handler(int) { running = false; }
|
||||
|
||||
static void write_file(const char *path, const char *val) {
|
||||
int fd = open(path, O_WRONLY);
|
||||
if (fd >= 0) { write(fd, val, strlen(val)); close(fd); }
|
||||
}
|
||||
|
||||
static int read_pin(const char *path) {
|
||||
int fd = open(path, O_RDONLY);
|
||||
char v = '0';
|
||||
if (fd >= 0) { read(fd, &v, 1); close(fd); }
|
||||
return (v == '1') ? 1 : 0;
|
||||
}
|
||||
|
||||
static long now_ns() {
|
||||
struct timespec ts;
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
return ts.tv_sec * 1000000000L + ts.tv_nsec;
|
||||
}
|
||||
|
||||
int main() {
|
||||
signal(SIGINT, sig_handler);
|
||||
signal(SIGTERM, sig_handler);
|
||||
|
||||
printf("=== Encoder Brake Demo v2 ===\n");
|
||||
printf("编码器: gpio67(脉冲) gpio72(方向)\n");
|
||||
printf("左后轮: gpio12 pwm2 | 右后轮: gpio13 pwm1\n");
|
||||
printf("掰任意后轮 → 两轮同时反向制动\n\n");
|
||||
|
||||
write_file("/sys/class/gpio/gpio73/value", "1");
|
||||
|
||||
write_file("/sys/class/gpio/gpio12/direction", "out");
|
||||
write_file("/sys/class/gpio/gpio13/direction", "out");
|
||||
write_file("/sys/class/gpio/gpio67/direction", "in");
|
||||
write_file("/sys/class/gpio/gpio72/direction", "in");
|
||||
|
||||
write_file("/sys/class/pwm/pwmchip8/pwm1/duty_cycle", "0");
|
||||
write_file("/sys/class/pwm/pwmchip8/pwm2/duty_cycle", "0");
|
||||
|
||||
const long WINDOW_NS = 100000000L; // 100ms 测速窗口
|
||||
const int MAX_DUTY = 10000; // 最大占空比 20%
|
||||
const double SCALE = 10.0; // 速度 → 占空比 缩放系数
|
||||
|
||||
int prev_lsb = read_pin("/sys/class/gpio/gpio67/value");
|
||||
int pulse_cnt = 0;
|
||||
long t0 = now_ns();
|
||||
int brake_duty = 0;
|
||||
long last_print = 0;
|
||||
int last_dir = read_pin("/sys/class/gpio/gpio72/value");
|
||||
|
||||
printf("%-8s %8s %8s %6s %8s %s\n",
|
||||
"raw_pulse", "speed", "duty_ns", "enc_dir", "mot_dir", "");
|
||||
|
||||
while (running) {
|
||||
int cur_lsb = read_pin("/sys/class/gpio/gpio67/value");
|
||||
int cur_dir = read_pin("/sys/class/gpio/gpio72/value");
|
||||
|
||||
if (cur_lsb != prev_lsb) {
|
||||
pulse_cnt++;
|
||||
prev_lsb = cur_lsb;
|
||||
}
|
||||
if (cur_dir != last_dir) last_dir = cur_dir;
|
||||
|
||||
long t1 = now_ns();
|
||||
long dt = t1 - t0;
|
||||
|
||||
if (dt >= WINDOW_NS) {
|
||||
double speed = pulse_cnt / (dt / 1e9);
|
||||
|
||||
if (speed > 0.5) {
|
||||
brake_duty = (int)(speed * SCALE);
|
||||
if (brake_duty > MAX_DUTY) brake_duty = MAX_DUTY;
|
||||
|
||||
const char *mdir = cur_dir ? "0" : "1";
|
||||
write_file("/sys/class/gpio/gpio12/value", mdir);
|
||||
write_file("/sys/class/gpio/gpio13/value", mdir);
|
||||
} else {
|
||||
brake_duty = 0;
|
||||
}
|
||||
|
||||
char buf[16];
|
||||
snprintf(buf, sizeof(buf), "%d", brake_duty);
|
||||
write_file("/sys/class/pwm/pwmchip8/pwm1/duty_cycle", buf);
|
||||
write_file("/sys/class/pwm/pwmchip8/pwm2/duty_cycle", buf);
|
||||
|
||||
if (t1 - last_print > 300000000L) {
|
||||
printf("%-8d %8.1f %8d %6d %8s\r",
|
||||
pulse_cnt, speed, brake_duty, cur_dir,
|
||||
cur_dir ? "rev" : "fwd");
|
||||
fflush(stdout);
|
||||
last_print = t1;
|
||||
}
|
||||
|
||||
pulse_cnt = 0;
|
||||
t0 = t1;
|
||||
}
|
||||
}
|
||||
|
||||
write_file("/sys/class/pwm/pwmchip8/pwm1/duty_cycle", "0");
|
||||
write_file("/sys/class/pwm/pwmchip8/pwm2/duty_cycle", "0");
|
||||
write_file("/sys/class/gpio/gpio73/value", "0");
|
||||
printf("\n=== Demo stopped ===\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -1,5 +0,0 @@
|
||||
# demo1
|
||||
add_executable(encoder_demo encoder_demo.cpp)
|
||||
|
||||
# 链接 OpenCV 库
|
||||
target_link_libraries(encoder_demo common_lib ${OpenCV_LIBS})
|
||||
@@ -1,22 +0,0 @@
|
||||
/*
|
||||
* @Author: ilikara 3435193369@qq.com
|
||||
* @Date: 2024-11-30 09:06:41
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-01-08 07:09:44
|
||||
* @FilePath: /smartcar/encoder_demo/encoder_demo.cpp
|
||||
* @Description: 编码器测试用
|
||||
*
|
||||
* Copyright (c) 2024 by ilikara 3435193369@qq.com, All Rights Reserved.
|
||||
*/
|
||||
#include "encoder.h"
|
||||
#include <iostream>
|
||||
int main()
|
||||
{
|
||||
ENCODER encoder(0, 73);
|
||||
while (1)
|
||||
{
|
||||
std::cout << encoder.pulse_counter_update() << std::endl;
|
||||
usleep(5000);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -1,5 +0,0 @@
|
||||
# demo1
|
||||
add_executable(framebuffer_demo framebuffer_demo.cpp)
|
||||
|
||||
# 链接 OpenCV 库
|
||||
target_link_libraries(framebuffer_demo common_lib ${OpenCV_LIBS})
|
||||
@@ -1,207 +0,0 @@
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <linux/fb.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <cstring>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
|
||||
// Framebuffer 设备
|
||||
const char *fb_device = "/dev/fb0";
|
||||
|
||||
// 屏幕分辨率
|
||||
const int screen_width = 160;
|
||||
const int screen_height = 128;
|
||||
|
||||
// RGB565 颜色宏
|
||||
#define RGB565(r, g, b) ((((r) >> 3) << 11) | (((g) >> 2) << 5) | ((b) >> 3))
|
||||
|
||||
// 8x8 位图字体（26 个字母 + 10 个数字）
|
||||
const unsigned char font[36][8] = {
|
||||
// A-Z
|
||||
{0b00111000, 0b01000100, 0b01000100, 0b01111100, 0b01000100, 0b01000100, 0b01000100, 0b00000000}, // A
|
||||
{0b01111000, 0b01000100, 0b01000100, 0b01111000, 0b01000100, 0b01000100, 0b01111000, 0b00000000}, // B
|
||||
{0b00111100, 0b01000010, 0b01000000, 0b01000000, 0b01000000, 0b01000010, 0b00111100, 0b00000000}, // C
|
||||
{0b01111000, 0b01000100, 0b01000100, 0b01000100, 0b01000100, 0b01000100, 0b01111000, 0b00000000}, // D
|
||||
{0b01111110, 0b01000000, 0b01000000, 0b01111000, 0b01000000, 0b01000000, 0b01111110, 0b00000000}, // E
|
||||
{0b01111110, 0b01000000, 0b01000000, 0b01111000, 0b01000000, 0b01000000, 0b01000000, 0b00000000}, // F
|
||||
{0b00111100, 0b01000010, 0b01000000, 0b01001110, 0b01000010, 0b01000010, 0b00111100, 0b00000000}, // G
|
||||
{0b01000100, 0b01000100, 0b01000100, 0b01111100, 0b01000100, 0b01000100, 0b01000100, 0b00000000}, // H
|
||||
{0b00111000, 0b00010000, 0b00010000, 0b00010000, 0b00010000, 0b00010000, 0b00111000, 0b00000000}, // I
|
||||
{0b00011110, 0b00001000, 0b00001000, 0b00001000, 0b00001000, 0b01001000, 0b00110000, 0b00000000}, // J
|
||||
{0b01000100, 0b01001000, 0b01010000, 0b01100000, 0b01010000, 0b01001000, 0b01000100, 0b00000000}, // K
|
||||
{0b01000000, 0b01000000, 0b01000000, 0b01000000, 0b01000000, 0b01000000, 0b01111110, 0b00000000}, // L
|
||||
{0b01000100, 0b01101100, 0b01010100, 0b01010100, 0b01000100, 0b01000100, 0b01000100, 0b00000000}, // M
|
||||
{0b01000100, 0b01100100, 0b01010100, 0b01001100, 0b01000100, 0b01000100, 0b01000100, 0b00000000}, // N
|
||||
{0b00111000, 0b01000100, 0b01000100, 0b01000100, 0b01000100, 0b01000100, 0b00111000, 0b00000000}, // O
|
||||
{0b01111000, 0b01000100, 0b01000100, 0b01111000, 0b01000000, 0b01000000, 0b01000000, 0b00000000}, // P
|
||||
{0b00111000, 0b01000100, 0b01000100, 0b01000100, 0b01010100, 0b01001000, 0b00110100, 0b00000000}, // Q
|
||||
{0b01111000, 0b01000100, 0b01000100, 0b01111000, 0b01010000, 0b01001000, 0b01000100, 0b00000000}, // R
|
||||
{0b00111100, 0b01000010, 0b01000000, 0b00111000, 0b00000100, 0b01000010, 0b00111100, 0b00000000}, // S
|
||||
{0b01111100, 0b00010000, 0b00010000, 0b00010000, 0b00010000, 0b00010000, 0b00010000, 0b00000000}, // T
|
||||
{0b01000100, 0b01000100, 0b01000100, 0b01000100, 0b01000100, 0b01000100, 0b00111000, 0b00000000}, // U
|
||||
{0b01000100, 0b01000100, 0b01000100, 0b01000100, 0b01000100, 0b00101000, 0b00010000, 0b00000000}, // V
|
||||
{0b01000100, 0b01000100, 0b01000100, 0b01010100, 0b01010100, 0b01101100, 0b01000100, 0b00000000}, // W
|
||||
{0b01000100, 0b01000100, 0b00101000, 0b00010000, 0b00101000, 0b01000100, 0b01000100, 0b00000000}, // X
|
||||
{0b01000100, 0b01000100, 0b00101000, 0b00010000, 0b00010000, 0b00010000, 0b00010000, 0b00000000}, // Y
|
||||
{0b01111100, 0b00000100, 0b00001000, 0b00010000, 0b00100000, 0b01000000, 0b01111100, 0b00000000}, // Z
|
||||
// 0-9
|
||||
{0b00111000, 0b01000100, 0b01001100, 0b01010100, 0b01100100, 0b01000100, 0b00111000, 0b00000000}, // 0
|
||||
{0b00010000, 0b00110000, 0b00010000, 0b00010000, 0b00010000, 0b00010000, 0b00111000, 0b00000000}, // 1
|
||||
{0b00111000, 0b01000100, 0b00000100, 0b00001000, 0b00010000, 0b00100000, 0b01111100, 0b00000000}, // 2
|
||||
{0b00111000, 0b01000100, 0b00000100, 0b00011000, 0b00000100, 0b01000100, 0b00111000, 0b00000000}, // 3
|
||||
{0b00001000, 0b00011000, 0b00101000, 0b01001000, 0b01111100, 0b00001000, 0b00001000, 0b00000000}, // 4
|
||||
{0b01111100, 0b01000000, 0b01111000, 0b00000100, 0b00000100, 0b01000100, 0b00111000, 0b00000000}, // 5
|
||||
{0b00111000, 0b01000100, 0b01000000, 0b01111000, 0b01000100, 0b01000100, 0b00111000, 0b00000000}, // 6
|
||||
{0b01111100, 0b00000100, 0b00001000, 0b00010000, 0b00100000, 0b00100000, 0b00100000, 0b00000000}, // 7
|
||||
{0b00111000, 0b01000100, 0b01000100, 0b00111000, 0b01000100, 0b01000100, 0b00111000, 0b00000000}, // 8
|
||||
{0b00111000, 0b01000100, 0b01000100, 0b00111100, 0b00000100, 0b01000100, 0b00111000, 0b00000000} // 9
|
||||
};
|
||||
|
||||
// 绘制一个像素
|
||||
void draw_pixel(ushort *fb_data, int x, int y, ushort color)
|
||||
{
|
||||
if (x >= 0 && x < screen_width && y >= 0 && y < screen_height)
|
||||
{
|
||||
fb_data[y * screen_width + x] = color;
|
||||
}
|
||||
}
|
||||
|
||||
// 绘制一个矩形
|
||||
void draw_rect(ushort *fb_data, int x, int y, int width, int height, ushort color)
|
||||
{
|
||||
for (int i = x; i < x + width; i++)
|
||||
{
|
||||
for (int j = y; j < y + height; j++)
|
||||
{
|
||||
draw_pixel(fb_data, i, j, color);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 绘制一个圆形
|
||||
void draw_circle(ushort *fb_data, int center_x, int center_y, int radius, ushort color)
|
||||
{
|
||||
for (int y = -radius; y <= radius; y++)
|
||||
{
|
||||
for (int x = -radius; x <= radius; x++)
|
||||
{
|
||||
if (x * x + y * y <= radius * radius)
|
||||
{
|
||||
draw_pixel(fb_data, center_x + x, center_y + y, color);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 绘制一个字符
|
||||
void draw_char(ushort *fb_data, int x, int y, char c, ushort color)
|
||||
{
|
||||
int index = -1;
|
||||
if (c >= 'A' && c <= 'Z')
|
||||
{
|
||||
index = c - 'A'; // A-Z 对应 0-25
|
||||
}
|
||||
if (c >= 'a' && c <= 'z')
|
||||
{
|
||||
index = c - 'a'; // A-Z 对应 0-25
|
||||
}
|
||||
else if (c >= '0' && c <= '9')
|
||||
{
|
||||
index = c - '0' + 26; // 0-9 对应 26-35
|
||||
}
|
||||
|
||||
if (index >= 0 && index < 36)
|
||||
{
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
for (int j = 0; j < 8; j++)
|
||||
{
|
||||
if (font[index][i] & (1 << (7 - j)))
|
||||
{
|
||||
draw_pixel(fb_data, x + j, y + i, color);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 绘制字符串
|
||||
void draw_string(ushort *fb_data, int x, int y, const char *str, ushort color)
|
||||
{
|
||||
while (*str)
|
||||
{
|
||||
draw_char(fb_data, x, y, *str, color);
|
||||
x += 8; // 每个字符占 8 像素宽度
|
||||
str++;
|
||||
}
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// 打开 Framebuffer 设备
|
||||
int fb_fd = open(fb_device, O_RDWR);
|
||||
if (fb_fd == -1)
|
||||
{
|
||||
cerr << "Error: Cannot open framebuffer device" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 获取 Framebuffer 信息
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo))
|
||||
{
|
||||
cerr << "Error: Cannot get framebuffer information" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 检查 Framebuffer 是否支持 RGB565
|
||||
if (vinfo.bits_per_pixel != 16)
|
||||
{
|
||||
cerr << "Error: Framebuffer is not RGB565 format" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 映射 Framebuffer 到内存
|
||||
size_t fb_size = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;
|
||||
ushort *fb_data = (ushort *)mmap(0, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb_fd, 0);
|
||||
if (fb_data == MAP_FAILED)
|
||||
{
|
||||
cerr << "Error: Failed to map framebuffer to memory" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 清空屏幕（填充黑色）
|
||||
memset(fb_data, 0, fb_size);
|
||||
|
||||
// 绘制渐变背景
|
||||
for (int y = 0; y < screen_height; y++)
|
||||
{
|
||||
for (int x = 0; x < screen_width; x++)
|
||||
{
|
||||
ushort color = RGB565(x, y, (x + y) / 2);
|
||||
draw_pixel(fb_data, x, y, color);
|
||||
}
|
||||
}
|
||||
|
||||
// 绘制一个矩形
|
||||
draw_rect(fb_data, 20, 20, 50, 30, RGB565(255, 0, 0)); // 红色矩形
|
||||
|
||||
// 绘制一个圆形
|
||||
draw_circle(fb_data, 100, 80, 20, RGB565(0, 255, 0)); // 绿色圆形
|
||||
|
||||
// 绘制一段文字
|
||||
draw_string(fb_data, 10, 100, "Hello World", RGB565(255, 255, 255)); // 白色文字
|
||||
|
||||
// 释放资源
|
||||
munmap(fb_data, fb_size);
|
||||
close(fb_fd);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,2 +0,0 @@
|
||||
add_executable(gd13_demo gd13_demo.cpp)
|
||||
target_link_libraries(gd13_demo common_lib ${OpenCV_LIBS})
|
||||
@@ -1,103 +0,0 @@
|
||||
/*
|
||||
* gd13_demo — 斑马线检测演示 (调用 zebra_detect.h)
|
||||
*/
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <iostream>
|
||||
#include <cstring>
|
||||
#include <csignal>
|
||||
#include <atomic>
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/fb.h>
|
||||
|
||||
#include "zebra_detect.h"
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
|
||||
static atomic<bool> g_running{true};
|
||||
static void signal_handler(int) { g_running.store(false); }
|
||||
|
||||
static uint16_t rgb565(uint8_t r, uint8_t g, uint8_t b)
|
||||
{
|
||||
return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
|
||||
}
|
||||
|
||||
static void display_on_fb(const Mat &bgr, uint16_t *fb_buf, int scr_w, int scr_h)
|
||||
{
|
||||
Mat display;
|
||||
resize(bgr, display, Size(scr_w, scr_h));
|
||||
for (int y = 0; y < scr_h; ++y)
|
||||
for (int x = 0; x < scr_w; ++x)
|
||||
{
|
||||
Vec3b px = display.at<Vec3b>(y, x);
|
||||
fb_buf[y * scr_w + x] = rgb565(px[2], px[1], px[0]);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
bool no_display = false;
|
||||
for (int i = 1; i < argc; ++i)
|
||||
if (strcmp(argv[i], "--no-display") == 0) no_display = true;
|
||||
|
||||
signal(SIGINT, signal_handler);
|
||||
signal(SIGTERM, signal_handler);
|
||||
|
||||
VideoCapture cap(0);
|
||||
cap.open(0, CAP_V4L2);
|
||||
if (!cap.isOpened()) cap.open(0);
|
||||
if (!cap.isOpened()) { cerr << "摄像头打开失败" << endl; return 1; }
|
||||
printf("摄像头已打开\n");
|
||||
|
||||
int fb_fd = -1;
|
||||
uint16_t *fb_buf = nullptr;
|
||||
int scr_w = 0, scr_h = 0;
|
||||
if (!no_display)
|
||||
{
|
||||
fb_fd = open("/dev/fb0", O_RDWR);
|
||||
if (fb_fd >= 0)
|
||||
{
|
||||
struct fb_var_screeninfo vinfo;
|
||||
ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo);
|
||||
scr_w = vinfo.xres; scr_h = vinfo.yres;
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * 2;
|
||||
fb_buf = (uint16_t *)mmap(nullptr, fb_size, PROT_READ|PROT_WRITE,
|
||||
MAP_SHARED, fb_fd, 0);
|
||||
}
|
||||
}
|
||||
|
||||
Mat frame, result;
|
||||
printf("开始斑马线检测...\n");
|
||||
|
||||
while (g_running)
|
||||
{
|
||||
if (!cap.read(frame) || frame.empty()) { usleep(5000); continue; }
|
||||
|
||||
ZebraResult zr = detect_zebra_crossing(frame);
|
||||
|
||||
result = frame.clone();
|
||||
if (zr.detected)
|
||||
{
|
||||
putText(result, "ZEBRA! dist=" + to_string(zr.distance_px) + "px",
|
||||
Point(10, 30), FONT_HERSHEY_SIMPLEX, 0.8,
|
||||
Scalar(0, 0, 255), 2);
|
||||
printf("ZEBRA detected | distance=%d px\n", zr.distance_px);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("not detected | distance=%d\n", zr.distance_px);
|
||||
}
|
||||
|
||||
if (!no_display && fb_buf)
|
||||
display_on_fb(result, fb_buf, scr_w, scr_h);
|
||||
}
|
||||
|
||||
cap.release();
|
||||
if (fb_buf) munmap(fb_buf, scr_w * scr_h * 2);
|
||||
if (fb_fd >= 0) close(fb_fd);
|
||||
return 0;
|
||||
}
|
||||
@@ -1,5 +0,0 @@
|
||||
# demo2
|
||||
add_executable(image_test image_test.cpp)
|
||||
|
||||
# 链接 OpenCV 库
|
||||
target_link_libraries(image_test common_lib ${OpenCV_LIBS})
|
||||
@@ -1,420 +0,0 @@
|
||||
/*
|
||||
* @Author: ilikara 3435193369@qq.com
|
||||
* @Date: 2025-01-07 06:26:01
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-03-26 12:26:22
|
||||
* @FilePath: /smartcar/opencv_demo2/opencv_demo2.cpp
|
||||
* @Description:
|
||||
*/
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <dirent.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
#include <cstdlib>
|
||||
#include <cstdio>
|
||||
#include <algorithm>
|
||||
#include <sstream>
|
||||
#include <fcntl.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/mman.h>
|
||||
#include <linux/fb.h>
|
||||
|
||||
#include "image_cv.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
using ImageCallback = void (*)(const string &fullpath);
|
||||
|
||||
const int cameraWidth = 320, cameraHeight = 240;
|
||||
int screenWidth, screenHeight;
|
||||
const int calc_scale = 2;
|
||||
int newWidth, newHeight;
|
||||
|
||||
ImageCallback image_callback = nullptr;
|
||||
string base_path;
|
||||
|
||||
struct termios orig_termios;
|
||||
map<int, string> file_map;
|
||||
int current_num = -1;
|
||||
|
||||
const char *fb_device = "/dev/fb0";
|
||||
int fb_fd;
|
||||
ushort *fb_data;
|
||||
struct fb_var_screeninfo vinfo;
|
||||
|
||||
std::chrono::_V2::system_clock::time_point frame_begin, frame_end;
|
||||
std::chrono::duration<double> frame_duration;
|
||||
|
||||
int displayMode = 0;
|
||||
const int displayModeCount = 7;
|
||||
|
||||
void imgInit()
|
||||
{
|
||||
// 动态设置屏幕分辨率
|
||||
screenWidth = vinfo.xres;
|
||||
screenHeight = vinfo.yres;
|
||||
|
||||
// 计算 newWidth 和 newHeight，确保图像适应屏幕
|
||||
double widthRatio = static_cast<double>(screenWidth) / cameraWidth;
|
||||
double heightRatio = static_cast<double>(screenHeight) / cameraHeight;
|
||||
double scale = std::min(widthRatio, heightRatio); // 选择较小的比例，确保图像不超出屏幕
|
||||
|
||||
newWidth = static_cast<int>(cameraWidth * scale);
|
||||
newHeight = static_cast<int>(cameraHeight * scale);
|
||||
|
||||
line_tracking_width = newWidth / calc_scale;
|
||||
line_tracking_height = newHeight / calc_scale;
|
||||
}
|
||||
|
||||
// 将 RGB888 转换为 RGB565
|
||||
ushort rgb888_to_rgb565(const cv::Vec3b &color)
|
||||
{
|
||||
return ((color[2] >> 3) << 11) | ((color[1] >> 2) << 5) | (color[0] >> 3);
|
||||
}
|
||||
|
||||
cv::Mat img;
|
||||
|
||||
void display()
|
||||
{
|
||||
cv::Mat fbMat(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0));
|
||||
cv::Mat resizedFrame;
|
||||
cv::Mat coloredResizedFrame;
|
||||
|
||||
// 将图像从 BGR 转换为 HSV
|
||||
cv::Mat hsvImage;
|
||||
cv::cvtColor(img, hsvImage, cv::COLOR_BGR2HSV);
|
||||
|
||||
// 分离 HSV 通道
|
||||
std::vector<cv::Mat> hsvChannels;
|
||||
cv::split(hsvImage, hsvChannels);
|
||||
|
||||
cv::Mat bframe;
|
||||
switch (displayMode)
|
||||
{
|
||||
case 0:
|
||||
// 缩放视频到新尺寸
|
||||
cv::resize(binarizedFrame, resizedFrame, cv::Size(newWidth, newHeight));
|
||||
// 将单通道的二值化图像转换为三通道的彩色图像
|
||||
cv::cvtColor(resizedFrame, coloredResizedFrame, cv::COLOR_GRAY2BGR); // 转换为彩色图像
|
||||
break;
|
||||
case 1:
|
||||
cv::resize(raw_frame, coloredResizedFrame, cv::Size(newWidth, newHeight));
|
||||
break;
|
||||
case 2:
|
||||
// 缩放视频到新尺寸
|
||||
cv::resize(morphologyExFrame, resizedFrame, cv::Size(newWidth, newHeight));
|
||||
// 将单通道的二值化图像转换为三通道的彩色图像
|
||||
cv::cvtColor(resizedFrame, coloredResizedFrame, cv::COLOR_GRAY2BGR); // 转换为彩色图像
|
||||
break;
|
||||
case 3:
|
||||
case 4:
|
||||
case 5:
|
||||
// 缩放视频到新尺寸
|
||||
cv::resize(hsvChannels[displayMode - 3], resizedFrame, cv::Size(newWidth, newHeight));
|
||||
cv::threshold(resizedFrame, bframe, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);
|
||||
// 将单通道的二值化图像转换为三通道的彩色图像
|
||||
cv::cvtColor(bframe, coloredResizedFrame, cv::COLOR_GRAY2BGR); // 转换为彩色图像
|
||||
break;
|
||||
case 6:
|
||||
// 缩放视频到新尺寸
|
||||
cv::resize(hsvImage, resizedFrame, cv::Size(newWidth, newHeight));
|
||||
cv::Mat yellowMask;
|
||||
inRange(resizedFrame, cv::Scalar(10, 100, 100), cv::Scalar(40, 255, 255), yellowMask);
|
||||
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2, 2));
|
||||
morphologyEx(yellowMask, yellowMask, cv::MORPH_OPEN, kernel);
|
||||
morphologyEx(yellowMask, yellowMask, cv::MORPH_CLOSE, kernel);
|
||||
bframe = yellowMask;
|
||||
// 将单通道的二值化图像转换为三通道的彩色图像
|
||||
cv::cvtColor(bframe, coloredResizedFrame, cv::COLOR_GRAY2BGR); // 转换为彩色图像
|
||||
break;
|
||||
}
|
||||
|
||||
// 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
|
||||
fbMat.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
|
||||
cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
|
||||
coloredResizedFrame.copyTo(fbMat(roi));
|
||||
|
||||
// 绘制左右边界线和中线
|
||||
int scaledLeftX, scaledRightX, scaledMidX, scaledLeftFilteredX, scaledRightFilteredX, scaledMidFilteredX, scaledY;
|
||||
|
||||
cv::line(fbMat(roi), cv::Point(newWidth / 2, 0), cv::Point(newWidth / 2, newHeight), cv::Scalar(0, 0, 0), 1);
|
||||
cv::line(fbMat(roi), cv::Point(0, 40), cv::Point(newWidth, 40), cv::Scalar(0, 0, 0), 1);
|
||||
for (int y = 0; y < line_tracking_height; y++)
|
||||
{
|
||||
// 根据缩放比例调整X坐标
|
||||
scaledLeftX = static_cast<int>(left_line[y] * calc_scale);
|
||||
scaledRightX = static_cast<int>(right_line[y] * calc_scale);
|
||||
scaledMidX = static_cast<int>(mid_line[y] * calc_scale);
|
||||
scaledLeftFilteredX = static_cast<int>(left_line_filtered[y] * calc_scale);
|
||||
scaledRightFilteredX = static_cast<int>(right_line_filtered[y] * calc_scale);
|
||||
scaledMidFilteredX = static_cast<int>(mid_line_filtered[y] * calc_scale);
|
||||
scaledY = static_cast<int>(y * calc_scale);
|
||||
|
||||
cv::line(fbMat(roi), cv::Point(scaledLeftX, scaledY), cv::Point(scaledLeftX, scaledY), cv::Scalar(0, 0, 255), calc_scale);
|
||||
cv::line(fbMat(roi), cv::Point(scaledLeftFilteredX, scaledY), cv::Point(scaledLeftFilteredX, scaledY), cv::Scalar(255, 255, 0), calc_scale);
|
||||
|
||||
cv::line(fbMat(roi), cv::Point(scaledRightX, scaledY), cv::Point(scaledRightX, scaledY), cv::Scalar(0, 255, 0), calc_scale);
|
||||
cv::line(fbMat(roi), cv::Point(scaledRightFilteredX, scaledY), cv::Point(scaledRightFilteredX, scaledY), cv::Scalar(255, 0, 255), calc_scale);
|
||||
|
||||
cv::line(fbMat(roi), cv::Point(scaledMidX, scaledY), cv::Point(scaledMidX, scaledY), cv::Scalar(255, 0, 0), calc_scale);
|
||||
cv::line(fbMat(roi), cv::Point(scaledMidFilteredX, scaledY), cv::Point(scaledMidFilteredX, scaledY), cv::Scalar(0, 255, 255), calc_scale);
|
||||
}
|
||||
|
||||
for (int y = 0; y < fbMat.rows; y++)
|
||||
{
|
||||
for (int x = 0; x < fbMat.cols; x++)
|
||||
{
|
||||
cv::Vec3b color = fbMat.at<cv::Vec3b>(y, x);
|
||||
fb_data[y * vinfo.xres + x] = rgb888_to_rgb565(color);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void defaultImageCallback(const string &fullpath)
|
||||
{
|
||||
img = cv::imread(fullpath);
|
||||
if (img.empty())
|
||||
{
|
||||
cerr << "Error: Cannot grab frame" << endl;
|
||||
return;
|
||||
}
|
||||
raw_frame = img;
|
||||
|
||||
frame_begin = std::chrono::high_resolution_clock::now();
|
||||
|
||||
image_main();
|
||||
|
||||
frame_end = std::chrono::high_resolution_clock::now();
|
||||
frame_duration = frame_end - frame_begin;
|
||||
|
||||
display();
|
||||
}
|
||||
|
||||
void triggerCallback(int num)
|
||||
{
|
||||
if (!image_callback)
|
||||
return;
|
||||
|
||||
auto it = file_map.find(num);
|
||||
if (it != file_map.end())
|
||||
{
|
||||
string fullpath = base_path + "/" + it->second;
|
||||
image_callback(fullpath);
|
||||
}
|
||||
}
|
||||
|
||||
int fbInit()
|
||||
{
|
||||
// Framebuffer 设备
|
||||
fb_fd = open(fb_device, O_RDWR);
|
||||
if (fb_fd == -1)
|
||||
{
|
||||
cerr << "Error: Cannot open framebuffer device" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 获取 Framebuffer 信息
|
||||
if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo))
|
||||
{
|
||||
cerr << "Error: Cannot get framebuffer information" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 检查 Framebuffer 是否支持 RGB565
|
||||
if (vinfo.bits_per_pixel != 16)
|
||||
{
|
||||
cerr << "Error: Framebuffer is not RGB565 format" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 映射 Framebuffer 到内存
|
||||
size_t fb_size = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;
|
||||
fb_data = (ushort *)mmap(0, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb_fd, 0);
|
||||
if (fb_data == MAP_FAILED)
|
||||
{
|
||||
cerr << "Error: Failed to map framebuffer to memory" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void disableRawMode()
|
||||
{
|
||||
tcsetattr(STDIN_FILENO, TCSAFLUSH, &orig_termios);
|
||||
}
|
||||
|
||||
void enableRawMode()
|
||||
{
|
||||
tcgetattr(STDIN_FILENO, &orig_termios);
|
||||
atexit(disableRawMode);
|
||||
struct termios raw = orig_termios;
|
||||
raw.c_lflag &= ~(ICANON | ECHO);
|
||||
tcsetattr(STDIN_FILENO, TCSAFLUSH, &raw);
|
||||
}
|
||||
|
||||
bool collectImages(const char *path)
|
||||
{
|
||||
base_path = path;
|
||||
DIR *dir = opendir(path);
|
||||
if (!dir)
|
||||
{
|
||||
cerr << "无法打开目录: " << path << endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
struct dirent *entry;
|
||||
while ((entry = readdir(dir)) != nullptr)
|
||||
{
|
||||
string filename = entry->d_name;
|
||||
if (filename.length() != 15)
|
||||
continue;
|
||||
if (filename.substr(0, 6) != "image_")
|
||||
continue;
|
||||
if (filename.substr(11, 4) != ".jpg")
|
||||
continue;
|
||||
|
||||
string num_str = filename.substr(6, 5);
|
||||
if (num_str.find_first_not_of("0123456789") != string::npos)
|
||||
continue;
|
||||
|
||||
int num = stoi(num_str);
|
||||
file_map[num] = filename;
|
||||
}
|
||||
|
||||
closedir(dir);
|
||||
return !file_map.empty();
|
||||
}
|
||||
|
||||
void showHelp()
|
||||
{
|
||||
cout << "控制命令:\n"
|
||||
<< " 左方向键 - 上一张图片\n"
|
||||
<< " 右方向键 - 下一张图片\n"
|
||||
<< " g - 跳转到指定序号\n"
|
||||
<< " q - 退出程序\n";
|
||||
}
|
||||
|
||||
void jumpToNumber()
|
||||
{
|
||||
disableRawMode();
|
||||
cout << "\n输入要跳转的图片序号: ";
|
||||
string input;
|
||||
getline(cin, input);
|
||||
tcflush(STDIN_FILENO, TCIFLUSH);
|
||||
|
||||
try
|
||||
{
|
||||
int target = stoi(input);
|
||||
auto it = file_map.find(target);
|
||||
if (it != file_map.end())
|
||||
{
|
||||
current_num = target;
|
||||
triggerCallback(current_num);
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "未找到序号: " << target << endl;
|
||||
}
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
cout << "无效输入" << endl;
|
||||
}
|
||||
enableRawMode();
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
if (fbInit())
|
||||
{
|
||||
cerr << "Framebuffer初始化失败" << endl;
|
||||
return 1;
|
||||
}
|
||||
imgInit();
|
||||
const char *path = argc > 1 ? argv[1] : ".";
|
||||
if (!collectImages(path))
|
||||
{
|
||||
cerr << "未找到符合要求的图片文件" << endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
enableRawMode();
|
||||
// 设置回调函数
|
||||
image_callback = defaultImageCallback;
|
||||
|
||||
// 初始化时触发第一次回调
|
||||
current_num = file_map.begin()->first;
|
||||
triggerCallback(current_num);
|
||||
showHelp();
|
||||
|
||||
while (true)
|
||||
{
|
||||
auto it = file_map.find(current_num);
|
||||
if (it == file_map.end())
|
||||
{
|
||||
cerr << "当前文件不可用" << endl;
|
||||
break;
|
||||
}
|
||||
|
||||
cout << "\r当前图片: " << it->second << " (序号: " << current_num << ")" << "计算耗时：" << frame_duration.count() << "秒" << " 当前模式: " << displayMode << flush;
|
||||
|
||||
char c;
|
||||
if (read(STDIN_FILENO, &c, 1) == 1)
|
||||
{
|
||||
if (c == '\033')
|
||||
{ // ESC序列
|
||||
char seq[2];
|
||||
if (read(STDIN_FILENO, seq, 2) == 2)
|
||||
{
|
||||
if (seq[0] == '[')
|
||||
{
|
||||
auto next_it = file_map.upper_bound(current_num);
|
||||
auto prev_it = file_map.lower_bound(current_num);
|
||||
|
||||
switch (seq[1])
|
||||
{
|
||||
case 'D': // 左方向键
|
||||
if (prev_it != file_map.begin())
|
||||
{
|
||||
current_num = (--prev_it)->first;
|
||||
triggerCallback(current_num);
|
||||
}
|
||||
break;
|
||||
case 'C': // 右方向键
|
||||
if (next_it != file_map.end())
|
||||
{
|
||||
current_num = next_it->first;
|
||||
triggerCallback(current_num);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (c == 'q')
|
||||
{
|
||||
break;
|
||||
}
|
||||
else if (c == 'g')
|
||||
{
|
||||
jumpToNumber();
|
||||
}
|
||||
else if (c == 't')
|
||||
{
|
||||
displayMode = (displayMode + 1) % displayModeCount;
|
||||
display();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
disableRawMode();
|
||||
cout << "\n程序已退出" << endl;
|
||||
return 0;
|
||||
}
|
||||
@@ -1,5 +0,0 @@
|
||||
# demo1
|
||||
add_executable(jy62_demo jy62_demo.cpp)
|
||||
|
||||
# 链接 OpenCV 库
|
||||
target_link_libraries(jy62_demo)
|
||||
@@ -1,182 +0,0 @@
|
||||
/*
|
||||
* @Author: Ilikara 3435193369@qq.com
|
||||
* @Date: 2025-01-26 17:00:24
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-02-28 08:42:56
|
||||
* @FilePath: /2k300_smartcar/jy62_demo/jy62_demo.cpp
|
||||
* @Description:
|
||||
*
|
||||
* Copyright (c) 2025 by ${git_name_email}, All Rights Reserved.
|
||||
*/
|
||||
#include <iostream>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <cstring>
|
||||
#include <vector>
|
||||
#include <cstdint>
|
||||
#include <termios.h>
|
||||
#include <atomic>
|
||||
|
||||
const size_t PACKET_LENGTH = 11;
|
||||
|
||||
const uint8_t HEADER = 0x55;
|
||||
|
||||
std::atomic<int16_t> accel[3], gyro[3], angle[3];
|
||||
|
||||
uint8_t calculateChecksum(const std::vector<uint8_t> &data)
|
||||
{
|
||||
uint8_t checksum = 0;
|
||||
for (uint8_t byte : data)
|
||||
{
|
||||
checksum += byte;
|
||||
}
|
||||
return checksum;
|
||||
}
|
||||
|
||||
bool parsePacket(const std::vector<uint8_t> &packet)
|
||||
{
|
||||
if (packet[0] != HEADER)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (packet[1] > 0x53 || packet[1] < 0x51)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t receivedChecksum = packet.back();
|
||||
uint8_t calculatedChecksum = calculateChecksum(std::vector<uint8_t>(packet.begin(), packet.end() - 1));
|
||||
|
||||
return receivedChecksum == calculatedChecksum;
|
||||
}
|
||||
|
||||
bool setupSerialPort(int fd, int baudRate)
|
||||
{
|
||||
struct termios tty;
|
||||
memset(&tty, 0, sizeof tty);
|
||||
|
||||
// 获取当前串口配置
|
||||
if (tcgetattr(fd, &tty) != 0)
|
||||
{
|
||||
std::cerr << "Error getting terminal attributes" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 设置输入输出波特率
|
||||
cfsetospeed(&tty, baudRate);
|
||||
cfsetispeed(&tty, baudRate);
|
||||
|
||||
// 设置数据位为 8 位
|
||||
tty.c_cflag &= ~CSIZE;
|
||||
tty.c_cflag |= CS8;
|
||||
|
||||
// 禁用奇偶校验
|
||||
tty.c_cflag &= ~PARENB;
|
||||
|
||||
// 设置停止位为 1 位
|
||||
tty.c_cflag &= ~CSTOPB;
|
||||
|
||||
// 禁用硬件流控制
|
||||
tty.c_cflag &= ~CRTSCTS;
|
||||
|
||||
// 启用接收和本地模式
|
||||
tty.c_cflag |= CREAD | CLOCAL;
|
||||
|
||||
// 禁用软件流控制
|
||||
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
|
||||
|
||||
// 设置原始模式（禁用规范模式）
|
||||
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
||||
|
||||
// 禁用输出处理
|
||||
tty.c_oflag &= ~OPOST;
|
||||
|
||||
// 设置读取超时和最小读取字符数
|
||||
tty.c_cc[VMIN] = 1; // 至少读取 1 个字符
|
||||
tty.c_cc[VTIME] = 5; // 等待 0.5 秒
|
||||
|
||||
// 应用配置
|
||||
if (tcsetattr(fd, TCSANOW, &tty) != 0)
|
||||
{
|
||||
std::cerr << "Error setting terminal attributes" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void data_handler(const std::vector<uint8_t> &packet)
|
||||
{
|
||||
switch (packet[1])
|
||||
{
|
||||
case 0x51:
|
||||
accel[0].store((int16_t)packet[3] << 8 | packet[2]);
|
||||
accel[1].store((int16_t)packet[5] << 8 | packet[4]);
|
||||
accel[2].store((int16_t)packet[7] << 8 | packet[6]);
|
||||
break;
|
||||
case 0x52:
|
||||
gyro[0].store((int16_t)packet[3] << 8 | packet[2]);
|
||||
gyro[1].store((int16_t)packet[5] << 8 | packet[4]);
|
||||
gyro[2].store((int16_t)packet[7] << 8 | packet[6]);
|
||||
break;
|
||||
case 0x53:
|
||||
angle[0].store((int16_t)packet[3] << 8 | packet[2]);
|
||||
angle[1].store((int16_t)packet[5] << 8 | packet[4]);
|
||||
angle[2].store((int16_t)packet[7] << 8 | packet[6]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
const char *device = "/dev/ttyS2";
|
||||
int fd = open(device, O_RDWR | O_NOCTTY);
|
||||
if (fd < 0)
|
||||
{
|
||||
std::cerr << "Failed to open " << device << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!setupSerialPort(fd, B115200))
|
||||
{
|
||||
std::cerr << "Failed to setup serial port" << std::endl;
|
||||
close(fd);
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> buffer;
|
||||
while (true)
|
||||
{
|
||||
uint8_t byte;
|
||||
ssize_t n = read(fd, &byte, 1);
|
||||
if (n > 0)
|
||||
{
|
||||
buffer.push_back(byte);
|
||||
|
||||
if (buffer.size() >= PACKET_LENGTH)
|
||||
{
|
||||
if (parsePacket(buffer))
|
||||
{
|
||||
data_handler(buffer);
|
||||
buffer.erase(buffer.begin(), buffer.begin() + PACKET_LENGTH);
|
||||
std::cout << "accel:" << accel[0] << " " << accel[1] << " " << accel[2] << std::endl;
|
||||
std::cout << "gyro:" << gyro[0] << " " << gyro[1] << " " << gyro[2] << std::endl;
|
||||
std::cout << "angle:" << angle[0] << " " << angle[1] << " " << angle[2] << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cerr << "Invalid packet, discarding the first byte." << std::endl;
|
||||
buffer.erase(buffer.begin());
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (n < 0)
|
||||
{
|
||||
std::cerr << "Error reading from UART." << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
close(fd);
|
||||
return 0;
|
||||
}
|
||||
@@ -1,5 +0,0 @@
|
||||
# demo1
|
||||
add_executable(key_demo key_demo.cpp)
|
||||
|
||||
# 链接库
|
||||
target_link_libraries(key_demo common_lib)
|
||||
@@ -1,68 +0,0 @@
|
||||
/*
|
||||
* @Author: Ilikara 3435193369@qq.com
|
||||
* @Date: 2025-01-26 17:00:24
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-03-10 14:23:16
|
||||
* @FilePath: /2k300_smartcar/jy62_demo/jy62_demo.cpp
|
||||
* @Description:
|
||||
*
|
||||
* Copyright (c) 2025 by ${git_name_email}, All Rights Reserved.
|
||||
*/
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "GPIO.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
GPIO keys[9] = {
|
||||
GPIO(0),
|
||||
GPIO(3),
|
||||
GPIO(1),
|
||||
GPIO(2),
|
||||
GPIO(27),
|
||||
GPIO(17),
|
||||
GPIO(16),
|
||||
GPIO(15),
|
||||
GPIO(14)};
|
||||
|
||||
string keysName[9] = {
|
||||
"Down",
|
||||
"Left",
|
||||
"Up",
|
||||
"OK",
|
||||
"Right",
|
||||
"SW1",
|
||||
"SW2",
|
||||
"SW3",
|
||||
"SW4"};
|
||||
|
||||
|
||||
void draw_interface()
|
||||
{
|
||||
cout << "\033[H\033[J";
|
||||
|
||||
for (size_t i = 0; i < 9; ++i)
|
||||
{
|
||||
string status = keys[i].readValue() ? "HIGH" : "LOW";
|
||||
printf("│ GPIO %-7s │ %-9s │\n", keysName[i].c_str(), status.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
for (size_t i = 0; i < 9; ++i)
|
||||
{
|
||||
keys[i].setDirection("in");
|
||||
}
|
||||
|
||||
while (true)
|
||||
{
|
||||
draw_interface();
|
||||
usleep(100000); // 100ms刷新间隔
|
||||
cout << "\033[13A"; // 将光标移动到界面起始位置
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -20,6 +20,17 @@ const std::string steer_gain_file = "./steer_gain";
|
||||
const std::string center_bias_file = "./center_bias";
|
||||
const std::string debug_file = "./debug";
|
||||
|
||||
const std::string cone_avoid_gain_file = "./cone_avoid_gain";
|
||||
const std::string cone_avoid_range_file = "./cone_avoid_range";
|
||||
const std::string cone_speed_file = "./cone_speed";
|
||||
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 brake_scale_file = "./brake_scale";
|
||||
const std::string brake_max_file = "./brake_max";
|
||||
|
||||
double readDoubleFromFile(const std::string &filename);
|
||||
bool readFlag(const std::string &filename);
|
||||
void cfg_load_all();
|
||||
@@ -35,6 +46,17 @@ struct CfgCache {
|
||||
int start = 0; // 1=启动 0=停止
|
||||
int showImg = 0; // LCD 预览开关
|
||||
int debug = 0; // debug 模式
|
||||
|
||||
double cone_avoid_gain = 0.3; // 锥桶中线变形推离量 (归一化)
|
||||
int cone_avoid_range = 30; // 变形斜坡陡峭度
|
||||
double cone_speed = 0.5; // 锥桶触发时速度倍率
|
||||
int cone_min_frames = 3; // 连续确认帧数
|
||||
int cone_margin = 0; // 0=中心点 1=框边缘
|
||||
double cone_thresh = 0.80; // 锥桶置信度阈值
|
||||
int cone_hold_frames = 30; // 锥桶消失后保持变形的帧数
|
||||
|
||||
double brake_scale = 10; // 速度(pps) → 刹车占空比(ns) 缩放系数
|
||||
int brake_max = 10000; // 最大刹车占空比 (ns)
|
||||
};
|
||||
extern CfgCache g_cfg;
|
||||
|
||||
|
||||
@@ -26,6 +26,13 @@ int main(void)
|
||||
|
||||
cfg_load_all();
|
||||
|
||||
double avoid_gain_val = readDoubleFromFile(cone_avoid_gain_file);
|
||||
int avoid_range_val = (int)readDoubleFromFile(cone_avoid_range_file);
|
||||
int hold_frames_val = (int)readDoubleFromFile(cone_hold_frames_file);
|
||||
if (avoid_gain_val > 0) g_cfg.cone_avoid_gain = avoid_gain_val;
|
||||
if (avoid_range_val > 0) g_cfg.cone_avoid_range = avoid_range_val;
|
||||
if (hold_frames_val > 0) g_cfg.cone_hold_frames = hold_frames_val;
|
||||
|
||||
if (CameraInit(0, dest_fps, 320, 240) < 0) {
|
||||
std::cerr << "CameraInit failed" << std::endl;
|
||||
return -1;
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1,5 +0,0 @@
|
||||
# demo3
|
||||
add_executable(opencv_demo1 opencv_demo1.cpp)
|
||||
|
||||
# 链接 OpenCV 库
|
||||
target_link_libraries(opencv_demo1 ${OpenCV_LIBS})
|
||||
@@ -1,158 +0,0 @@
|
||||
/*
|
||||
* @Author: ilikara 3435193369@qq.com
|
||||
* @Date: 2025-01-07 06:55:37
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-01-07 08:10:54
|
||||
* @FilePath: /smartcar/opencv_demo1/opencv_demo1.cpp
|
||||
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||
*/
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <linux/fb.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <chrono>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
|
||||
// 将 RGB888 转换为 RGB565
|
||||
ushort rgb888_to_rgb565(const Vec3b &color)
|
||||
{
|
||||
return ((color[2] >> 3) << 11) | ((color[1] >> 2) << 5) | (color[0] >> 3);
|
||||
}
|
||||
|
||||
// 获取当前时间字符串
|
||||
string getCurrentTime()
|
||||
{
|
||||
auto now = chrono::system_clock::now();
|
||||
auto now_time_t = chrono::system_clock::to_time_t(now);
|
||||
auto now_tm = *localtime(&now_time_t);
|
||||
|
||||
stringstream ss;
|
||||
ss << put_time(&now_tm, "%Y-%m-%d %H:%M:%S");
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// 打开 USB 摄像头
|
||||
VideoCapture cap(0); // 0 表示默认摄像头
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
cerr << "Error: Cannot open camera" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 设置摄像头分辨率（可选）
|
||||
cap.set(CAP_PROP_FRAME_WIDTH, 320);
|
||||
cap.set(CAP_PROP_FRAME_HEIGHT, 240);
|
||||
|
||||
// Framebuffer 设备
|
||||
const char *fb_device = "/dev/fb0";
|
||||
int fb_fd = open(fb_device, O_RDWR);
|
||||
if (fb_fd == -1)
|
||||
{
|
||||
cerr << "Error: Cannot open framebuffer device" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 获取 Framebuffer 信息
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo))
|
||||
{
|
||||
cerr << "Error: Cannot get framebuffer information" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 检查 Framebuffer 是否支持 RGB565
|
||||
if (vinfo.bits_per_pixel != 16)
|
||||
{
|
||||
cerr << "Error: Framebuffer is not RGB565 format" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 映射 Framebuffer 到内存
|
||||
size_t fb_size = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;
|
||||
ushort *fb_data = (ushort *)mmap(0, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb_fd, 0);
|
||||
if (fb_data == MAP_FAILED)
|
||||
{
|
||||
cerr << "Error: Failed to map framebuffer to memory" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 主循环
|
||||
Mat frame, resizedFrame;
|
||||
int frameCount = 0;
|
||||
while (true)
|
||||
{
|
||||
// 从摄像头捕获一帧图像
|
||||
cap >> frame;
|
||||
if (frame.empty())
|
||||
{
|
||||
cerr << "Error: Captured frame is empty" << endl;
|
||||
break;
|
||||
}
|
||||
|
||||
// 调整图像大小为 160x128
|
||||
resize(frame, resizedFrame, Size(160, 128));
|
||||
|
||||
// 获取当前时间和帧数
|
||||
string timeStr = getCurrentTime();
|
||||
string frameStr = "Frame: " + to_string(frameCount);
|
||||
|
||||
// 在图像左上角绘制黑色边框的白色文字
|
||||
int fontFace = FONT_HERSHEY_SIMPLEX;
|
||||
double fontScale = 0.4;
|
||||
int thickness = 1;
|
||||
int baseline = 0;
|
||||
|
||||
// 计算文字位置
|
||||
Point timePos(5, 15); // 时间文字位置
|
||||
Point framePos(5, 30); // 帧数文字位置
|
||||
|
||||
// 绘制黑色边框
|
||||
putText(resizedFrame, timeStr, timePos + Point(-1, -1), fontFace, fontScale, Scalar(0, 0, 0), thickness + 1);
|
||||
putText(resizedFrame, timeStr, timePos + Point(1, -1), fontFace, fontScale, Scalar(0, 0, 0), thickness + 1);
|
||||
putText(resizedFrame, timeStr, timePos + Point(-1, 1), fontFace, fontScale, Scalar(0, 0, 0), thickness + 1);
|
||||
putText(resizedFrame, timeStr, timePos + Point(1, 1), fontFace, fontScale, Scalar(0, 0, 0), thickness + 1);
|
||||
|
||||
putText(resizedFrame, frameStr, framePos + Point(-1, -1), fontFace, fontScale, Scalar(0, 0, 0), thickness + 1);
|
||||
putText(resizedFrame, frameStr, framePos + Point(1, -1), fontFace, fontScale, Scalar(0, 0, 0), thickness + 1);
|
||||
putText(resizedFrame, frameStr, framePos + Point(-1, 1), fontFace, fontScale, Scalar(0, 0, 0), thickness + 1);
|
||||
putText(resizedFrame, frameStr, framePos + Point(1, 1), fontFace, fontScale, Scalar(0, 0, 0), thickness + 1);
|
||||
|
||||
// 绘制白色文字
|
||||
putText(resizedFrame, timeStr, timePos, fontFace, fontScale, Scalar(255, 255, 255), thickness);
|
||||
putText(resizedFrame, frameStr, framePos, fontFace, fontScale, Scalar(255, 255, 255), thickness);
|
||||
|
||||
// 将图像数据写入 Framebuffer
|
||||
for (int y = 0; y < resizedFrame.rows; y++)
|
||||
{
|
||||
for (int x = 0; x < resizedFrame.cols; x++)
|
||||
{
|
||||
Vec3b color = resizedFrame.at<Vec3b>(y, x);
|
||||
fb_data[y * vinfo.xres + x] = rgb888_to_rgb565(color);
|
||||
}
|
||||
}
|
||||
|
||||
// 更新帧数
|
||||
frameCount++;
|
||||
|
||||
// 等待一段时间（例如 30ms）
|
||||
usleep(30000);
|
||||
}
|
||||
|
||||
// 释放资源
|
||||
munmap(fb_data, fb_size);
|
||||
close(fb_fd);
|
||||
cap.release();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,5 +0,0 @@
|
||||
# demo2
|
||||
add_executable(opencv_demo2 opencv_demo2.cpp)
|
||||
|
||||
# 链接 OpenCV 库
|
||||
target_link_libraries(opencv_demo2 ${OpenCV_LIBS})
|
||||
@@ -1,222 +0,0 @@
|
||||
/*
|
||||
* @Author: ilikara 3435193369@qq.com
|
||||
* @Date: 2025-01-07 06:26:01
|
||||
* @LastEditors: Ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-01-17 15:41:43
|
||||
* @FilePath: /2k300_smartcar/opencv_demo2/opencv_demo2.cpp
|
||||
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||
*/
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <linux/fb.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <algorithm> // for sort
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
|
||||
// 定义立方体的顶点
|
||||
vector<Point3f> vertices = {
|
||||
{-1, -1, -1},
|
||||
{1, -1, -1},
|
||||
{1, 1, -1},
|
||||
{-1, 1, -1},
|
||||
{-1, -1, 1},
|
||||
{1, -1, 1},
|
||||
{1, 1, 1},
|
||||
{-1, 1, 1}};
|
||||
|
||||
// 定义立方体的面
|
||||
vector<vector<int>> faces = {
|
||||
{0, 1, 2, 3}, // 前面
|
||||
{4, 5, 6, 7}, // 后面
|
||||
{0, 1, 5, 4}, // 底面
|
||||
{2, 3, 7, 6}, // 顶面
|
||||
{0, 3, 7, 4}, // 左面
|
||||
{1, 2, 6, 5} // 右面
|
||||
};
|
||||
|
||||
// 定义每个面的颜色
|
||||
vector<Scalar> faceColors = {
|
||||
Scalar(255, 0, 0), // 红色
|
||||
Scalar(0, 255, 0), // 绿色
|
||||
Scalar(0, 0, 255), // 蓝色
|
||||
Scalar(255, 255, 0), // 黄色
|
||||
Scalar(255, 0, 255), // 紫色
|
||||
Scalar(0, 255, 255) // 青色
|
||||
};
|
||||
|
||||
// 将3D点投影到2D平面（透视投影）
|
||||
Point2f projectPoint(Point3f point, Mat rotationMatrix, float scale, Point2f offset, float fov)
|
||||
{
|
||||
Mat pointMat = (Mat_<float>(3, 1) << point.x, point.y, point.z);
|
||||
Mat rotatedPoint = rotationMatrix * pointMat;
|
||||
|
||||
// 透视投影
|
||||
float z = rotatedPoint.at<float>(2, 0);
|
||||
float x = rotatedPoint.at<float>(0, 0) / (z / fov + 1) * scale + offset.x;
|
||||
float y = rotatedPoint.at<float>(1, 0) / (z / fov + 1) * scale + offset.y;
|
||||
|
||||
return Point2f(x, y);
|
||||
}
|
||||
|
||||
// 将 RGB888 转换为 RGB565
|
||||
ushort rgb888_to_rgb565(const Vec3b &color)
|
||||
{
|
||||
return ((color[2] >> 3) << 11) | ((color[1] >> 2) << 5) | (color[0] >> 3);
|
||||
}
|
||||
|
||||
// 计算面的中心深度
|
||||
float calculateFaceDepth(const vector<Point3f> &faceVertices, Mat rotationMatrix)
|
||||
{
|
||||
float depth = 0;
|
||||
for (const auto &vertex : faceVertices)
|
||||
{
|
||||
Mat pointMat = (Mat_<float>(3, 1) << vertex.x, vertex.y, vertex.z);
|
||||
Mat rotatedPoint = rotationMatrix * pointMat;
|
||||
depth += rotatedPoint.at<float>(2, 0); // Z 轴深度
|
||||
}
|
||||
return depth / faceVertices.size(); // 返回平均深度
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// Framebuffer 设备
|
||||
const char *fb_device = "/dev/fb0";
|
||||
int fb_fd = open(fb_device, O_RDWR);
|
||||
if (fb_fd == -1)
|
||||
{
|
||||
cerr << "Error: Cannot open framebuffer device" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 获取 Framebuffer 信息
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo))
|
||||
{
|
||||
cerr << "Error: Cannot get framebuffer information" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 检查 Framebuffer 是否支持 RGB565
|
||||
if (vinfo.bits_per_pixel != 16)
|
||||
{
|
||||
cerr << "Error: Framebuffer is not RGB565 format" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 映射 Framebuffer 到内存
|
||||
size_t fb_size = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;
|
||||
ushort *fb_data = (ushort *)mmap(0, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb_fd, 0);
|
||||
if (fb_data == MAP_FAILED)
|
||||
{
|
||||
cerr << "Error: Failed to map framebuffer to memory" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 创建 OpenCV 图像（160x128，RGB888）
|
||||
Mat image(128, 160, CV_8UC3, Scalar(0, 0, 0));
|
||||
|
||||
float angle = 0;
|
||||
float scale = 50; // 缩放因子
|
||||
Point2f offset(80, 64); // 图像中心点
|
||||
float fov = 500; // 透视投影的视野
|
||||
|
||||
while (true)
|
||||
{
|
||||
// 清空图像
|
||||
image.setTo(Scalar(0, 0, 0));
|
||||
|
||||
// 计算旋转矩阵
|
||||
Mat rotationMatrixX = (Mat_<float>(3, 3) << 1, 0, 0,
|
||||
0, cos(angle), -sin(angle),
|
||||
0, sin(angle), cos(angle));
|
||||
|
||||
Mat rotationMatrixY = (Mat_<float>(3, 3) << cos(angle), 0, sin(angle),
|
||||
0, 1, 0,
|
||||
-sin(angle), 0, cos(angle));
|
||||
|
||||
Mat rotationMatrixZ = (Mat_<float>(3, 3) << cos(angle), -sin(angle), 0,
|
||||
sin(angle), cos(angle), 0,
|
||||
0, 0, 1);
|
||||
|
||||
Mat rotationMatrix = rotationMatrixZ * rotationMatrixY * rotationMatrixX;
|
||||
|
||||
// 计算每个面的深度并排序
|
||||
vector<pair<float, int>> faceDepths; // {深度, 面索引}
|
||||
for (size_t i = 0; i < faces.size(); i++)
|
||||
{
|
||||
vector<Point3f> faceVertices;
|
||||
for (int vertexIndex : faces[i])
|
||||
{
|
||||
faceVertices.push_back(vertices[vertexIndex]);
|
||||
}
|
||||
float depth = calculateFaceDepth(faceVertices, rotationMatrix);
|
||||
faceDepths.push_back({depth, i});
|
||||
}
|
||||
|
||||
// 按深度从远到近排序
|
||||
sort(faceDepths.begin(), faceDepths.end(), [](const pair<float, int> &a, const pair<float, int> &b)
|
||||
{
|
||||
return a.first > b.first; // 深度大的先绘制
|
||||
});
|
||||
|
||||
// 按排序后的顺序绘制面
|
||||
for (const auto &faceDepth : faceDepths)
|
||||
{
|
||||
int faceIndex = faceDepth.second;
|
||||
vector<Point2f> facePointsFloat;
|
||||
for (int vertexIndex : faces[faceIndex])
|
||||
{
|
||||
Point3f vertex = vertices[vertexIndex];
|
||||
Point2f projectedPoint = projectPoint(vertex, rotationMatrix, scale, offset, fov);
|
||||
facePointsFloat.push_back(projectedPoint);
|
||||
}
|
||||
|
||||
// 将 Point2f 转换为 Point（CV_32S 类型）
|
||||
vector<Point> facePoints;
|
||||
for (const auto &pt : facePointsFloat)
|
||||
{
|
||||
facePoints.push_back(Point(cvRound(pt.x), cvRound(pt.y)));
|
||||
}
|
||||
|
||||
// 填充面
|
||||
fillConvexPoly(image, facePoints, faceColors[faceIndex]);
|
||||
|
||||
// 绘制边
|
||||
for (size_t j = 0; j < facePoints.size(); j++)
|
||||
{
|
||||
line(image, facePoints[j], facePoints[(j + 1) % facePoints.size()], Scalar(0, 0, 0), 2);
|
||||
}
|
||||
}
|
||||
|
||||
// 将 OpenCV 图像（RGB888）转换为 RGB565 并写入 Framebuffer
|
||||
for (int y = 0; y < image.rows; y++)
|
||||
{
|
||||
for (int x = 0; x < image.cols; x++)
|
||||
{
|
||||
Vec3b color = image.at<Vec3b>(y, x);
|
||||
fb_data[y * vinfo.xres + x] = rgb888_to_rgb565(color);
|
||||
}
|
||||
}
|
||||
|
||||
// 更新角度
|
||||
angle += 0.02;
|
||||
|
||||
// 等待一段时间
|
||||
usleep(1000000 / 50);
|
||||
}
|
||||
|
||||
// 释放资源
|
||||
munmap(fb_data, fb_size);
|
||||
close(fb_fd);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,5 +0,0 @@
|
||||
# demo2
|
||||
add_executable(opencv_demo3 opencv_demo3.cpp)
|
||||
|
||||
# 链接 OpenCV 库
|
||||
target_link_libraries(opencv_demo3 ${OpenCV_LIBS})
|
||||
@@ -1,141 +0,0 @@
|
||||
/*
|
||||
* @Author: ilikara 3435193369@qq.com
|
||||
* @Date: 2025-01-07 06:26:01
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-01-07 08:10:45
|
||||
* @FilePath: /smartcar/opencv_demo2/opencv_demo2.cpp
|
||||
* @Description: 使用Haar级联分类器从USB摄像头获取图像，并将图像居中显示到160x128的RGB565 framebuffer设备
|
||||
*/
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <linux/fb.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
|
||||
// 将 RGB888 转换为 RGB565
|
||||
ushort rgb888_to_rgb565(const Vec3b &color)
|
||||
{
|
||||
return ((color[2] >> 3) << 11) | ((color[1] >> 2) << 5) | (color[0] >> 3);
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// Framebuffer 设备
|
||||
const char *fb_device = "/dev/fb0";
|
||||
int fb_fd = open(fb_device, O_RDWR);
|
||||
if (fb_fd == -1)
|
||||
{
|
||||
cerr << "Error: Cannot open framebuffer device" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 获取 Framebuffer 信息
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo))
|
||||
{
|
||||
cerr << "Error: Cannot get framebuffer information" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 检查 Framebuffer 是否支持 RGB565
|
||||
if (vinfo.bits_per_pixel != 16)
|
||||
{
|
||||
cerr << "Error: Framebuffer is not RGB565 format" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 映射 Framebuffer 到内存
|
||||
size_t fb_size = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;
|
||||
ushort *fb_data = (ushort *)mmap(0, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb_fd, 0);
|
||||
if (fb_data == MAP_FAILED)
|
||||
{
|
||||
cerr << "Error: Failed to map framebuffer to memory" << endl;
|
||||
close(fb_fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 打开USB摄像头
|
||||
VideoCapture cap(0); // 0表示第一个摄像头
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
cerr << "Error: Cannot open camera" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 设置摄像头参数
|
||||
cap.set(CAP_PROP_FRAME_WIDTH, 640);
|
||||
cap.set(CAP_PROP_FRAME_HEIGHT, 360);
|
||||
cap.set(CAP_PROP_FOURCC, VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
cap.set(CAP_PROP_AUTO_EXPOSURE, -1); // 启用自动曝光
|
||||
|
||||
// 加载Haar级联分类器
|
||||
CascadeClassifier face_cascade;
|
||||
if (!face_cascade.load("opencv/share/opencv4/haarcascades/haarcascade_frontalface_default.xml"))
|
||||
{
|
||||
cerr << "Error: Cannot load Haar cascade classifier" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 创建 OpenCV 图像（160x128，RGB888）
|
||||
Mat image(128, 160, CV_8UC3, Scalar(0, 0, 0));
|
||||
|
||||
while (true)
|
||||
{
|
||||
Mat frame;
|
||||
cap >> frame; // 从摄像头获取一帧图像
|
||||
if (frame.empty())
|
||||
{
|
||||
cerr << "Error: Cannot grab frame from camera" << endl;
|
||||
break;
|
||||
}
|
||||
|
||||
// 缩放图像到160x90
|
||||
Mat resized_frame;
|
||||
resize(frame, resized_frame, Size(160, 90));
|
||||
|
||||
// 转换为灰度图像
|
||||
Mat gray;
|
||||
cvtColor(resized_frame, gray, COLOR_BGR2GRAY);
|
||||
|
||||
// 使用Haar级联分类器检测人脸
|
||||
vector<Rect> faces;
|
||||
face_cascade.detectMultiScale(gray, faces, 1.1, 2, 0 | CASCADE_SCALE_IMAGE, Size(30, 30));
|
||||
|
||||
// 在图像上绘制检测到的人脸
|
||||
for (const auto &face : faces)
|
||||
{
|
||||
rectangle(resized_frame, face, Scalar(255, 0, 0), 2);
|
||||
}
|
||||
|
||||
// 将图像居中显示到160x128的framebuffer
|
||||
image.setTo(Scalar(0, 0, 0)); // 清空图像
|
||||
int y_offset = (128 - 90) / 2;
|
||||
resized_frame.copyTo(image(Rect(0, y_offset, 160, 90)));
|
||||
|
||||
// 将 OpenCV 图像（RGB888）转换为 RGB565 并写入 Framebuffer
|
||||
for (int y = 0; y < image.rows; y++)
|
||||
{
|
||||
for (int x = 0; x < image.cols; x++)
|
||||
{
|
||||
Vec3b color = image.at<Vec3b>(y, x);
|
||||
fb_data[y * vinfo.xres + x] = rgb888_to_rgb565(color);
|
||||
}
|
||||
}
|
||||
|
||||
// 等待一段时间
|
||||
usleep(33333);
|
||||
}
|
||||
|
||||
// 释放资源
|
||||
munmap(fb_data, fb_size);
|
||||
close(fb_fd);
|
||||
cap.release();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,2 +0,0 @@
|
||||
add_executable(screenshot_demo screenshot_demo.cpp)
|
||||
target_link_libraries(screenshot_demo common_lib ${OpenCV_LIBS})
|
||||
@@ -1,111 +0,0 @@
|
||||
/*
|
||||
* screenshot_demo — 摄像头截图保存
|
||||
* ======================================
|
||||
* 每次运行截一张图, 保存到 ./screenshot/image_N.jpg
|
||||
* 用法:
|
||||
* ./screenshot_demo 截一张图 + LCD 显示
|
||||
* ./screenshot_demo --no-display 无 LCD
|
||||
*/
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <iostream>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/fb.h>
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
|
||||
static uint16_t rgb565(uint8_t r, uint8_t g, uint8_t b)
|
||||
{
|
||||
return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
bool no_display = false;
|
||||
for (int i = 1; i < argc; ++i)
|
||||
if (strcmp(argv[i], "--no-display") == 0)
|
||||
no_display = true;
|
||||
|
||||
// ---- 创建目录 ----
|
||||
mkdir("./screenshot", 0755);
|
||||
|
||||
// ---- 摄像头 ----
|
||||
VideoCapture cap;
|
||||
cap.open(0, CAP_V4L2);
|
||||
if (!cap.isOpened()) cap.open(0);
|
||||
if (!cap.isOpened()) { cerr << "无法打开摄像头" << endl; return 1; }
|
||||
cap.set(CAP_PROP_FRAME_WIDTH, 640);
|
||||
cap.set(CAP_PROP_FRAME_HEIGHT, 480);
|
||||
cap.set(CAP_PROP_FOURCC, VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
printf("摄像头: %dx%d\n",
|
||||
(int)cap.get(CAP_PROP_FRAME_WIDTH),
|
||||
(int)cap.get(CAP_PROP_FRAME_HEIGHT));
|
||||
|
||||
// ---- 丢弃前几帧 (曝光稳定) ----
|
||||
Mat frame;
|
||||
for (int i = 0; i < 10; ++i)
|
||||
cap.read(frame);
|
||||
usleep(100000);
|
||||
|
||||
// ---- 截图 ----
|
||||
if (!cap.read(frame) || frame.empty())
|
||||
{
|
||||
cerr << "截图失败" << endl;
|
||||
cap.release();
|
||||
return 1;
|
||||
}
|
||||
|
||||
// 自增文件名
|
||||
int idx = 0;
|
||||
char fname[64];
|
||||
struct stat st;
|
||||
do {
|
||||
snprintf(fname, sizeof(fname),
|
||||
"./screenshot/image_%05d.jpg", ++idx);
|
||||
} while (stat(fname, &st) == 0);
|
||||
|
||||
imwrite(fname, frame);
|
||||
printf("截图: %s (%dx%d)\n", fname, frame.cols, frame.rows);
|
||||
|
||||
// ---- LCD 显示 ----
|
||||
if (!no_display)
|
||||
{
|
||||
int fb_fd = open("/dev/fb0", O_RDWR);
|
||||
if (fb_fd >= 0)
|
||||
{
|
||||
struct fb_var_screeninfo vinfo;
|
||||
ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo);
|
||||
int scr_w = vinfo.xres, scr_h = vinfo.yres;
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * 2;
|
||||
uint16_t *fb_buf = (uint16_t *)mmap(nullptr, fb_size,
|
||||
PROT_READ|PROT_WRITE,
|
||||
MAP_SHARED, fb_fd, 0);
|
||||
|
||||
Mat disp;
|
||||
resize(frame, disp, Size(scr_w, scr_h));
|
||||
rectangle(disp, Point(0, 0), Point(disp.cols, disp.rows),
|
||||
Scalar(0, 255, 255), 3);
|
||||
|
||||
for (int y = 0; y < scr_h; ++y)
|
||||
for (int x = 0; x < scr_w; ++x)
|
||||
{
|
||||
Vec3b px = disp.at<Vec3b>(y, x);
|
||||
fb_buf[y * scr_w + x] = rgb565(px[2], px[1], px[0]);
|
||||
}
|
||||
|
||||
sleep(1);
|
||||
munmap(fb_buf, fb_size);
|
||||
close(fb_fd);
|
||||
}
|
||||
}
|
||||
|
||||
cap.release();
|
||||
return 0;
|
||||
}
|
||||
+171
-7
@@ -22,7 +22,8 @@ PwmController servo(1, 0);
|
||||
|
||||
// ── 模型检测参数 ──
|
||||
#define ZEBRA_CLASS 3
|
||||
static float g_thresh[4] = {0.80f, 0.80f, 0.80f, 0.75f};
|
||||
#define CONE_CLASS 0
|
||||
static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.90f};
|
||||
|
||||
// ── 斑马线去抖 ──
|
||||
#define ZEBRA_MIN_FRAMES 5
|
||||
@@ -36,6 +37,23 @@ static ZState g_zstate = Z_NORMAL;
|
||||
static time_t g_ztime = 0;
|
||||
static bool g_zebra_ever = false;
|
||||
|
||||
// ── 红绿灯状态机 ──
|
||||
#define TL_RED_CLASS 1
|
||||
#define TL_GREEN_CLASS 2
|
||||
enum TLState { TL_NORMAL, TL_STOP, TL_WAIT_GREEN };
|
||||
static TLState g_tl_state = TL_NORMAL;
|
||||
static int g_tl_red_frames = 0;
|
||||
static int g_tl_green_frames = 0;
|
||||
|
||||
// ── 锥桶检测 ──
|
||||
static int g_cone_frames = 0;
|
||||
static int g_cone_last_row = -1;
|
||||
static int g_cone_last_col = -1;
|
||||
static bool g_cone_confirmed = false;
|
||||
static int g_cone_hold_ctr = 0;
|
||||
static int g_cone_hold_src_row = -1;
|
||||
static int g_cone_hold_src_col = -1;
|
||||
|
||||
// ── 模型检测结果缓存 ──
|
||||
static DetectBoxV10 g_boxes[16];
|
||||
static int g_box_count = 0;
|
||||
@@ -110,10 +128,10 @@ int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
|
||||
line_tracking_width = newWidth / calc_scale;
|
||||
line_tracking_height = newHeight / calc_scale;
|
||||
|
||||
if (!model_v10_init("./nanodetv10.bin")) {
|
||||
if (!model_v10_init("./mild_v12.bin")) {
|
||||
printf("[MODEL] 警告: 模型加载失败\n");
|
||||
} else {
|
||||
printf("[MODEL] v10 模型已加载\n");
|
||||
printf("[MODEL] Mild v12 模型已加载\n");
|
||||
}
|
||||
|
||||
i2c_audio_open();
|
||||
@@ -283,6 +301,138 @@ int CameraHandler(void)
|
||||
break;
|
||||
}
|
||||
|
||||
// ── 5.5 红绿灯状态机 ──
|
||||
bool red_seen = false;
|
||||
bool green_seen = false;
|
||||
for (int i = 0; i < g_box_count; ++i) {
|
||||
if (g_boxes[i].cls == TL_RED_CLASS) red_seen = true;
|
||||
if (g_boxes[i].cls == TL_GREEN_CLASS) green_seen = true;
|
||||
}
|
||||
|
||||
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++;
|
||||
else g_tl_green_frames = std::max(0, g_tl_green_frames - 1);
|
||||
|
||||
switch (g_tl_state) {
|
||||
case TL_NORMAL:
|
||||
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) {
|
||||
g_tl_state = TL_WAIT_GREEN;
|
||||
if (g_cfg.debug) printf("[TL] 等待绿灯\n");
|
||||
}
|
||||
break;
|
||||
case TL_WAIT_GREEN:
|
||||
if (g_tl_green_frames >= 3) {
|
||||
g_tl_state = TL_NORMAL;
|
||||
g_tl_red_frames = 0;
|
||||
g_tl_green_frames = 0;
|
||||
if (g_cfg.debug) printf("[TL] 绿灯通行\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
bool tl_block = (g_tl_state != TL_NORMAL);
|
||||
|
||||
// ── 5.6 锥桶检测 + 中线变形 ──
|
||||
bool cone_seen = false;
|
||||
int cone_row_keep = -1;
|
||||
int cone_col_keep = -1;
|
||||
|
||||
if (g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
|
||||
for (int i = 0; i < g_box_count; ++i) {
|
||||
if (g_boxes[i].cls != CONE_CLASS) continue;
|
||||
if (g_boxes[i].conf < g_cfg.cone_thresh) continue;
|
||||
|
||||
int col_lt = g_boxes[i].cx * line_tracking_width / raw_frame.cols;
|
||||
int row_lt = g_boxes[i].cy * line_tracking_height / raw_frame.rows;
|
||||
|
||||
if (row_lt < 10 || row_lt >= line_tracking_height) continue;
|
||||
if (left_line[row_lt] == -1 || right_line[row_lt] == -1) continue;
|
||||
|
||||
if (g_cfg.cone_margin) {
|
||||
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[row_lt] || bl > right_line[row_lt]) continue;
|
||||
} else {
|
||||
if (col_lt < left_line[row_lt] || col_lt > right_line[row_lt]) continue;
|
||||
}
|
||||
|
||||
if (row_lt > cone_row_keep) { cone_row_keep = row_lt; cone_col_keep = col_lt; }
|
||||
cone_seen = true;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
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_keep;
|
||||
g_cone_last_col = cone_col_keep;
|
||||
g_cone_frames = 1;
|
||||
} else {
|
||||
if (std::abs(cone_row_keep - g_cone_last_row) <= row_tol &&
|
||||
std::abs(cone_col_keep - g_cone_last_col) <= col_tol) {
|
||||
g_cone_frames++;
|
||||
g_cone_last_row = cone_row_keep;
|
||||
g_cone_last_col = cone_col_keep;
|
||||
} else {
|
||||
g_cone_frames = 1;
|
||||
g_cone_last_row = cone_row_keep;
|
||||
g_cone_last_col = cone_col_keep;
|
||||
}
|
||||
}
|
||||
} 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);
|
||||
|
||||
if (g_cone_confirmed) {
|
||||
g_cone_hold_ctr = 0;
|
||||
g_cone_hold_src_row = cone_row_keep;
|
||||
g_cone_hold_src_col = cone_col_keep;
|
||||
} else if (g_cone_hold_src_row > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) {
|
||||
g_cone_hold_ctr++;
|
||||
}
|
||||
|
||||
bool cone_active = g_cone_confirmed ||
|
||||
(g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);
|
||||
|
||||
if (cone_active && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
|
||||
int src_row = g_cone_confirmed ? cone_row_keep : g_cone_hold_src_row;
|
||||
int src_col = g_cone_confirmed ? cone_col_keep : g_cone_hold_src_col;
|
||||
int start_row = 10;
|
||||
if (src_row > start_row) {
|
||||
double total_rows = src_row - start_row;
|
||||
double rng = (double)g_cfg.cone_avoid_range;
|
||||
double half_w = line_tracking_width / 2.0;
|
||||
double mid_at_cone = mid_line[src_row];
|
||||
double dir = (src_col < mid_at_cone) ? 1.0 : -1.0;
|
||||
double decay = g_cone_confirmed
|
||||
? 1.0
|
||||
: (1.0 - (double)g_cone_hold_ctr / g_cfg.cone_hold_frames);
|
||||
|
||||
for (int row = start_row; row <= src_row; row++) {
|
||||
double t_raw = (row - start_row) / total_rows;
|
||||
double t = std::clamp(t_raw * (total_rows / rng), 0.0, 1.0);
|
||||
double push = t * g_cfg.cone_avoid_gain * half_w * dir * decay;
|
||||
double new_mid = mid_line[row] + push;
|
||||
new_mid = std::clamp(new_mid,
|
||||
(double)left_line[row] + 2.0,
|
||||
(double)right_line[row] - 2.0);
|
||||
mid_line[row] = (int)(new_mid + 0.5);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ── 6. 舵机 ──
|
||||
if (g_cfg.start) {
|
||||
int foresee = (int)g_cfg.foresee;
|
||||
@@ -304,7 +454,15 @@ int CameraHandler(void)
|
||||
}
|
||||
|
||||
// ── 7. 电机控制 ──
|
||||
ControlUpdate(target_speed, g_zstate == Z_STOP);
|
||||
{
|
||||
double spd = target_speed;
|
||||
bool cone_slow = g_cone_confirmed ||
|
||||
(g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);
|
||||
if (cone_slow && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
|
||||
spd *= g_cfg.cone_speed;
|
||||
}
|
||||
ControlUpdate(spd, g_zstate == Z_STOP || tl_block);
|
||||
}
|
||||
clock_gettime(CLOCK_MONOTONIC,&t4);
|
||||
|
||||
// ── 8. LCD ──
|
||||
@@ -340,14 +498,20 @@ int CameraHandler(void)
|
||||
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 == ZEBRA_CLASS) color=cv::Scalar(255,0,255);
|
||||
else if (g_boxes[i].cls == CONE_CLASS) color=cv::Scalar(0,165,255);
|
||||
cv::rectangle(lcd_fbImage(roi), cv::Point(x1,y1), cv::Point(x2,y2), color, 2);
|
||||
char lab[16]; std::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);
|
||||
}
|
||||
|
||||
const char* ztxt="N";
|
||||
if (g_zstate==Z_STOP) ztxt="S";
|
||||
else if (g_zstate==Z_COOLDOWN) ztxt="C";
|
||||
char ztxt[8];
|
||||
int zidx = 0;
|
||||
if (g_tl_state == TL_STOP) ztxt[zidx++] = 'R';
|
||||
else if (g_tl_state == TL_WAIT_GREEN) ztxt[zidx++] = 'G';
|
||||
if (g_zstate == Z_STOP) ztxt[zidx++] = 'S';
|
||||
else if (g_zstate == Z_COOLDOWN) ztxt[zidx++] = 'C';
|
||||
else ztxt[zidx++] = 'N';
|
||||
ztxt[zidx] = '\0';
|
||||
cv::putText(lcd_fbImage(roi), ztxt, cv::Point(2,newHeight-4), cv::FONT_HERSHEY_SIMPLEX,0.4,cv::Scalar(0,255,255),1);
|
||||
|
||||
convertMatToRGB565(lcd_fbImage, fb_buffer, screenWidth, screenHeight);
|
||||
|
||||
+79
-6
@@ -1,23 +1,70 @@
|
||||
/*
|
||||
* control — 电机控制层
|
||||
*
|
||||
* 开环占空比控制：根据目标速度和偏差计算占空比，直接输出到 PWM。
|
||||
* 无编码器反馈，无 PID 闭环。
|
||||
* 开环占空比控制 + 斑马线编码器比例刹车。
|
||||
* 编码器线程: 高速轮询 gpio67 → 算速度(脉冲/秒) → 原子变量共享。
|
||||
*/
|
||||
#include "control.h"
|
||||
#include "global.h"
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <cmath>
|
||||
|
||||
extern double g_steer_deviation;
|
||||
|
||||
MotorController *motorController[2] = {nullptr, nullptr};
|
||||
GPIO mortorEN(73);
|
||||
|
||||
static GPIO encoderLSB(67);
|
||||
static GPIO encoderDIR(72);
|
||||
|
||||
static std::thread g_enc_thread;
|
||||
static std::atomic<bool> g_enc_running{false};
|
||||
static std::atomic<double> g_enc_speed{0.0};
|
||||
static std::atomic<int> g_enc_dir{0};
|
||||
|
||||
static long enc_now_ns() {
|
||||
struct timespec ts;
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
return ts.tv_sec * 1000000000L + ts.tv_nsec;
|
||||
}
|
||||
|
||||
static void encoder_thread() {
|
||||
int prev_lsb = encoderLSB.readValue() ? 1 : 0;
|
||||
int pulse_cnt = 0;
|
||||
long t0 = enc_now_ns();
|
||||
|
||||
while (g_enc_running.load()) {
|
||||
int cur_lsb = encoderLSB.readValue() ? 1 : 0;
|
||||
int cur_dir = encoderDIR.readValue() ? 1 : 0;
|
||||
|
||||
if (cur_lsb != prev_lsb) {
|
||||
pulse_cnt++;
|
||||
prev_lsb = cur_lsb;
|
||||
}
|
||||
g_enc_dir.store(cur_dir);
|
||||
|
||||
long t1 = enc_now_ns();
|
||||
long dt = t1 - t0;
|
||||
if (dt >= 100000000L) {
|
||||
g_enc_speed.store(pulse_cnt / (dt / 1e9));
|
||||
pulse_cnt = 0;
|
||||
t0 = t1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ControlInit()
|
||||
{
|
||||
mortorEN.setDirection("out");
|
||||
mortorEN.setValue(1);
|
||||
|
||||
// 左电机方向: GPIO12 (右电机方向由 MotorController 自管, 但左In2也一样要设)
|
||||
encoderLSB.setDirection("in");
|
||||
encoderDIR.setDirection("in");
|
||||
|
||||
g_enc_running.store(true);
|
||||
g_enc_thread = std::thread(encoder_thread);
|
||||
|
||||
GPIO leftIn2(13);
|
||||
leftIn2.setDirection("out");
|
||||
leftIn2.setValue(1);
|
||||
@@ -37,15 +84,38 @@ void ControlInit()
|
||||
|
||||
void ControlUpdate(double speed, bool zebra_block)
|
||||
{
|
||||
if (zebra_block || !g_cfg.start)
|
||||
if (!g_cfg.start)
|
||||
{
|
||||
for (int i = 0; i < 2; ++i)
|
||||
if (motorController[i]) motorController[i]->updateduty(0);
|
||||
if (!g_cfg.start) mortorEN.setValue(0);
|
||||
mortorEN.setValue(0);
|
||||
return;
|
||||
}
|
||||
|
||||
if (zebra_block)
|
||||
{
|
||||
double cur_speed = g_enc_speed.load();
|
||||
int cur_dir = g_enc_dir.load();
|
||||
|
||||
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)
|
||||
int dir = cur_dir ? 1 : 0;
|
||||
|
||||
for (int i = 0; i < 2; ++i)
|
||||
if (motorController[i])
|
||||
motorController[i]->updateduty(dir ? -brake_pct : brake_pct);
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < 2; ++i)
|
||||
if (motorController[i]) motorController[i]->updateduty(0);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// 弯道降速: 偏差越大，速度越低 (最低 60%)
|
||||
double curve = 1.0 - std::abs(g_steer_deviation) * 0.4;
|
||||
if (curve < 0.6) curve = 0.6;
|
||||
double spd = speed * curve;
|
||||
@@ -58,6 +128,9 @@ void ControlUpdate(double speed, bool zebra_block)
|
||||
|
||||
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];
|
||||
|
||||
@@ -31,4 +31,12 @@ void cfg_load_all()
|
||||
g_cfg.start = readFlag(start_file);
|
||||
g_cfg.showImg = readFlag(showImg_file);
|
||||
g_cfg.debug = readFlag(debug_file);
|
||||
|
||||
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);
|
||||
|
||||
g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
|
||||
g_cfg.brake_max = readDoubleFromFile(brake_max_file);
|
||||
}
|
||||
|
||||
+396
-496
@@ -1,602 +1,502 @@
|
||||
/*
|
||||
* NanoDetHeat v10: 3.2M MACs, 4-class
|
||||
* 优化: BN折叠 + conv_bn_relu融合 + 快速exp + 边界修正
|
||||
* YOLO_Mega_Mild_r2 C++ inference engine v3
|
||||
* Architecture: 3-9-9-13-13-19-19-19-44-44-64-64 -> head(64-64,dw) -> out(64-9)
|
||||
*/
|
||||
#include "model_v10.hpp"
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <cmath>
|
||||
#include <ctime>
|
||||
#include <new>
|
||||
#include <algorithm>
|
||||
#include <unordered_map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
// ============================================================
|
||||
// 快速 exp — Schraudolph 方法 (IEEE 754 整数近似)
|
||||
// ============================================================
|
||||
static inline float fast_exp(float x) {
|
||||
// clamp to avoid overflow: exp(88) fits in float
|
||||
x = std::min(x, 88.0f);
|
||||
x = std::max(x, -87.0f);
|
||||
// exp(x) ~ 2^(x * log2(e))
|
||||
// float 按位: 2^23 * (127 + x*log2(e))
|
||||
union { float f; int i; } u;
|
||||
u.i = (int)(12102203.0f * x) + 1064866805; // 2^23 * log2(e) = 12102203
|
||||
return u.f;
|
||||
}
|
||||
static inline float fast_sigmoid(float x) {
|
||||
return 1.0f / (1.0f + fast_exp(-x));
|
||||
static inline float clamp_f(float v, float lo, float hi) { return v < lo ? lo : (v > hi ? hi : v); }
|
||||
static inline float sigmoid_f(float x) {
|
||||
x = clamp_f(x, -30.0f, 30.0f);
|
||||
return 1.0f / (1.0f + std::exp(-x));
|
||||
}
|
||||
static inline float relu_maybe(float s, bool relu) { return (relu && s < 0.0f) ? 0.0f : s; }
|
||||
|
||||
// ============================================================
|
||||
// 权重 (新增 BN alpha/beta 预计算)
|
||||
// ============================================================
|
||||
struct WT { char n[128]; int nd; int s[4]; float* d; };
|
||||
static int gn=0; static WT* gw=nullptr;
|
||||
static int gn = 0;
|
||||
static WT* gw = nullptr;
|
||||
|
||||
static float* wf(const char* k) {
|
||||
for(int i=0;i<gn;++i) if(!std::strcmp(gw[i].n,k)) return gw[i].d;
|
||||
static WT* wt(const char* k) {
|
||||
for (int i = 0; i < gn; ++i)
|
||||
if (!std::strcmp(gw[i].n, k)) return &gw[i];
|
||||
return nullptr;
|
||||
}
|
||||
static float* wf(const char* k) { WT* t = wt(k); return t ? t->d : nullptr; }
|
||||
|
||||
static void init_lut();
|
||||
|
||||
// 加载后扫描 BN 参数，预计算 alpha/beta 存入权重表
|
||||
static void bn_precompute() {
|
||||
// 扫描所有权重，找 BN 三元组 (weight/bias/running_mean/running_var)
|
||||
std::unordered_map<std::string, int> base_idx;
|
||||
for (int i=0; i<gn; ++i) {
|
||||
std::unordered_map<std::string, int> bn_weight_idx;
|
||||
for (int i = 0; i < gn; ++i) {
|
||||
const char* name = gw[i].n;
|
||||
// 匹配 xxx.weight (BN) 或 xxx.bias (BN) → base = "xxx"
|
||||
const char* dot = std::strrchr(name, '.');
|
||||
if (!dot) continue;
|
||||
std::string base(name, dot - name);
|
||||
int sz = gw[i].s[0];
|
||||
|
||||
if (std::strcmp(dot+1, "weight")==0 && sz>0 && sz <= 256) {
|
||||
base_idx[base] = i;
|
||||
}
|
||||
if (!dot || std::strcmp(dot + 1, "weight") != 0) continue;
|
||||
if (gw[i].nd != 1) continue;
|
||||
bn_weight_idx[std::string(name, dot - name)] = i;
|
||||
}
|
||||
|
||||
std::vector<WT> new_wts;
|
||||
for (auto& [base, wi] : base_idx) {
|
||||
// 找对应的 bias / running_mean / running_var
|
||||
std::string w_key = base + ".weight";
|
||||
std::string b_key = base + ".bias";
|
||||
std::string m_key = base + ".running_mean";
|
||||
std::string v_key = base + ".running_var";
|
||||
|
||||
float* w = wf(w_key.c_str());
|
||||
float* b = wf(b_key.c_str());
|
||||
float* m = wf(m_key.c_str());
|
||||
float* v = wf(v_key.c_str());
|
||||
|
||||
if (!w || !m || !v) continue; // cls_head/size_head 没有 BN
|
||||
int C = gw[wi].s[0];
|
||||
for (auto& kv : bn_weight_idx) {
|
||||
const std::string& base = kv.first;
|
||||
float* w = gw[kv.second].d;
|
||||
float* b = wf((base + ".bias").c_str());
|
||||
float* m = wf((base + ".running_mean").c_str());
|
||||
float* v = wf((base + ".running_var").c_str());
|
||||
if (!m || !v) continue;
|
||||
const int C = gw[kv.second].s[0];
|
||||
float* alpha = new float[C];
|
||||
float* beta = new float[C];
|
||||
const float eps = 1e-5f;
|
||||
for (int c=0; c<C; ++c) {
|
||||
for (int c = 0; c < C; ++c) {
|
||||
alpha[c] = w[c] / std::sqrt(v[c] + eps);
|
||||
beta[c] = (b ? b[c] : 0.0f) - m[c] * alpha[c];
|
||||
}
|
||||
// 存为新的权重条目
|
||||
new_wts.push_back({});
|
||||
std::snprintf(new_wts.back().n, 128, "%s._alpha", base.c_str());
|
||||
new_wts.back().nd=1; new_wts.back().s[0]=C; new_wts.back().s[1]=1; new_wts.back().s[2]=1; new_wts.back().s[3]=1;
|
||||
new_wts.back().d = alpha;
|
||||
new_wts.push_back({});
|
||||
std::snprintf(new_wts.back().n, 128, "%s._beta", base.c_str());
|
||||
new_wts.back().nd=1; new_wts.back().s[0]=C; new_wts.back().s[1]=1; new_wts.back().s[2]=1; new_wts.back().s[3]=1;
|
||||
new_wts.back().d = beta;
|
||||
WT ta = {}; std::snprintf(ta.n, 128, "%s._alpha", base.c_str());
|
||||
ta.nd = 1; ta.s[0] = C; ta.s[1] = ta.s[2] = ta.s[3] = 1; ta.d = alpha;
|
||||
WT tb = {}; std::snprintf(tb.n, 128, "%s._beta", base.c_str());
|
||||
tb.nd = 1; tb.s[0] = C; tb.s[1] = tb.s[2] = tb.s[3] = 1; tb.d = beta;
|
||||
new_wts.push_back(ta); new_wts.push_back(tb);
|
||||
}
|
||||
|
||||
// 重新分配权重数组 (旧数组 + 新条目)
|
||||
WT* old_gw = gw;
|
||||
int old_gn = gn;
|
||||
int new_gn = old_gn + (int)new_wts.size();
|
||||
gw = new WT[new_gn];
|
||||
std::memcpy(gw, old_gw, old_gn * sizeof(WT));
|
||||
for (size_t i=0; i<new_wts.size(); ++i) {
|
||||
gw[old_gn + i] = new_wts[i];
|
||||
}
|
||||
gn = new_gn;
|
||||
delete[] old_gw;
|
||||
init_lut();
|
||||
WT* old = gw;
|
||||
const int old_n = gn, new_n = old_n + (int)new_wts.size();
|
||||
gw = new WT[new_n];
|
||||
std::memcpy(gw, old, old_n * sizeof(WT));
|
||||
for (size_t i = 0; i < new_wts.size(); ++i) gw[old_n + i] = new_wts[i];
|
||||
gn = new_n;
|
||||
delete[] old;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 内存
|
||||
// ============================================================
|
||||
struct M10 {
|
||||
float preproc[3*120*160]; // 预处理 buffer (静态, 避免每帧 new/delete)
|
||||
float stem[6*60*80];
|
||||
float b1[8*60*80];
|
||||
float b2[12*30*40];
|
||||
float b3[16*15*20];
|
||||
float b4[32*15*20];
|
||||
float b5[48*15*20];
|
||||
float b6[48*15*20];
|
||||
float sh[24*15*20];
|
||||
float cl[5*15*20];
|
||||
float sz[2*15*20];
|
||||
float dw_out[8*60*80];
|
||||
float se_buf[80];
|
||||
uint8 peak_mask[15*20];
|
||||
};
|
||||
static M10* m=nullptr;
|
||||
static constexpr int IN_W = 160, IN_H = 120;
|
||||
static constexpr int BUF_FLOATS = 9 * 60 * 80;
|
||||
static float* g_preproc = nullptr;
|
||||
static float* g_A = nullptr;
|
||||
static float* g_B = nullptr;
|
||||
static float* g_T = nullptr;
|
||||
static float* g_out = nullptr;
|
||||
static uint8_t g_resized[IN_W * IN_H * 3];
|
||||
static float g_se[160];
|
||||
static float g_prob[5 * 15 * 20];
|
||||
static float g_lut[256];
|
||||
static char g_dump_dir[256] = {0};
|
||||
|
||||
// ============================================================
|
||||
// Relu
|
||||
// ============================================================
|
||||
static void relu_f(float* x, int N) {
|
||||
for (int i=0; i<N; ++i) if (x[i]<0) x[i]=0;
|
||||
static void dump_tensor(const char* name, const float* d, int count) {
|
||||
if (!g_dump_dir[0]) return;
|
||||
char path[320];
|
||||
std::snprintf(path, sizeof(path), "%s/%s.f32", g_dump_dir, name);
|
||||
FILE* f = std::fopen(path, "wb");
|
||||
if (f) { std::fwrite(d, sizeof(float), count, f); std::fclose(f); }
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 融合: conv + BN + ReLU (使用预计算的 alpha/beta)
|
||||
// ============================================================
|
||||
static void conv_bn_relu(float* o, const float* in,
|
||||
const float* conv_w, const float* conv_b,
|
||||
const float* bn_alpha, const float* bn_beta,
|
||||
int H, int W, int iC, int oC, int K, int str, int grp, bool use_relu)
|
||||
static void conv1x1(float* o, const float* in, const float* cw, const float* cb,
|
||||
const float* ba, const float* bb,
|
||||
int H, int W, int iC, int oC, bool relu)
|
||||
{
|
||||
if (!conv_w || !in || !o) return;
|
||||
int Ho=H/str, Wo=W/str, pad=K/2;
|
||||
int icpg=iC/grp, ocpg=oC/grp;
|
||||
const int N = H * W;
|
||||
for (int oc = 0; oc < oC; ++oc) {
|
||||
float* oo = o + oc * N;
|
||||
const float* ww = cw + oc * iC;
|
||||
const float bias = cb ? cb[oc] : 0.0f;
|
||||
for (int i = 0; i < N; ++i) oo[i] = bias;
|
||||
for (int ic = 0; ic < iC; ++ic) {
|
||||
const float w = ww[ic];
|
||||
const float* ii = in + ic * N;
|
||||
for (int i = 0; i < N; ++i) oo[i] += w * ii[i];
|
||||
}
|
||||
const float a = ba ? ba[oc] : 1.0f;
|
||||
const float b = bb ? bb[oc] : 0.0f;
|
||||
for (int i = 0; i < N; ++i)
|
||||
oo[i] = relu_maybe(oo[i] * a + b, relu);
|
||||
}
|
||||
}
|
||||
|
||||
if (K==1 && str==1) {
|
||||
// 1×1 fused: 指针递增避免 y*W+x
|
||||
for (int g=0; g<grp; ++g) {
|
||||
for (int oc=0; oc<ocpg; ++oc) {
|
||||
int occ = g*ocpg + oc;
|
||||
float* oo = o + occ*H*W;
|
||||
float cb = conv_b ? conv_b[occ] : 0.0f;
|
||||
const float* ww = conv_w + occ*icpg;
|
||||
float ba=bn_alpha?bn_alpha[occ]:1.0f, bb=bn_beta?bn_beta[occ]:0.0f;
|
||||
static void dwconv3x3(float* o, const float* in, const float* cw,
|
||||
const float* ba, const float* bb,
|
||||
int H, int W, int C, bool relu)
|
||||
{
|
||||
for (int c = 0; c < C; ++c) {
|
||||
const float* ii = in + c * H * W;
|
||||
float* oo = o + c * H * W;
|
||||
const float* ww = cw + c * 9;
|
||||
const float w0=ww[0],w1=ww[1],w2=ww[2],w3=ww[3],w4=ww[4],
|
||||
w5=ww[5],w6=ww[6],w7=ww[7],w8=ww[8];
|
||||
const float a = ba ? ba[c] : 1.0f;
|
||||
const float b = bb ? bb[c] : 0.0f;
|
||||
|
||||
for (int y=0; y<H; ++y) {
|
||||
float* oy = oo + y*W;
|
||||
const float* iy = in + y*W;
|
||||
for (int x=0; x<W; ++x) {
|
||||
float s = cb;
|
||||
const float* px = iy + x;
|
||||
for (int ic=0; ic<icpg; ++ic) {
|
||||
int icc = g*icpg + ic;
|
||||
s += ww[ic] * px[icc*H*W];
|
||||
}
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oy[x] = s;
|
||||
}
|
||||
}
|
||||
for (int y = 1; y < H - 1; ++y) {
|
||||
const float *i0 = ii + (y - 1) * W, *i1 = i0 + W, *i2 = i1 + W;
|
||||
float* oy = oo + y * W;
|
||||
for (int x = 1; x < W - 1; ++x) {
|
||||
float s = w0*i0[x-1] + w1*i0[x] + w2*i0[x+1]
|
||||
+ w3*i1[x-1] + w4*i1[x] + w5*i1[x+1]
|
||||
+ w6*i2[x-1] + w7*i2[x] + w8*i2[x+1];
|
||||
oy[x] = relu_maybe(s * a + b, relu);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (grp==iC && K==3 && str==1) {
|
||||
// 3×3 dw fused + interior unrolled / border with checks
|
||||
for (int c=0; c<iC; ++c) {
|
||||
float* const oo = o + c*H*W;
|
||||
const float* const ii = in + c*H*W;
|
||||
const float* const ww = conv_w + c*9;
|
||||
float cb = conv_b ? conv_b[c] : 0.0f;
|
||||
float ba=bn_alpha?bn_alpha[c]:1.0f, bb=bn_beta?bn_beta[c]:0.0f;
|
||||
float w0=ww[0], w1=ww[1], w2=ww[2], w3=ww[3], w4=ww[4], w5=ww[5], w6=ww[6], w7=ww[7], w8=ww[8];
|
||||
|
||||
if (H>=3 && W>=3) {
|
||||
// interior: 1≤y<H-1, 1≤x<W-1 (no boundary checks)
|
||||
for (int y=1; y<H-1; ++y) {
|
||||
float* const oy = oo + y*W + 1;
|
||||
const float* const i0 = ii + (y-1)*W;
|
||||
const float* const i1 = i0 + W;
|
||||
const float* const i2 = i1 + W;
|
||||
for (int x=1; x<W-1; ++x) {
|
||||
float s = cb
|
||||
+ w0*i0[x-1] + w1*i0[x] + w2*i0[x+1]
|
||||
+ w3*i1[x-1] + w4*i1[x] + w5*i1[x+1]
|
||||
+ w6*i2[x-1] + w7*i2[x] + w8*i2[x+1];
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oy[x-1] = s;
|
||||
}
|
||||
}
|
||||
// top row (y=0): boundary check — ky=-1 clamps to y=0
|
||||
{
|
||||
float* const oy = oo;
|
||||
const float* const i0 = ii; // y=0 (ky=-1 and ky=0)
|
||||
const float* const i1 = ii + W; // y=1 (ky=1)
|
||||
for (int x=0; x<W; ++x) {
|
||||
float s = cb;
|
||||
// ky=-1: use i0 (clamped), ky=0: use i0, ky=1: use i1
|
||||
int xl=(x>0)?x-1:x, xr=(x<W-1)?x+1:x;
|
||||
s += w0*i0[xl] + w1*i0[x] + w2*i0[xr] // ky=-1
|
||||
+ w3*i0[xl] + w4*i0[x] + w5*i0[xr] // ky=0
|
||||
+ w6*i1[xl] + w7*i1[x] + w8*i1[xr]; // ky=1
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oy[x] = s;
|
||||
}
|
||||
}
|
||||
// bottom row (y=H-1): boundary check — ky=1 clamps to y=H-1
|
||||
{
|
||||
float* const oy = oo + (H-1)*W;
|
||||
const float* const i0 = ii + (H-2)*W; // y=H-2 (ky=-1)
|
||||
const float* const i1 = ii + (H-1)*W; // y=H-1 (ky=0 and ky=1)
|
||||
for (int x=0; x<W; ++x) {
|
||||
float s = cb;
|
||||
int xl=(x>0)?x-1:x, xr=(x<W-1)?x+1:x;
|
||||
s += w0*i0[xl] + w1*i0[x] + w2*i0[xr] // ky=-1
|
||||
+ w3*i1[xl] + w4*i1[x] + w5*i1[xr] // ky=0
|
||||
+ w6*i1[xl] + w7*i1[x] + w8*i1[xr]; // ky=1
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oy[x] = s;
|
||||
}
|
||||
}
|
||||
// middle rows left border (1≤y<H-1, x=0)
|
||||
for (int y=1; y<H-1; ++y) {
|
||||
const float* const i0 = ii + (y-1)*W;
|
||||
const float* const i1 = i0 + W;
|
||||
const float* const i2 = i1 + W;
|
||||
float s = cb
|
||||
+ w1*i0[0] + w2*i0[1]
|
||||
+ w4*i1[0] + w5*i1[1]
|
||||
+ w7*i2[0] + w8*i2[1];
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oo[y*W] = s;
|
||||
}
|
||||
// middle rows right border (1≤y<H-1, x=W-1)
|
||||
for (int y=1; y<H-1; ++y) {
|
||||
const float* const i0 = ii + (y-1)*W + (W-2);
|
||||
const float* const i1 = i0 + W;
|
||||
const float* const i2 = i1 + W;
|
||||
float s = cb
|
||||
+ w0*i0[0] + w1*i0[1]
|
||||
+ w3*i1[0] + w4*i1[1]
|
||||
+ w6*i2[0] + w7*i2[1];
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oo[y*W+W-1] = s;
|
||||
}
|
||||
} else {
|
||||
// degenerate (small H/W): fallback
|
||||
for (int y=0; y<H; ++y) {
|
||||
for (int x=0; x<W; ++x) {
|
||||
float s = cb;
|
||||
for (int ky=0; ky<3; ++ky) {
|
||||
int iy = y + ky - 1;
|
||||
if (iy<0||iy>=H) continue;
|
||||
for (int kx=0; kx<3; ++kx) {
|
||||
int ix = x + kx - 1;
|
||||
if (ix<0||ix>=W) continue;
|
||||
s += ww[ky*3+kx] * ii[iy*W+ix];
|
||||
}
|
||||
}
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oo[y*W+x] = s;
|
||||
}
|
||||
auto edge = [&](int y, int x) {
|
||||
float s = 0.0f;
|
||||
for (int ky = 0; ky < 3; ++ky) {
|
||||
const int iy = y + ky - 1;
|
||||
if (iy < 0 || iy >= H) continue;
|
||||
for (int kx = 0; kx < 3; ++kx) {
|
||||
const int ix = x + kx - 1;
|
||||
if (ix < 0 || ix >= W) continue;
|
||||
s += ww[ky * 3 + kx] * ii[iy * W + ix];
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
oo[y * W + x] = relu_maybe(s * a + b, relu);
|
||||
};
|
||||
for (int x = 0; x < W; ++x) { edge(0, x); edge(H - 1, x); }
|
||||
for (int y = 1; y < H - 1; ++y) { edge(y, 0); edge(y, W - 1); }
|
||||
}
|
||||
}
|
||||
|
||||
// 通用路径 — 常数外提 + iy*W 预计算
|
||||
for (int g=0; g<grp; ++g) {
|
||||
int ic_base = g*icpg;
|
||||
for (int oc=0; oc<ocpg; ++oc) {
|
||||
int occ = g*ocpg + oc;
|
||||
float* oo = o + occ*Ho*Wo;
|
||||
float cb = conv_b ? conv_b[occ] : 0.0f;
|
||||
float ba=bn_alpha?bn_alpha[occ]:1.0f, bb=bn_beta?bn_beta[occ]:0.0f;
|
||||
|
||||
for (int y=0; y<Ho; ++y) {
|
||||
int yi0 = y*str;
|
||||
for (int x=0; x<Wo; ++x) {
|
||||
float s = cb;
|
||||
int xi0 = x*str;
|
||||
for (int ic=0; ic<icpg; ++ic) {
|
||||
int icc = ic_base + ic;
|
||||
const float* ww = conv_w + ((occ*icpg+ic)*K*K);
|
||||
const float* ii = in + icc*H*W;
|
||||
for (int ky=0; ky<K; ++ky) {
|
||||
int iy = yi0 + ky - pad;
|
||||
if (iy<0||iy>=H) continue;
|
||||
int iy_off = iy*W;
|
||||
for (int kx=0; kx<K; ++kx) {
|
||||
int ix = xi0 + kx - pad;
|
||||
if (ix<0||ix>=W) continue;
|
||||
s += ww[ky*K+kx] * ii[iy_off + ix];
|
||||
}
|
||||
static void conv_generic(float* o, const float* in, const float* cw, const float* cb,
|
||||
const float* ba, const float* bb,
|
||||
int H, int W, int iC, int oC, int K, int str, int grp, bool relu)
|
||||
{
|
||||
const int Ho = H / str, Wo = W / str, pad = K / 2;
|
||||
const int icpg = iC / grp, ocpg = oC / grp;
|
||||
for (int g = 0; g < grp; ++g) {
|
||||
const int ic_base = g * icpg;
|
||||
for (int oc = 0; oc < ocpg; ++oc) {
|
||||
const int occ = g * ocpg + oc;
|
||||
float* oo = o + occ * Ho * Wo;
|
||||
const float bias = cb ? cb[occ] : 0.0f;
|
||||
const float a = ba ? ba[occ] : 1.0f;
|
||||
const float b = bb ? bb[occ] : 0.0f;
|
||||
for (int y = 0; y < Ho; ++y) {
|
||||
const int yi0 = y * str - pad;
|
||||
const int ky0 = std::max(0, -yi0);
|
||||
const int ky1 = std::min(K, H - yi0);
|
||||
for (int x = 0; x < Wo; ++x) {
|
||||
const int xi0 = x * str - pad;
|
||||
const int kx0 = std::max(0, -xi0);
|
||||
const int kx1 = std::min(K, W - xi0);
|
||||
float s = bias;
|
||||
for (int ic = 0; ic < icpg; ++ic) {
|
||||
const float* ww = cw + ((occ * icpg + ic) * K * K);
|
||||
const float* ii = in + (ic_base + ic) * H * W;
|
||||
for (int ky = ky0; ky < ky1; ++ky) {
|
||||
const float* irow = ii + (yi0 + ky) * W + xi0;
|
||||
const float* wrow = ww + ky * K;
|
||||
for (int kx = kx0; kx < kx1; ++kx)
|
||||
s += wrow[kx] * irow[kx];
|
||||
}
|
||||
}
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oo[y*Wo+x] = s;
|
||||
oo[y * Wo + x] = relu_maybe(s * a + b, relu);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 纯 conv (无 BN/ReLU, cls_head / size_head / skip 的 conv 部分)
|
||||
// ============================================================
|
||||
static void conv2d(float* o, const float* in, const float* w, const float* b,
|
||||
int H, int W, int iC, int oC, int K, int str, int grp) {
|
||||
conv_bn_relu(o, in, w, b, nullptr, nullptr, H, W, iC, oC, K, str, grp, false);
|
||||
static void conv_bn_relu(float* o, const float* in,
|
||||
const float* cw, const float* cb,
|
||||
const float* ba, const float* bb,
|
||||
int H, int W, int iC, int oC, int K, int str, int grp, bool relu)
|
||||
{
|
||||
if (!cw || !in || !o) return;
|
||||
if (K == 1 && str == 1 && grp == 1)
|
||||
conv1x1(o, in, cw, cb, ba, bb, H, W, iC, oC, relu);
|
||||
else if (K == 3 && str == 1 && grp == iC && iC == oC)
|
||||
dwconv3x3(o, in, cw, ba, bb, H, W, iC, relu);
|
||||
else
|
||||
conv_generic(o, in, cw, cb, ba, bb, H, W, iC, oC, K, str, grp, relu);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// SE 通道注意力 (使用快速 sigmoid)
|
||||
// ============================================================
|
||||
static void se_module(float* x, int C, int N,
|
||||
const float* w1, const float* b1,
|
||||
const float* w2, const float* b2,
|
||||
float* buf) {
|
||||
if (!w1||!w2) return;
|
||||
int mid=C/4;
|
||||
float* pool=buf;
|
||||
const WT* w1t, const float* b1,
|
||||
const WT* w2t, const float* b2)
|
||||
{
|
||||
if (!w1t || !w2t) return;
|
||||
const float* w1 = w1t->d;
|
||||
const float* w2 = w2t->d;
|
||||
const int mid = w1t->s[0];
|
||||
|
||||
// AdaptiveAvgPool
|
||||
for (int c=0; c<C; ++c) {
|
||||
float s=0;
|
||||
float* px = x+c*N, *end = px+N;
|
||||
while (px < end) { s += *px++; }
|
||||
pool[c]=s/(float)N;
|
||||
float* pool = g_se;
|
||||
float* fc1 = g_se + C;
|
||||
float* fc2 = g_se + C + mid;
|
||||
|
||||
const float inv_n = 1.0f / (float)N;
|
||||
for (int c = 0; c < C; ++c) {
|
||||
float s = 0.0f;
|
||||
const float* px = x + c * N;
|
||||
for (int i = 0; i < N; ++i) s += px[i];
|
||||
pool[c] = s * inv_n;
|
||||
}
|
||||
// fc1: C→mid + ReLU
|
||||
float* fc1=buf+C;
|
||||
for (int o=0; o<mid; ++o) {
|
||||
float s=b1?b1[o]:0;
|
||||
const float* ww=w1+o*C;
|
||||
for (int ic=0; ic<C; ++ic) s+=pool[ic]*ww[ic];
|
||||
fc1[o]=s>0?s:0;
|
||||
for (int o = 0; o < mid; ++o) {
|
||||
float s = b1 ? b1[o] : 0.0f;
|
||||
const float* ww = w1 + o * C;
|
||||
for (int ic = 0; ic < C; ++ic) s += pool[ic] * ww[ic];
|
||||
fc1[o] = s > 0.0f ? s : 0.0f;
|
||||
}
|
||||
// fc2: mid→C + Sigmoid (fast)
|
||||
float* fc2=buf+C+mid;
|
||||
for (int o=0; o<C; ++o) {
|
||||
float s=b2?b2[o]:0;
|
||||
const float* ww=w2+o*mid;
|
||||
for (int ic=0; ic<mid; ++ic) s+=fc1[ic]*ww[ic];
|
||||
fc2[o]=fast_sigmoid(s);
|
||||
for (int o = 0; o < C; ++o) {
|
||||
float s = b2 ? b2[o] : 0.0f;
|
||||
const float* ww = w2 + o * mid;
|
||||
for (int ic = 0; ic < mid; ++ic) s += fc1[ic] * ww[ic];
|
||||
fc2[o] = sigmoid_f(s);
|
||||
}
|
||||
for (int c=0; c<C; ++c) {
|
||||
float* xc=x+c*N; float sc=fc2[c];
|
||||
for (int i=0; i<N; ++i) xc[i]*=sc;
|
||||
for (int c = 0; c < C; ++c) {
|
||||
float* xc = x + c * N;
|
||||
const float sc = fc2[c];
|
||||
for (int i = 0; i < N; ++i) xc[i] *= sc;
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// SE-ResDW Block v10
|
||||
// ============================================================
|
||||
static void se_block_v10(float* o, const float* in, int iC, int oC, int H, int W, int str, const char* pfx) {
|
||||
int Ho=H/str, Wo=W/str, No=Ho*Wo;
|
||||
char na[128], wr[128], bi[128];
|
||||
static constexpr int STRIDE = 8, OH = 15, OW = 20, NC = 4;
|
||||
|
||||
// dw conv + BN + ReLU (fused)
|
||||
std::snprintf(na,128,"%s.dw.0.weight",pfx);
|
||||
std::snprintf(wr,128,"%s.dw.1._alpha",pfx);
|
||||
std::snprintf(bi,128,"%s.dw.1._beta",pfx);
|
||||
conv_bn_relu(m->dw_out, in, wf(na), nullptr,
|
||||
wf(wr), wf(bi), H, W, iC, iC, 3, str, iC, true);
|
||||
static void model_compute() {
|
||||
struct Lay { int iC, oC, H, W, s; const char* bn; const char* sn; };
|
||||
static const Lay lays[] = {
|
||||
{3, 9, 120, 160, 2, "b1", "se_0"},
|
||||
{9, 9, 60, 80, 1, "b2", "se_1"},
|
||||
{9, 13, 60, 80, 2, "b3", "se_2"},
|
||||
{13, 13, 30, 40, 1, "b4", "se_3"},
|
||||
{13, 19, 30, 40, 2, "b5", "se_4"},
|
||||
{19, 19, 15, 20, 1, "b6", "se_5"},
|
||||
{19, 19, 15, 20, 1, "b7", "se_6"},
|
||||
{19, 44, 15, 20, 1, "b8", "se_7"},
|
||||
{44, 44, 15, 20, 1, "b9", "se_8"},
|
||||
{44, 64, 15, 20, 1, "b10", "se_9"},
|
||||
{64, 64, 15, 20, 1, "b11", "se_10"},
|
||||
};
|
||||
|
||||
// pw conv + BN + ReLU (fused)
|
||||
std::snprintf(na,128,"%s.pw.0.weight",pfx);
|
||||
std::snprintf(wr,128,"%s.pw.1._alpha",pfx);
|
||||
std::snprintf(bi,128,"%s.pw.1._beta",pfx);
|
||||
conv_bn_relu(o, m->dw_out, wf(na), nullptr,
|
||||
wf(wr), wf(bi), Ho, Wo, iC, oC, 1, 1, 1, true);
|
||||
const float* in = g_preproc;
|
||||
float* bufs[2] = { g_A, g_B };
|
||||
int cur = 0;
|
||||
|
||||
// skip (BN fused, no ReLU)
|
||||
bool iden=(str==1 && iC==oC);
|
||||
float* skip=m->dw_out;
|
||||
if (iden) {
|
||||
std::memcpy(skip, in, iC*H*W*4);
|
||||
} else {
|
||||
std::snprintf(na,128,"%s.sk.0.weight",pfx);
|
||||
std::snprintf(wr,128,"%s.sk.1._alpha",pfx);
|
||||
std::snprintf(bi,128,"%s.sk.1._beta",pfx);
|
||||
conv_bn_relu(skip, in, wf(na), nullptr,
|
||||
wf(wr), wf(bi), H, W, iC, oC, 1, str, 1, false);
|
||||
for (const Lay& l : lays) {
|
||||
int K, grp;
|
||||
if (l.s == 2) { K = 3; grp = 1; }
|
||||
else if (l.iC == l.oC) { K = 3; grp = l.iC; }
|
||||
else { K = 1; grp = 1; }
|
||||
|
||||
const int Ho = l.H / l.s, Wo = l.W / l.s;
|
||||
float* out = bufs[cur];
|
||||
|
||||
char wkey[128], akey[128], bkey[128];
|
||||
std::snprintf(wkey, 128, "%s.0.weight", l.bn);
|
||||
std::snprintf(akey, 128, "%s.1._alpha", l.bn);
|
||||
std::snprintf(bkey, 128, "%s.1._beta", l.bn);
|
||||
conv_bn_relu(out, in, wf(wkey), nullptr, wf(akey), wf(bkey),
|
||||
l.H, l.W, l.iC, l.oC, K, l.s, grp, true);
|
||||
|
||||
char s1[128], s1b[128], s3[128], s3b[128];
|
||||
std::snprintf(s1, 128, "%s.1.weight", l.sn);
|
||||
std::snprintf(s1b, 128, "%s.1.bias", l.sn);
|
||||
std::snprintf(s3, 128, "%s.3.weight", l.sn);
|
||||
std::snprintf(s3b, 128, "%s.3.bias", l.sn);
|
||||
se_module(out, l.oC, Ho * Wo, wt(s1), wf(s1b), wt(s3), wf(s3b));
|
||||
|
||||
in = out;
|
||||
cur ^= 1;
|
||||
}
|
||||
for (int i=0; i<oC*No; ++i) o[i]+=skip[i];
|
||||
|
||||
// SE
|
||||
std::snprintf(na,128,"%s.se.1.weight",pfx);
|
||||
std::snprintf(bi,128,"%s.se.1.bias",pfx);
|
||||
char se2[128], se2b[128];
|
||||
std::snprintf(se2,128,"%s.se.3.weight",pfx);
|
||||
std::snprintf(se2b,128,"%s.se.3.bias",pfx);
|
||||
se_module(o, oC, No, wf(na), wf(bi), wf(se2), wf(se2b), m->se_buf);
|
||||
conv_bn_relu(g_T, in, wf("head.0.weight"), nullptr,
|
||||
wf("head.1._alpha"), wf("head.1._beta"),
|
||||
15, 20, 64, 64, 3, 1, 64, true);
|
||||
|
||||
relu_f(o, oC*No);
|
||||
conv_bn_relu(g_out, g_T, wf("out.weight"), wf("out.bias"),
|
||||
nullptr, nullptr, 15, 20, 64, 9, 1, 1, 1, false);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 解码 — softmax + NMS (预计算 peak mask)
|
||||
// ============================================================
|
||||
static constexpr int OH=15, OW=20, STRIDE=8, NC=4;
|
||||
static constexpr float RS[NC][2] = {
|
||||
{56.85f,45.39f}, {101.44f,66.53f}, {96.75f,62.86f}, {66.01f,35.49f},
|
||||
};
|
||||
static int decode(DetectBoxV10* boxes, int max_boxes, float thr) {
|
||||
const int N = OH * OW;
|
||||
int cnt = 0;
|
||||
|
||||
static int decode(DetectBoxV10* boxes, int max, const float* th) {
|
||||
int N=OH*OW, cnt=0;
|
||||
|
||||
// 预计算 peak mask: 每个类别做一次 maxpool
|
||||
uint8* mask = m->peak_mask;
|
||||
for (int c=0; c<NC; ++c) {
|
||||
const float* cp = m->cl + c*N;
|
||||
uint8* mp = mask + c*N;
|
||||
for (int gy=0; gy<OH; ++gy) {
|
||||
for (int gx=0; gx<OW; ++gx) {
|
||||
if (cp[gy*OW+gx] < 0) { mp[gy*OW+gx]=0; continue; }
|
||||
bool peak=true;
|
||||
float val=cp[gy*OW+gx];
|
||||
for (int dy=-1; dy<=1&&peak; ++dy)
|
||||
for (int dx=-1; dx<=1&&peak; ++dx) {
|
||||
int ny=gy+dy, nx=gx+dx;
|
||||
if (ny<0||ny>=OH||nx<0||nx>=OW) continue;
|
||||
if (cp[ny*OW+nx] > val) peak=false;
|
||||
}
|
||||
mp[gy*OW+gx] = peak ? 1 : 0;
|
||||
}
|
||||
for (int idx = 0; idx < N; ++idx) {
|
||||
float mx = -1e30f;
|
||||
for (int c = 0; c <= NC; ++c) mx = std::max(mx, g_out[c * N + idx]);
|
||||
float sum = 0.0f;
|
||||
for (int c = 0; c <= NC; ++c) {
|
||||
const float e = std::exp(g_out[c * N + idx] - mx);
|
||||
g_prob[c * N + idx] = e;
|
||||
sum += e;
|
||||
}
|
||||
const float inv = 1.0f / sum;
|
||||
for (int c = 0; c <= NC; ++c) g_prob[c * N + idx] *= inv;
|
||||
}
|
||||
|
||||
// softmax + check per cell
|
||||
for (int gy=0; gy<OH && cnt<max; ++gy) {
|
||||
for (int gx=0; gx<OW && cnt<max; ++gx) {
|
||||
int idx=gy*OW+gx;
|
||||
for (int c = 0; c < NC && cnt < max_boxes; ++c) {
|
||||
for (int gy = 0; gy < OH; ++gy) {
|
||||
for (int gx = 0; gx < OW; ++gx) {
|
||||
const int idx = gy * OW + gx;
|
||||
const float pc = g_prob[c * N + idx];
|
||||
if (pc <= thr || pc <= g_prob[NC * N + idx]) continue;
|
||||
|
||||
// softmax over NC+1 classes (fast exp)
|
||||
float sm[5], mx=-1e9f, sum=0;
|
||||
for (int c=0; c<=NC; ++c) {
|
||||
float v=m->cl[c*N+idx];
|
||||
sm[c]=v; if(v>mx) mx=v;
|
||||
bool peak = true;
|
||||
for (int dy = -1; dy <= 1 && peak; ++dy)
|
||||
for (int dx = -1; dx <= 1 && peak; ++dx) {
|
||||
const int ny = gy + dy, nx = gx + dx;
|
||||
if (ny < 0 || ny >= OH || nx < 0 || nx >= OW) continue;
|
||||
if (g_prob[c * N + ny * OW + nx] > pc) peak = false;
|
||||
}
|
||||
if (!peak) continue;
|
||||
|
||||
const float tx = clamp_f(g_out[(NC + 1) * N + idx], 0.0f, 1.0f);
|
||||
const float ty = clamp_f(g_out[(NC + 2) * N + idx], 0.0f, 1.0f);
|
||||
const float tw = std::max(g_out[(NC + 3) * N + idx], 0.5f);
|
||||
const float th = std::max(g_out[(NC + 4) * N + idx], 0.5f);
|
||||
const float cx = (gx + tx) * STRIDE, cy = (gy + ty) * STRIDE;
|
||||
const float w = tw * STRIDE, h = th * STRIDE;
|
||||
|
||||
boxes[cnt].cls = c;
|
||||
boxes[cnt].conf = pc;
|
||||
boxes[cnt].cx = cx;
|
||||
boxes[cnt].cy = cy;
|
||||
boxes[cnt].w = w;
|
||||
boxes[cnt].h = h;
|
||||
if (++cnt >= max_boxes) return cnt;
|
||||
}
|
||||
for (int c=0; c<=NC; ++c) { sm[c]=fast_exp(sm[c]-mx); sum+=sm[c]; }
|
||||
|
||||
// best class (excluding bg=NC), per-class threshold
|
||||
int bc=-1; float bs=0;
|
||||
for (int c=0; c<NC; ++c) {
|
||||
if (!mask[c*N+idx]) continue;
|
||||
float p=sm[c]/sum;
|
||||
if (p>bs && p>=th[c]) { bs=p; bc=c; }
|
||||
}
|
||||
if (bc<0) continue;
|
||||
|
||||
float pw=std::max(1.0f, m->sz[0*N+idx]*RS[bc][0]);
|
||||
float ph=std::max(1.0f, m->sz[1*N+idx]*RS[bc][1]);
|
||||
boxes[cnt].cls=bc; boxes[cnt].conf=bs;
|
||||
boxes[cnt].cx=((float)gx+0.5f)*STRIDE;
|
||||
boxes[cnt].cy=((float)gy+0.5f)*STRIDE;
|
||||
boxes[cnt].w=pw; boxes[cnt].h=ph;
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 预处理 LUT: uint8→float normalized
|
||||
// ============================================================
|
||||
static float g_lut[256];
|
||||
static void init_lut() {
|
||||
for (int i=0; i<256; ++i) g_lut[i] = (float)i / 255.0f;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 前向推理 — 模型计算体 (preproc 已由调用方完成)
|
||||
// ============================================================
|
||||
static void model_compute() {
|
||||
float* in = m->preproc;
|
||||
|
||||
// stem: 3→6, s2 (fused)
|
||||
conv_bn_relu(m->stem, in, wf("s.0.weight"), nullptr,
|
||||
wf("s.1._alpha"), wf("s.1._beta"), 120, 160, 3, 6, 3, 2, 1, true);
|
||||
|
||||
// blocks
|
||||
se_block_v10(m->b1, m->stem, 6, 8, 60, 80, 1, "b1");
|
||||
se_block_v10(m->b2, m->b1, 8, 12, 60, 80, 2, "b2");
|
||||
se_block_v10(m->b3, m->b2, 12, 16, 30, 40, 2, "b3");
|
||||
se_block_v10(m->b4, m->b3, 16, 32, 15, 20, 1, "b4");
|
||||
se_block_v10(m->b5, m->b4, 32, 48, 15, 20, 1, "b5");
|
||||
se_block_v10(m->b6, m->b5, 48, 48, 15, 20, 1, "b6");
|
||||
|
||||
// shared: 48→24, 1×1 (fused)
|
||||
conv_bn_relu(m->sh, m->b6, wf("sh.0.weight"), nullptr,
|
||||
wf("sh.1._alpha"), wf("sh.1._beta"), 15, 20, 48, 24, 1, 1, 1, true);
|
||||
|
||||
// heads (pure conv, no BN)
|
||||
conv2d(m->cl, m->sh, wf("ch.weight"), wf("ch.bias"), 15, 20, 24, 5, 1, 1, 1);
|
||||
conv2d(m->sz, m->sh, wf("sz.weight"), wf("sz.bias"), 15, 20, 24, 2, 1, 1, 1);
|
||||
}
|
||||
|
||||
// ── 自写 640×480 → 3×120×160 float planar (4×4 box avg + BGR→RGB + /255) ──
|
||||
static void preproc_640(const uint8* bgr) {
|
||||
float* in = m->preproc;
|
||||
const int sw=640, sh=480;
|
||||
for(int c=0;c<3;++c){
|
||||
int sc=2-c;
|
||||
float* ch = in + c*120*160;
|
||||
for(int y=0;y<120;++y){
|
||||
int iy=y*4;
|
||||
for(int x=0;x<160;++x){
|
||||
int ix=x*4, sum=0;
|
||||
for(int dy=0;dy<4;++dy)
|
||||
for(int dx=0;dx<4;++dx)
|
||||
sum += bgr[(iy+dy)*sw*3 + (ix+dx)*3 + sc];
|
||||
ch[y*160+x] = (float)sum * (1.0f/(16.0f*255.0f));
|
||||
// cv2.resize INTER_LINEAR compatible
|
||||
static void resize_bilinear_hwc3(const uint8_t* src, int sw, int sh, uint8_t* dst) {
|
||||
const float fx = (float)sw / (float)IN_W;
|
||||
const float fy = (float)sh / (float)IN_H;
|
||||
static int x0i[IN_W], x1i[IN_W];
|
||||
static float xw[IN_W];
|
||||
for (int x = 0; x < IN_W; ++x) {
|
||||
float sx = (x + 0.5f) * fx - 0.5f;
|
||||
if (sx < 0) sx = 0;
|
||||
int x0 = (int)sx;
|
||||
if (x0 > sw - 1) x0 = sw - 1;
|
||||
int x1 = std::min(x0 + 1, sw - 1);
|
||||
x0i[x] = x0 * 3; x1i[x] = x1 * 3; xw[x] = sx - (float)x0;
|
||||
}
|
||||
for (int y = 0; y < IN_H; ++y) {
|
||||
float sy = (y + 0.5f) * fy - 0.5f;
|
||||
if (sy < 0) sy = 0;
|
||||
int y0 = (int)sy;
|
||||
if (y0 > sh - 1) y0 = sh - 1;
|
||||
const int y1 = std::min(y0 + 1, sh - 1);
|
||||
const float wy = sy - (float)y0;
|
||||
const uint8_t* r0 = src + (size_t)y0 * sw * 3;
|
||||
const uint8_t* r1 = src + (size_t)y1 * sw * 3;
|
||||
uint8_t* d = dst + y * IN_W * 3;
|
||||
for (int x = 0; x < IN_W; ++x) {
|
||||
const float wx = xw[x];
|
||||
const int a = x0i[x], b = x1i[x];
|
||||
for (int c = 0; c < 3; ++c) {
|
||||
const float top = r0[a + c] + (r0[b + c] - r0[a + c]) * wx;
|
||||
const float bot = r1[a + c] + (r1[b + c] - r1[a + c]) * wx;
|
||||
const float v = top + (bot - top) * wy;
|
||||
d[x * 3 + c] = (uint8_t)(v + 0.5f);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ── 原版: 160×120 BGR → 3×120×160 float planar ──
|
||||
static void preproc_160(const uint8* bgr) {
|
||||
float* in = m->preproc;
|
||||
for (int c=0; c<3; ++c) {
|
||||
int sc=2-c; float* ch=in+c*120*160;
|
||||
for (int y=0; y<120; ++y) {
|
||||
const uint8* row=bgr+y*160*3;
|
||||
for (int x=0; x<160; ++x) ch[y*160+x]=g_lut[row[x*3+sc]];
|
||||
static void preproc_160(const uint8_t* img, YoloPixFmt fmt) {
|
||||
for (int c = 0; c < 3; ++c) {
|
||||
const int sc = (fmt == YOLO_FMT_BGR) ? (2 - c) : c;
|
||||
float* ch = g_preproc + c * IN_H * IN_W;
|
||||
for (int y = 0; y < IN_H; ++y) {
|
||||
const uint8_t* row = img + y * IN_W * 3;
|
||||
for (int x = 0; x < IN_W; ++x) ch[y * IN_W + x] = g_lut[row[x * 3 + sc]];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ── 内部: preproc + compute ──
|
||||
static void forward(const uint8* bgr) { preproc_160(bgr); model_compute(); }
|
||||
|
||||
// ============================================================
|
||||
// 接口
|
||||
// ============================================================
|
||||
static bool g_rdy=false;
|
||||
static bool g_rdy = false;
|
||||
|
||||
bool model_v10_init(const char* path) {
|
||||
FILE* f=fopen(path,"rb");
|
||||
if(!f) return false;
|
||||
fread(&gn,sizeof(int),1,f);
|
||||
gw=new WT[gn];
|
||||
for (int i=0; i<gn; ++i) {
|
||||
WT& t=gw[i]; int nl; fread(&nl,4,1,f); fread(t.n,1,nl,f); t.n[nl]=0;
|
||||
fread(&t.nd,4,1,f); int tot=1;
|
||||
for (int d=0; d<t.nd; ++d) { fread(&t.s[d],4,1,f); tot*=t.s[d]; }
|
||||
for (int d=t.nd; d<4; ++d) t.s[d]=1;
|
||||
t.d=new float[tot]; fread(t.d,4,tot,f);
|
||||
FILE* f = std::fopen(path, "rb");
|
||||
if (!f) return false;
|
||||
if (std::fread(&gn, sizeof(int), 1, f) != 1 || gn <= 0 || gn > 4096) { std::fclose(f); return false; }
|
||||
gw = new WT[gn];
|
||||
for (int i = 0; i < gn; ++i) {
|
||||
WT& t = gw[i];
|
||||
int nl = 0;
|
||||
bool ok = std::fread(&nl, 4, 1, f) == 1 && nl > 0 && nl < 127
|
||||
&& std::fread(t.n, 1, nl, f) == (size_t)nl;
|
||||
if (ok) {
|
||||
t.n[nl] = 0;
|
||||
ok = std::fread(&t.nd, 4, 1, f) == 1 && t.nd >= 0 && t.nd <= 4;
|
||||
}
|
||||
if (ok) {
|
||||
int tot = 1;
|
||||
for (int d = 0; d < t.nd && ok; ++d) {
|
||||
ok = std::fread(&t.s[d], 4, 1, f) == 1 && t.s[d] > 0;
|
||||
tot *= t.s[d];
|
||||
}
|
||||
for (int d = t.nd; d < 4; ++d) t.s[d] = 1;
|
||||
if (ok) {
|
||||
t.d = new float[tot];
|
||||
ok = std::fread(t.d, 4, tot, f) == (size_t)tot;
|
||||
}
|
||||
}
|
||||
if (!ok) { gn = (t.d ? i + 1 : i); std::fclose(f); model_v10_deinit(); return false; }
|
||||
}
|
||||
fclose(f);
|
||||
std::fclose(f);
|
||||
|
||||
bn_precompute();
|
||||
m=new(std::nothrow) M10();
|
||||
if(!m) { delete[] gw; gw=nullptr; return false; }
|
||||
std::memset(m,0,sizeof(M10));
|
||||
g_rdy=true;
|
||||
for (int i = 0; i < 256; ++i) g_lut[i] = (float)i / 255.0f;
|
||||
|
||||
g_preproc = new(std::nothrow) float[3 * IN_H * IN_W];
|
||||
g_A = new(std::nothrow) float[BUF_FLOATS];
|
||||
g_B = new(std::nothrow) float[BUF_FLOATS];
|
||||
g_T = new(std::nothrow) float[BUF_FLOATS];
|
||||
g_out = new(std::nothrow) float[9 * 15 * 20];
|
||||
if (!g_preproc || !g_A || !g_B || !g_T || !g_out) { model_v10_deinit(); return false; }
|
||||
|
||||
g_rdy = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
int model_v10_detect(const uint8* bgr, int w, int h, DetectBoxV10* boxes, int max, const float* thresh) {
|
||||
if(!g_rdy) return 0;
|
||||
if(w==640 && h==480) {
|
||||
preproc_640(bgr);
|
||||
model_compute();
|
||||
return decode(boxes,max,thresh);
|
||||
int model_v10_detect_fmt(const uint8_t* img, int w, int h, YoloPixFmt fmt,
|
||||
DetectBoxV10* boxes, int max_boxes, float thr) {
|
||||
if (!g_rdy || !img || !boxes || max_boxes <= 0 || w <= 1 || h <= 1) return 0;
|
||||
|
||||
const uint8_t* net_in = img;
|
||||
if (w != IN_W || h != IN_H) {
|
||||
resize_bilinear_hwc3(img, w, h, g_resized);
|
||||
net_in = g_resized;
|
||||
}
|
||||
if(w!=160||h!=120) return 0;
|
||||
forward(bgr);
|
||||
return decode(boxes,max,thresh);
|
||||
preproc_160(net_in, fmt);
|
||||
model_compute();
|
||||
const int n = decode(boxes, max_boxes, thr);
|
||||
|
||||
const float sx = (float)w / (float)IN_W;
|
||||
const float sy = (float)h / (float)IN_H;
|
||||
for (int i = 0; i < n; ++i) {
|
||||
boxes[i].cx *= sx; boxes[i].w *= sx;
|
||||
boxes[i].cy *= sy; boxes[i].h *= sy;
|
||||
}
|
||||
return n;
|
||||
}
|
||||
|
||||
int model_v10_detect(const uint8_t* img, int w, int h, DetectBoxV10* boxes, int max_boxes, const float* thresh) {
|
||||
float thr = thresh ? thresh[0] : 0.6f;
|
||||
return model_v10_detect_fmt(img, w, h, YOLO_FMT_BGR, boxes, max_boxes, thr);
|
||||
}
|
||||
|
||||
const float* model_v10_forward_debug(const float* chw) {
|
||||
if (!g_rdy || !chw) return nullptr;
|
||||
std::memcpy(g_preproc, chw, 3 * IN_H * IN_W * sizeof(float));
|
||||
model_compute();
|
||||
return g_out;
|
||||
}
|
||||
|
||||
void model_v10_set_dump_dir(const char* dir) {
|
||||
if (dir) { std::strncpy(g_dump_dir, dir, sizeof(g_dump_dir) - 1); g_dump_dir[sizeof(g_dump_dir)-1] = 0; }
|
||||
else g_dump_dir[0] = 0;
|
||||
}
|
||||
|
||||
void model_v10_deinit() {
|
||||
if(gw) { for(int i=0; i<gn; ++i) delete[] gw[i].d; delete[] gw; gw=nullptr; }
|
||||
delete m; m=nullptr; g_rdy=false;
|
||||
if (gw) { for (int i = 0; i < gn; ++i) delete[] gw[i].d; delete[] gw; gw = nullptr; }
|
||||
gn = 0;
|
||||
delete[] g_preproc; delete[] g_A; delete[] g_B; delete[] g_T; delete[] g_out;
|
||||
g_preproc = g_A = g_B = g_T = g_out = nullptr;
|
||||
g_rdy = false;
|
||||
}
|
||||
|
||||
bool model_v10_ready() { return g_rdy; }
|
||||
|
||||
+3
-1
@@ -2,11 +2,13 @@
|
||||
#include "types_model.hpp"
|
||||
|
||||
struct DetectBoxV10 {
|
||||
int cls; // 0=锥 1=红灯 2=绿灯 3=行人
|
||||
int cls;
|
||||
float conf;
|
||||
float cx, cy, w, h;
|
||||
};
|
||||
|
||||
typedef enum { YOLO_FMT_BGR = 0, YOLO_FMT_RGB = 1 } YoloPixFmt;
|
||||
|
||||
bool model_v10_init(const char* path);
|
||||
int model_v10_detect(const uint8* bgr, int w, int h, DetectBoxV10* boxes, int max, const float* thresh);
|
||||
void model_v10_deinit();
|
||||
|
||||
@@ -1,5 +0,0 @@
|
||||
# demo2
|
||||
add_executable(udp_receive udp_receive.cpp)
|
||||
|
||||
# 链接 OpenCV 库
|
||||
target_link_libraries(udp_receive common_lib ${OpenCV_LIBS})
|
||||
@@ -1,104 +0,0 @@
|
||||
'''
|
||||
Author: ilikara 3435193369@qq.com
|
||||
Date: 2025-03-11 03:00:45
|
||||
LastEditors: Ilikara 3435193369@qq.com
|
||||
LastEditTime: 2025-03-11 17:04:09
|
||||
FilePath: /2k300_smartcar/udp_receive/slider.py
|
||||
Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||
'''
|
||||
import tkinter as tk
|
||||
import socket
|
||||
import struct
|
||||
|
||||
# 配置参数
|
||||
TARGET_IP = "192.168.43.238" # 目标板卡IP
|
||||
TARGET_PORT = 8888 # 目标端口
|
||||
|
||||
|
||||
class UdpSliderApp:
|
||||
def __init__(self, master):
|
||||
self.master = master
|
||||
master.title("Dual Slider Control")
|
||||
|
||||
# 创建UDP socket
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
|
||||
# 滑块1
|
||||
self.slider1_frame = tk.Frame(master)
|
||||
self.slider1_frame.pack(pady=10)
|
||||
self.label1 = tk.Label(self.slider1_frame, text="Angle: 0.00")
|
||||
self.label1.pack(side=tk.LEFT)
|
||||
self.slider1 = tk.Scale(
|
||||
self.slider1_frame,
|
||||
from_=-7.0, to=7.0,
|
||||
resolution=0.01,
|
||||
orient=tk.HORIZONTAL,
|
||||
length=300,
|
||||
command=lambda v: self.send_values()
|
||||
)
|
||||
self.slider1.pack(side=tk.LEFT)
|
||||
|
||||
# 滑块2
|
||||
self.slider2_frame = tk.Frame(master)
|
||||
self.slider2_frame.pack(pady=10)
|
||||
self.label2 = tk.Label(self.slider2_frame, text="Speed: 0.00")
|
||||
self.label2.pack(side=tk.LEFT)
|
||||
self.slider2 = tk.Scale(
|
||||
self.slider2_frame,
|
||||
from_=-20.0, to=20.0, # 示例范围可调
|
||||
resolution=0.1,
|
||||
orient=tk.HORIZONTAL,
|
||||
length=300,
|
||||
command=lambda v: self.send_values()
|
||||
)
|
||||
self.slider2.pack(side=tk.LEFT)
|
||||
|
||||
# 退出按钮
|
||||
self.exit_btn = tk.Button(
|
||||
master,
|
||||
text="Exit",
|
||||
command=master.quit,
|
||||
bg="#ff4444",
|
||||
fg="white"
|
||||
)
|
||||
self.exit_btn.pack(pady=20)
|
||||
|
||||
self.reset_btn = tk.Button(
|
||||
self.slider2_frame,
|
||||
text="Reset to 0",
|
||||
command=self.reset_slider2,
|
||||
bg="#ff6666",
|
||||
fg="white",
|
||||
width=8
|
||||
)
|
||||
self.reset_btn.pack(side=tk.LEFT, padx=10)
|
||||
|
||||
def send_values(self):
|
||||
"""打包并发送两个浮点数"""
|
||||
try:
|
||||
val1 = float(self.slider1.get())
|
||||
val2 = float(self.slider2.get())
|
||||
|
||||
# 更新标签
|
||||
self.label1.config(text=f"Angle: {val1:.2f}")
|
||||
self.label2.config(text=f"Speed: {val2:.2f}")
|
||||
|
||||
# 打包为网络字节序的两个float
|
||||
data = struct.pack('!ff', val1, val2) # !表示网络字节序
|
||||
|
||||
# 发送UDP数据
|
||||
self.sock.sendto(data, (TARGET_IP, TARGET_PORT))
|
||||
except Exception as e:
|
||||
print(f"发送错误: {str(e)}")
|
||||
|
||||
def reset_slider2(self):
|
||||
self.slider2.set(0.0) # 设置滑块物理位置
|
||||
self.label2.config(text="Slider2: 0.00") # 直接更新显示
|
||||
self.send_values() # 手动触发发送
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
root = tk.Tk()
|
||||
app = UdpSliderApp(root)
|
||||
root.mainloop()
|
||||
app.sock.close()
|
||||
@@ -1,175 +0,0 @@
|
||||
/*
|
||||
* @Author: ilikara 3435193369@qq.com
|
||||
* @Date: 2025-03-11 02:38:15
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-03-29 01:39:17
|
||||
* @FilePath: /2k300_smartcar/udp_receive/udp_receive.cpp
|
||||
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||
*/
|
||||
#include <iostream>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <unistd.h>
|
||||
#include <cstdint>
|
||||
#include <string.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <thread>
|
||||
|
||||
#include "MotorController.h"
|
||||
#include "PwmController.h"
|
||||
#include "GPIO.h"
|
||||
#include <iomanip>
|
||||
|
||||
cv::VideoCapture cap;
|
||||
|
||||
std::string directory = "./image";
|
||||
int saved_frame_count;
|
||||
void streamCapture(void)
|
||||
{
|
||||
cv::Mat frame;
|
||||
while (1)
|
||||
{
|
||||
cap.read(frame);
|
||||
if (frame.empty())
|
||||
{
|
||||
std::cerr << "Save Error: Frame is empty." << std::endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
// 构建文件名
|
||||
std::ostringstream filename;
|
||||
filename << directory << "/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg";
|
||||
|
||||
saved_frame_count++;
|
||||
// 保存图像
|
||||
cv::imwrite(filename.str(), frame);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
GPIO mortorEN(73);
|
||||
MotorController *motorController[2] = {nullptr, nullptr};
|
||||
PwmController servo(1, 0);
|
||||
|
||||
void Init()
|
||||
{
|
||||
mortorEN.setDirection("out");
|
||||
mortorEN.setValue(1);
|
||||
const int pwmchip[2] = {8, 8};
|
||||
const int pwmnum[2] = {2, 1};
|
||||
const int gpioNum[2] = {12, 13};
|
||||
const int encoder_pwmchip[2] = {0, 3};
|
||||
const int encoder_gpioNum[2] = {75, 72};
|
||||
const int encoder_dir[2] = {1, -1};
|
||||
const unsigned int period_ns = 50000; // 20 kHz
|
||||
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
motorController[i] = new MotorController(pwmchip[i], pwmnum[i], gpioNum[i], period_ns,
|
||||
0, 0, 0, 0,
|
||||
encoder_pwmchip[i], encoder_gpioNum[i], encoder_dir[i]);
|
||||
motorController[i]->updateduty(0);
|
||||
}
|
||||
|
||||
servo.setPeriod(3040000);
|
||||
servo.setDutyCycle(1520000);
|
||||
servo.enable();
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// 打开默认摄像头（设备编号 0）
|
||||
cap.open(0);
|
||||
|
||||
// 检查摄像头是否成功打开
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
printf("无法打开摄像头\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, 320); // 宽度
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, 240); // 高度
|
||||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); // 视频流格式
|
||||
cap.set(cv::CAP_PROP_AUTO_EXPOSURE, -1); // 设置自动曝光
|
||||
|
||||
std::thread camworker = std::thread(streamCapture);
|
||||
|
||||
Init();
|
||||
|
||||
const int PORT = 8888;
|
||||
const int BUFFER_SIZE = 2 * sizeof(float);
|
||||
|
||||
// 创建UDP Socket
|
||||
int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
if (sockfd < 0)
|
||||
{
|
||||
std::cerr << "Socket creation failed" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 绑定地址和端口
|
||||
sockaddr_in server_addr{};
|
||||
server_addr.sin_family = AF_INET;
|
||||
server_addr.sin_addr.s_addr = INADDR_ANY;
|
||||
server_addr.sin_port = htons(PORT);
|
||||
|
||||
if (bind(sockfd, (sockaddr *)&server_addr, sizeof(server_addr)))
|
||||
{
|
||||
std::cerr << "Bind failed" << std::endl;
|
||||
close(sockfd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 接收数据
|
||||
sockaddr_in client_addr{};
|
||||
socklen_t client_len = sizeof(client_addr);
|
||||
char buffer[BUFFER_SIZE];
|
||||
|
||||
while (true)
|
||||
{
|
||||
ssize_t bytes_received = recvfrom(
|
||||
sockfd,
|
||||
buffer,
|
||||
BUFFER_SIZE,
|
||||
0,
|
||||
(sockaddr *)&client_addr,
|
||||
&client_len);
|
||||
|
||||
if (bytes_received == BUFFER_SIZE)
|
||||
{
|
||||
// 解析第一个float
|
||||
uint32_t temp1;
|
||||
memcpy(&temp1, buffer, 4);
|
||||
temp1 = ntohl(temp1);
|
||||
float val1;
|
||||
memcpy(&val1, &temp1, 4);
|
||||
|
||||
// 解析第二个float
|
||||
uint32_t temp2;
|
||||
memcpy(&temp2, buffer + 4, 4);
|
||||
temp2 = ntohl(temp2);
|
||||
float val2;
|
||||
memcpy(&val2, &temp2, 4);
|
||||
|
||||
std::cout << "收到数据: "
|
||||
<< "滑块1=" << val1
|
||||
<< ", 滑块2=" << val2
|
||||
<< std::endl;
|
||||
|
||||
double servoduty_ns = (val1) / 100 * servo.readPeriod() + 1520000;
|
||||
servo.setDutyCycle(servoduty_ns);
|
||||
|
||||
motorController[0]->updateduty(val2);
|
||||
motorController[1]->updateduty(val2);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cerr << "Invalid data size" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
close(sockfd);
|
||||
return 0;
|
||||
}
|
||||
@@ -1,10 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.5.0)
|
||||
project(vl53l0x_demo VERSION 0.1 LANGUAGES CXX)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
|
||||
include_directories(${CMAKE_SOURCE_DIR}/lib)
|
||||
include_directories(${CMAKE_SOURCE_DIR}/src)
|
||||
|
||||
add_executable(vl53l0x_demo vl53l0x_demo.cpp)
|
||||
target_link_libraries(vl53l0x_demo common_lib pthread)
|
||||
@@ -1,44 +0,0 @@
|
||||
/*
|
||||
* VL53L0X 激光测距 demo — 持续读取距离并打印
|
||||
*/
|
||||
#include <iostream>
|
||||
#include <csignal>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include "vl53l0x.h"
|
||||
|
||||
std::atomic<bool> running(true);
|
||||
|
||||
void onSignal(int)
|
||||
{
|
||||
running = false;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
std::signal(SIGINT, onSignal);
|
||||
|
||||
VL53L0X tof;
|
||||
if (!tof.init())
|
||||
{
|
||||
std::cerr << "VL53L0X 初始化失败" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
VL53L0X_RangingMeasurementData_t data;
|
||||
while (running)
|
||||
{
|
||||
if (tof.readRange(data))
|
||||
{
|
||||
std::cout << "dist=" << data.RangeMilliMeter << "mm"
|
||||
<< " status=" << (int)data.RangeStatus
|
||||
<< " signal=" << data.SignalRateRtnMegaCps
|
||||
<< std::endl;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
|
||||
tof.stop();
|
||||
std::cout << "VL53L0X demo exit" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
@@ -1,32 +0,0 @@
|
||||
#!/bin/sh
|
||||
# VL53L0X 激光测距初始化脚本
|
||||
# GPIO 50=SCL, GPIO 51=SDA -> I2C2_SCL[1]/I2C2_SDA[1]
|
||||
|
||||
echo "[VL53L0X] === 检查/初始化激光测距 ==="
|
||||
|
||||
# 1. 检查设备节点是否已存在
|
||||
if [ -c /dev/stmvl53l0x_ranging ]; then
|
||||
echo "[VL53L0X] /dev/stmvl53l0x_ranging 已存在"
|
||||
else
|
||||
echo "[VL53L0X] /dev/stmvl53l0x_ranging 不存在，尝试加载驱动..."
|
||||
modprobe stmvl53l0x 2>/dev/null || modprobe vl53l0x 2>/dev/null
|
||||
if [ ! -c /dev/stmvl53l0x_ranging ]; then
|
||||
echo "[VL53L0X] 驱动未加载，需要检查内核配置或设备树"
|
||||
fi
|
||||
fi
|
||||
|
||||
# 2. 设置 GPIO 50/51 为 I2C 功能（尝试通过 debugfs pinctrl）
|
||||
PINMUX=/sys/kernel/debug/pinctrl/zx296718-pinctrl
|
||||
if [ -d "$PINMUX" ]; then
|
||||
echo "[VL53L0X] pinctrl debugfs 可用: $PINMUX"
|
||||
ls "$PINMUX"/
|
||||
else
|
||||
echo "[VL53L0X] pinctrl debugfs 不可用"
|
||||
fi
|
||||
|
||||
# 3. 最终确认
|
||||
if [ -c /dev/stmvl53l0x_ranging ]; then
|
||||
echo "[VL53L0X] OK — 设备就绪，可以使用"
|
||||
else
|
||||
echo "[VL53L0X] FAIL — 需要先配置内核/设备树中的 I2C 引脚复用"
|
||||
fi
|
||||
@@ -1,36 +0,0 @@
|
||||
LOCAL_PATH:= $(call my-dir)
|
||||
|
||||
|
||||
include $(CLEAR_VARS)
|
||||
|
||||
LOCAL_MODULE_TAGS := eng
|
||||
LOCAL_SRC_FILES:=vl53l0x_test.c
|
||||
LOCAL_MODULE:=vl53l0x_test
|
||||
LOCAL_CPPFLAGS += -DANDROID
|
||||
LOCAL_SHARED_LIBRARIES:=libc
|
||||
LOCAL_STATIC_LIBRARIES :=
|
||||
LOCAL_C_INCLUDES += $(LOCAL_PATH) $(LOCAL_PATH)/$(KERNEL_DIR)/include
|
||||
include $(BUILD_EXECUTABLE)
|
||||
|
||||
include $(CLEAR_VARS)
|
||||
|
||||
LOCAL_MODULE_TAGS := eng
|
||||
LOCAL_SRC_FILES:=vl53l0x_reg.c
|
||||
LOCAL_MODULE:=vl53l0x_reg
|
||||
LOCAL_CPPFLAGS += -DANDROID
|
||||
LOCAL_SHARED_LIBRARIES:=libc
|
||||
LOCAL_STATIC_LIBRARIES :=
|
||||
LOCAL_C_INCLUDES += $(LOCAL_PATH) $(LOCAL_PATH)/$(KERNEL_DIR)/include
|
||||
include $(BUILD_EXECUTABLE)
|
||||
|
||||
include $(CLEAR_VARS)
|
||||
|
||||
LOCAL_MODULE_TAGS := eng
|
||||
LOCAL_SRC_FILES:=vl53l0x_parameter.c
|
||||
LOCAL_MODULE:=vl53l0x_parameter
|
||||
LOCAL_CPPFLAGS += -DANDROID
|
||||
LOCAL_SHARED_LIBRARIES:=libc
|
||||
LOCAL_STATIC_LIBRARIES :=
|
||||
LOCAL_C_INCLUDES += $(LOCAL_PATH) $(LOCAL_PATH)/$(KERNEL_DIR)/include
|
||||
include $(BUILD_EXECUTABLE)
|
||||
|
||||
@@ -1,15 +0,0 @@
|
||||
CFLAGS=-I$(ROOTFS_INC) -I./
|
||||
CC=/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6/bin/loongarch64-linux-gnu-gcc
|
||||
all: vl53l0x_test vl53l0x_reg vl53l0x_parameter
|
||||
vl53l0x_test: vl53l0x_test.o
|
||||
$(CC) -o vl53l0x_test vl53l0x_test.o $(CFLAGS)
|
||||
vl53l0x_reg: vl53l0x_reg.o
|
||||
$(CC) -o vl53l0x_reg vl53l0x_reg.o $(CFLAGS)
|
||||
vl53l0x_parameter: vl53l0x_parameter.o
|
||||
$(CC) -o vl53l0x_parameter vl53l0x_parameter.o $(CFLAGS)
|
||||
|
||||
.PHONY: clean
|
||||
|
||||
clean:
|
||||
rm -f ./*.o *~ core vl53l0x_test vl53l0x_reg vl53l0x_parameter
|
||||
|
||||
@@ -1,640 +0,0 @@
|
||||
/*******************************************************************************
|
||||
Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
/**
|
||||
* @file VL53L0X_def.h
|
||||
*
|
||||
* @brief Type definitions for VL53L0X API.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _VL53L0X_DEF_H_
|
||||
#define _VL53L0X_DEF_H_
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** @defgroup VL53L0X_globaldefine_group VL53L0X Defines
|
||||
* @brief VL53L0X Defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
/** PAL SPECIFICATION major version */
|
||||
#define VL53L0X10_SPECIFICATION_VER_MAJOR 1
|
||||
/** PAL SPECIFICATION minor version */
|
||||
#define VL53L0X10_SPECIFICATION_VER_MINOR 2
|
||||
/** PAL SPECIFICATION sub version */
|
||||
#define VL53L0X10_SPECIFICATION_VER_SUB 7
|
||||
/** PAL SPECIFICATION sub version */
|
||||
#define VL53L0X10_SPECIFICATION_VER_REVISION 1440
|
||||
|
||||
/** VL53L0X PAL IMPLEMENTATION major version */
|
||||
#define VL53L0X10_IMPLEMENTATION_VER_MAJOR 1
|
||||
/** VL53L0X PAL IMPLEMENTATION minor version */
|
||||
#define VL53L0X10_IMPLEMENTATION_VER_MINOR 0
|
||||
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||
#define VL53L0X10_IMPLEMENTATION_VER_SUB 9
|
||||
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||
#define VL53L0X10_IMPLEMENTATION_VER_REVISION 3673
|
||||
|
||||
/** PAL SPECIFICATION major version */
|
||||
#define VL53L0X_SPECIFICATION_VER_MAJOR 1
|
||||
/** PAL SPECIFICATION minor version */
|
||||
#define VL53L0X_SPECIFICATION_VER_MINOR 2
|
||||
/** PAL SPECIFICATION sub version */
|
||||
#define VL53L0X_SPECIFICATION_VER_SUB 7
|
||||
/** PAL SPECIFICATION sub version */
|
||||
#define VL53L0X_SPECIFICATION_VER_REVISION 1440
|
||||
|
||||
/** VL53L0X PAL IMPLEMENTATION major version */
|
||||
#define VL53L0X_IMPLEMENTATION_VER_MAJOR 1
|
||||
/** VL53L0X PAL IMPLEMENTATION minor version */
|
||||
#define VL53L0X_IMPLEMENTATION_VER_MINOR 0
|
||||
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||
#define VL53L0X_IMPLEMENTATION_VER_SUB 1
|
||||
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||
#define VL53L0X_IMPLEMENTATION_VER_REVISION 4606
|
||||
#define VL53L0X_DEFAULT_MAX_LOOP 200
|
||||
#define VL53L0X_MAX_STRING_LENGTH 32
|
||||
|
||||
|
||||
#include "vl53l0x_device.h"
|
||||
#include "vl53l0x_types.h"
|
||||
|
||||
|
||||
/****************************************
|
||||
* PRIVATE define do not edit
|
||||
****************************************/
|
||||
|
||||
/** @brief Defines the parameters of the Get Version Functions
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t revision; /*!< revision number */
|
||||
uint8_t major; /*!< major number */
|
||||
uint8_t minor; /*!< minor number */
|
||||
uint8_t build; /*!< build number */
|
||||
} VL53L0X_Version_t;
|
||||
|
||||
|
||||
/** @brief Defines the parameters of the Get Device Info Functions
|
||||
*/
|
||||
typedef struct {
|
||||
char Name[VL53L0X_MAX_STRING_LENGTH];
|
||||
/*!< Name of the Device e.g. Left_Distance */
|
||||
char Type[VL53L0X_MAX_STRING_LENGTH];
|
||||
/*!< Type of the Device e.g VL53L0X */
|
||||
char ProductId[VL53L0X_MAX_STRING_LENGTH];
|
||||
/*!< Product Identifier String */
|
||||
uint8_t ProductType;
|
||||
/*!< Product Type, VL53L0X = 1, VL53L1 = 2 */
|
||||
uint8_t ProductRevisionMajor;
|
||||
/*!< Product revision major */
|
||||
uint8_t ProductRevisionMinor;
|
||||
/*!< Product revision minor */
|
||||
} VL53L0X_DeviceInfo_t;
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_Error_group Error and Warning code returned by API
|
||||
* The following DEFINE are used to identify the PAL ERROR
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef int8_t VL53L0X_Error;
|
||||
|
||||
#define VL53L0X_ERROR_NONE ((VL53L0X_Error) 0)
|
||||
#define VL53L0X_ERROR_CALIBRATION_WARNING ((VL53L0X_Error) -1)
|
||||
/*!< Warning invalid calibration data may be in used
|
||||
\a VL53L0X_InitData()
|
||||
\a VL53L0X_GetOffsetCalibrationData
|
||||
\a VL53L0X_SetOffsetCalibrationData */
|
||||
#define VL53L0X_ERROR_MIN_CLIPPED ((VL53L0X_Error) -2)
|
||||
/*!< Warning parameter passed was clipped to min before to be applied */
|
||||
|
||||
#define VL53L0X_ERROR_UNDEFINED ((VL53L0X_Error) -3)
|
||||
/*!< Unqualified error */
|
||||
#define VL53L0X_ERROR_INVALID_PARAMS ((VL53L0X_Error) -4)
|
||||
/*!< Parameter passed is invalid or out of range */
|
||||
#define VL53L0X_ERROR_NOT_SUPPORTED ((VL53L0X_Error) -5)
|
||||
/*!< Function is not supported in current mode or configuration */
|
||||
#define VL53L0X_ERROR_RANGE_ERROR ((VL53L0X_Error) -6)
|
||||
/*!< Device report a ranging error interrupt status */
|
||||
#define VL53L0X_ERROR_TIME_OUT ((VL53L0X_Error) -7)
|
||||
/*!< Aborted due to time out */
|
||||
#define VL53L0X_ERROR_MODE_NOT_SUPPORTED ((VL53L0X_Error) -8)
|
||||
/*!< Asked mode is not supported by the device */
|
||||
#define VL53L0X_ERROR_BUFFER_TOO_SMALL ((VL53L0X_Error) -9)
|
||||
/*!< ... */
|
||||
#define VL53L0X_ERROR_GPIO_NOT_EXISTING ((VL53L0X_Error) -10)
|
||||
/*!< User tried to setup a non-existing GPIO pin */
|
||||
#define VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED ((VL53L0X_Error) -11)
|
||||
/*!< unsupported GPIO functionality */
|
||||
#define VL53L0X_ERROR_INTERRUPT_NOT_CLEARED ((VL53L0X_Error) -12)
|
||||
/*!< Error during interrupt clear */
|
||||
#define VL53L0X_ERROR_CONTROL_INTERFACE ((VL53L0X_Error) -20)
|
||||
/*!< error reported from IO functions */
|
||||
#define VL53L0X_ERROR_INVALID_COMMAND ((VL53L0X_Error) -30)
|
||||
/*!< The command is not allowed in the current device state
|
||||
* (power down) */
|
||||
#define VL53L0X_ERROR_DIVISION_BY_ZERO ((VL53L0X_Error) -40)
|
||||
/*!< In the function a division by zero occurs */
|
||||
#define VL53L0X_ERROR_REF_SPAD_INIT ((VL53L0X_Error) -50)
|
||||
/*!< Error during reference SPAD initialization */
|
||||
#define VL53L0X_ERROR_NOT_IMPLEMENTED ((VL53L0X_Error) -99)
|
||||
/*!< Tells requested functionality has not been implemented yet or
|
||||
* not compatible with the device */
|
||||
/** @} VL53L0X_define_Error_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_DeviceModes_group Defines Device modes
|
||||
* Defines all possible modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_DeviceModes;
|
||||
|
||||
#define VL53L0X_DEVICEMODE_SINGLE_RANGING ((VL53L0X_DeviceModes) 0)
|
||||
#define VL53L0X_DEVICEMODE_CONTINUOUS_RANGING ((VL53L0X_DeviceModes) 1)
|
||||
#define VL53L0X_DEVICEMODE_SINGLE_HISTOGRAM ((VL53L0X_DeviceModes) 2)
|
||||
#define VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING ((VL53L0X_DeviceModes) 3)
|
||||
#define VL53L0X_DEVICEMODE_SINGLE_ALS ((VL53L0X_DeviceModes) 10)
|
||||
#define VL53L0X_DEVICEMODE_GPIO_DRIVE ((VL53L0X_DeviceModes) 20)
|
||||
#define VL53L0X_DEVICEMODE_GPIO_OSC ((VL53L0X_DeviceModes) 21)
|
||||
/* ... Modes to be added depending on device */
|
||||
/** @} VL53L0X_define_DeviceModes_group */
|
||||
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_HistogramModes_group Defines Histogram modes
|
||||
* Defines all possible Histogram modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_HistogramModes;
|
||||
|
||||
#define VL53L0X_HISTOGRAMMODE_DISABLED ((VL53L0X_HistogramModes) 0)
|
||||
/*!< Histogram Disabled */
|
||||
#define VL53L0X_HISTOGRAMMODE_REFERENCE_ONLY ((VL53L0X_HistogramModes) 1)
|
||||
/*!< Histogram Reference array only */
|
||||
#define VL53L0X_HISTOGRAMMODE_RETURN_ONLY ((VL53L0X_HistogramModes) 2)
|
||||
/*!< Histogram Return array only */
|
||||
#define VL53L0X_HISTOGRAMMODE_BOTH ((VL53L0X_HistogramModes) 3)
|
||||
/*!< Histogram both Reference and Return Arrays */
|
||||
/* ... Modes to be added depending on device */
|
||||
/** @} VL53L0X_define_HistogramModes_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_PowerModes_group List of available Power Modes
|
||||
* List of available Power Modes
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef uint8_t VL53L0X_PowerModes;
|
||||
|
||||
#define VL53L0X_POWERMODE_STANDBY_LEVEL1 ((VL53L0X_PowerModes) 0)
|
||||
/*!< Standby level 1 */
|
||||
#define VL53L0X_POWERMODE_STANDBY_LEVEL2 ((VL53L0X_PowerModes) 1)
|
||||
/*!< Standby level 2 */
|
||||
#define VL53L0X_POWERMODE_IDLE_LEVEL1 ((VL53L0X_PowerModes) 2)
|
||||
/*!< Idle level 1 */
|
||||
#define VL53L0X_POWERMODE_IDLE_LEVEL2 ((VL53L0X_PowerModes) 3)
|
||||
/*!< Idle level 2 */
|
||||
|
||||
/** @} VL53L0X_define_PowerModes_group */
|
||||
|
||||
|
||||
/** @brief Defines all parameters for the device
|
||||
*/
|
||||
typedef struct {
|
||||
VL53L0X_DeviceModes DeviceMode;
|
||||
/*!< Defines type of measurement to be done for the next measure */
|
||||
VL53L0X_HistogramModes HistogramMode;
|
||||
/*!< Defines type of histogram measurement to be done for the next
|
||||
* measure */
|
||||
uint32_t MeasurementTimingBudgetMicroSeconds;
|
||||
/*!< Defines the allowed total time for a single measurement */
|
||||
uint32_t InterMeasurementPeriodMilliSeconds;
|
||||
/*!< Defines time between two consecutive measurements (between two
|
||||
* measurement starts). If set to 0 means back-to-back mode */
|
||||
uint8_t XTalkCompensationEnable;
|
||||
/*!< Tells if Crosstalk compensation shall be enable or not */
|
||||
uint16_t XTalkCompensationRangeMilliMeter;
|
||||
/*!< CrossTalk compensation range in millimeter */
|
||||
FixPoint1616_t XTalkCompensationRateMegaCps;
|
||||
/*!< CrossTalk compensation rate in Mega counts per seconds.
|
||||
* Expressed in 16.16 fixed point format. */
|
||||
int32_t RangeOffsetMicroMeters;
|
||||
/*!< Range offset adjustment (mm). */
|
||||
|
||||
uint8_t LimitChecksEnable[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||
/*!< This Array store all the Limit Check enable for this device. */
|
||||
uint8_t LimitChecksStatus[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||
/*!< This Array store all the Status of the check linked to last
|
||||
* measurement. */
|
||||
FixPoint1616_t LimitChecksValue[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||
/*!< This Array store all the Limit Check value for this device */
|
||||
|
||||
uint8_t WrapAroundCheckEnable;
|
||||
/*!< Tells if Wrap Around Check shall be enable or not */
|
||||
} VL53L0X_DeviceParameters_t;
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_State_group Defines the current status of the device
|
||||
* Defines the current status of the device
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef uint8_t VL53L0X_State;
|
||||
|
||||
#define VL53L0X_STATE_POWERDOWN ((VL53L0X_State) 0)
|
||||
/*!< Device is in HW reset */
|
||||
#define VL53L0X_STATE_WAIT_STATICINIT ((VL53L0X_State) 1)
|
||||
/*!< Device is initialized and wait for static initialization */
|
||||
#define VL53L0X_STATE_STANDBY ((VL53L0X_State) 2)
|
||||
/*!< Device is in Low power Standby mode */
|
||||
#define VL53L0X_STATE_IDLE ((VL53L0X_State) 3)
|
||||
/*!< Device has been initialized and ready to do measurements */
|
||||
#define VL53L0X_STATE_RUNNING ((VL53L0X_State) 4)
|
||||
/*!< Device is performing measurement */
|
||||
#define VL53L0X_STATE_UNKNOWN ((VL53L0X_State) 98)
|
||||
/*!< Device is in unknown state and need to be rebooted */
|
||||
#define VL53L0X_STATE_ERROR ((VL53L0X_State) 99)
|
||||
/*!< Device is in error state and need to be rebooted */
|
||||
|
||||
/** @} VL53L0X_define_State_group */
|
||||
|
||||
|
||||
/** @brief Structure containing the Dmax computation parameters and data
|
||||
*/
|
||||
typedef struct {
|
||||
int32_t AmbTuningWindowFactor_K;
|
||||
/*!< internal algo tuning (*1000) */
|
||||
int32_t RetSignalAt0mm;
|
||||
/*!< intermediate dmax computation value caching */
|
||||
} VL53L0X_DMaxData_t;
|
||||
|
||||
/**
|
||||
* @struct VL53L0X_RangeData_t
|
||||
* @brief Range measurement data.
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t TimeStamp; /*!< 32-bit time stamp. */
|
||||
uint32_t MeasurementTimeUsec;
|
||||
/*!< Give the Measurement time needed by the device to do the
|
||||
* measurement.*/
|
||||
|
||||
|
||||
uint16_t RangeMilliMeter; /*!< range distance in millimeter. */
|
||||
|
||||
uint16_t RangeDMaxMilliMeter;
|
||||
/*!< Tells what is the maximum detection distance of the device
|
||||
* in current setup and environment conditions (Filled when
|
||||
* applicable) */
|
||||
|
||||
FixPoint1616_t SignalRateRtnMegaCps;
|
||||
/*!< Return signal rate (MCPS)\n these is a 16.16 fix point
|
||||
* value, which is effectively a measure of target
|
||||
* reflectance.*/
|
||||
FixPoint1616_t AmbientRateRtnMegaCps;
|
||||
/*!< Return ambient rate (MCPS)\n these is a 16.16 fix point
|
||||
* value, which is effectively a measure of the ambien
|
||||
* t light.*/
|
||||
|
||||
uint16_t EffectiveSpadRtnCount;
|
||||
/*!< Return the effective SPAD count for the return signal.
|
||||
* To obtain Real value it should be divided by 256 */
|
||||
|
||||
uint8_t ZoneId;
|
||||
/*!< Denotes which zone and range scheduler stage the range
|
||||
* data relates to. */
|
||||
uint8_t RangeFractionalPart;
|
||||
/*!< Fractional part of range distance. Final value is a
|
||||
* FixPoint168 value. */
|
||||
uint8_t RangeStatus;
|
||||
/*!< Range Status for the current measurement. This is device
|
||||
* dependent. Value = 0 means value is valid.
|
||||
* See \ref RangeStatusPage */
|
||||
} VL53L0X_RangingMeasurementData_t;
|
||||
|
||||
|
||||
#define VL53L0X_HISTOGRAM_BUFFER_SIZE 24
|
||||
|
||||
/**
|
||||
* @struct VL53L0X_HistogramData_t
|
||||
* @brief Histogram measurement data.
|
||||
*/
|
||||
typedef struct {
|
||||
/* Histogram Measurement data */
|
||||
uint32_t HistogramData[VL53L0X_HISTOGRAM_BUFFER_SIZE];
|
||||
/*!< Histogram data */
|
||||
uint8_t HistogramType; /*!< Indicate the types of histogram data :
|
||||
Return only, Reference only, both Return and Reference */
|
||||
uint8_t FirstBin; /*!< First Bin value */
|
||||
uint8_t BufferSize; /*!< Buffer Size - Set by the user.*/
|
||||
uint8_t NumberOfBins;
|
||||
/*!< Number of bins filled by the histogram measurement */
|
||||
|
||||
VL53L0X_DeviceError ErrorStatus;
|
||||
/*!< Error status of the current measurement. \n
|
||||
see @a ::VL53L0X_DeviceError @a VL53L0X_GetStatusErrorString() */
|
||||
} VL53L0X_HistogramMeasurementData_t;
|
||||
|
||||
#define VL53L0X_REF_SPAD_BUFFER_SIZE 6
|
||||
|
||||
/**
|
||||
* @struct VL53L0X_SpadData_t
|
||||
* @brief Spad Configuration Data.
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t RefSpadEnables[VL53L0X_REF_SPAD_BUFFER_SIZE];
|
||||
/*!< Reference Spad Enables */
|
||||
uint8_t RefGoodSpadMap[VL53L0X_REF_SPAD_BUFFER_SIZE];
|
||||
/*!< Reference Spad Good Spad Map */
|
||||
} VL53L0X_SpadData_t;
|
||||
|
||||
typedef struct {
|
||||
FixPoint1616_t OscFrequencyMHz; /* Frequency used */
|
||||
|
||||
uint16_t LastEncodedTimeout;
|
||||
/* last encoded Time out used for timing budget*/
|
||||
|
||||
VL53L0X_GpioFunctionality Pin0GpioFunctionality;
|
||||
/* store the functionality of the GPIO: pin0 */
|
||||
|
||||
uint32_t FinalRangeTimeoutMicroSecs;
|
||||
/*!< Execution time of the final range*/
|
||||
uint8_t FinalRangeVcselPulsePeriod;
|
||||
/*!< Vcsel pulse period (pll clocks) for the final range measurement*/
|
||||
uint32_t PreRangeTimeoutMicroSecs;
|
||||
/*!< Execution time of the final range*/
|
||||
uint8_t PreRangeVcselPulsePeriod;
|
||||
/*!< Vcsel pulse period (pll clocks) for the pre-range measurement*/
|
||||
|
||||
uint16_t SigmaEstRefArray;
|
||||
/*!< Reference array sigma value in 1/100th of [mm] e.g. 100 = 1mm */
|
||||
uint16_t SigmaEstEffPulseWidth;
|
||||
/*!< Effective Pulse width for sigma estimate in 1/100th
|
||||
* of ns e.g. 900 = 9.0ns */
|
||||
uint16_t SigmaEstEffAmbWidth;
|
||||
/*!< Effective Ambient width for sigma estimate in 1/100th of ns
|
||||
* e.g. 500 = 5.0ns */
|
||||
|
||||
|
||||
uint8_t ReadDataFromDeviceDone; /* Indicate if read from device has
|
||||
been done (==1) or not (==0) */
|
||||
uint8_t ModuleId; /* Module ID */
|
||||
uint8_t Revision; /* test Revision */
|
||||
char ProductId[VL53L0X_MAX_STRING_LENGTH];
|
||||
/* Product Identifier String */
|
||||
uint8_t ReferenceSpadCount; /* used for ref spad management */
|
||||
uint8_t ReferenceSpadType; /* used for ref spad management */
|
||||
uint8_t RefSpadsInitialised; /* reports if ref spads are initialised. */
|
||||
uint32_t PartUIDUpper; /*!< Unique Part ID Upper */
|
||||
uint32_t PartUIDLower; /*!< Unique Part ID Lower */
|
||||
FixPoint1616_t SignalRateMeasFixed400mm; /*!< Peek Signal rate
|
||||
at 400 mm*/
|
||||
|
||||
} VL53L0X_DeviceSpecificParameters_t;
|
||||
|
||||
/**
|
||||
* @struct VL53L0X_DevData_t
|
||||
*
|
||||
* @brief VL53L0X PAL device ST private data structure \n
|
||||
* End user should never access any of these field directly
|
||||
*
|
||||
* These must never access directly but only via macro
|
||||
*/
|
||||
typedef struct {
|
||||
VL53L0X_DMaxData_t DMaxData;
|
||||
/*!< Dmax Data */
|
||||
int32_t Part2PartOffsetNVMMicroMeter;
|
||||
/*!< backed up NVM value */
|
||||
int32_t Part2PartOffsetAdjustmentNVMMicroMeter;
|
||||
/*!< backed up NVM value representing additional offset adjustment */
|
||||
VL53L0X_DeviceParameters_t CurrentParameters;
|
||||
/*!< Current Device Parameter */
|
||||
VL53L0X_RangingMeasurementData_t LastRangeMeasure;
|
||||
/*!< Ranging Data */
|
||||
VL53L0X_HistogramMeasurementData_t LastHistogramMeasure;
|
||||
/*!< Histogram Data */
|
||||
VL53L0X_DeviceSpecificParameters_t DeviceSpecificParameters;
|
||||
/*!< Parameters specific to the device */
|
||||
VL53L0X_SpadData_t SpadData;
|
||||
/*!< Spad Data */
|
||||
uint8_t SequenceConfig;
|
||||
/*!< Internal value for the sequence config */
|
||||
uint8_t RangeFractionalEnable;
|
||||
/*!< Enable/Disable fractional part of ranging data */
|
||||
VL53L0X_State PalState;
|
||||
/*!< Current state of the PAL for this device */
|
||||
VL53L0X_PowerModes PowerMode;
|
||||
/*!< Current Power Mode */
|
||||
uint16_t SigmaEstRefArray;
|
||||
/*!< Reference array sigma value in 1/100th of [mm] e.g. 100 = 1mm */
|
||||
uint16_t SigmaEstEffPulseWidth;
|
||||
/*!< Effective Pulse width for sigma estimate in 1/100th
|
||||
* of ns e.g. 900 = 9.0ns */
|
||||
uint16_t SigmaEstEffAmbWidth;
|
||||
/*!< Effective Ambient width for sigma estimate in 1/100th of ns
|
||||
* e.g. 500 = 5.0ns */
|
||||
uint8_t StopVariable;
|
||||
/*!< StopVariable used during the stop sequence */
|
||||
uint16_t targetRefRate;
|
||||
/*!< Target Ambient Rate for Ref spad management */
|
||||
FixPoint1616_t SigmaEstimate;
|
||||
/*!< Sigma Estimate - based on ambient & VCSEL rates and
|
||||
* signal_total_events */
|
||||
FixPoint1616_t SignalEstimate;
|
||||
/*!< Signal Estimate - based on ambient & VCSEL rates and cross talk */
|
||||
FixPoint1616_t LastSignalRefMcps;
|
||||
/*!< Latest Signal ref in Mcps */
|
||||
uint8_t *pTuningSettingsPointer;
|
||||
/*!< Pointer for Tuning Settings table */
|
||||
uint8_t UseInternalTuningSettings;
|
||||
/*!< Indicate if we use Tuning Settings table */
|
||||
uint16_t LinearityCorrectiveGain;
|
||||
/*!< Linearity Corrective Gain value in x1000 */
|
||||
uint16_t DmaxCalRangeMilliMeter;
|
||||
/*!< Dmax Calibration Range millimeter */
|
||||
FixPoint1616_t DmaxCalSignalRateRtnMegaCps;
|
||||
/*!< Dmax Calibration Signal Rate Return MegaCps */
|
||||
|
||||
} VL53L0X_DevData_t;
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_InterruptPolarity_group Defines the Polarity
|
||||
* of the Interrupt
|
||||
* Defines the Polarity of the Interrupt
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_InterruptPolarity;
|
||||
|
||||
#define VL53L0X_INTERRUPTPOLARITY_LOW ((VL53L0X_InterruptPolarity) 0)
|
||||
/*!< Set active low polarity best setup for falling edge. */
|
||||
#define VL53L0X_INTERRUPTPOLARITY_HIGH ((VL53L0X_InterruptPolarity) 1)
|
||||
/*!< Set active high polarity best setup for rising edge. */
|
||||
|
||||
/** @} VL53L0X_define_InterruptPolarity_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_VcselPeriod_group Vcsel Period Defines
|
||||
* Defines the range measurement for which to access the vcsel period.
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_VcselPeriod;
|
||||
|
||||
#define VL53L0X_VCSEL_PERIOD_PRE_RANGE ((VL53L0X_VcselPeriod) 0)
|
||||
/*!<Identifies the pre-range vcsel period. */
|
||||
#define VL53L0X_VCSEL_PERIOD_FINAL_RANGE ((VL53L0X_VcselPeriod) 1)
|
||||
/*!<Identifies the final range vcsel period. */
|
||||
|
||||
/** @} VL53L0X_define_VcselPeriod_group */
|
||||
|
||||
/** @defgroup VL53L0X_define_SchedulerSequence_group Defines the steps
|
||||
* carried out by the scheduler during a range measurement.
|
||||
* @{
|
||||
* Defines the states of all the steps in the scheduler
|
||||
* i.e. enabled/disabled.
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t TccOn; /*!<Reports if Target Centre Check On */
|
||||
uint8_t MsrcOn; /*!<Reports if MSRC On */
|
||||
uint8_t DssOn; /*!<Reports if DSS On */
|
||||
uint8_t PreRangeOn; /*!<Reports if Pre-Range On */
|
||||
uint8_t FinalRangeOn; /*!<Reports if Final-Range On */
|
||||
} VL53L0X_SchedulerSequenceSteps_t;
|
||||
|
||||
/** @} VL53L0X_define_SchedulerSequence_group */
|
||||
|
||||
/** @defgroup VL53L0X_define_SequenceStepId_group Defines the Polarity
|
||||
* of the Interrupt
|
||||
* Defines the the sequence steps performed during ranging..
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_SequenceStepId;
|
||||
|
||||
#define VL53L0X_SEQUENCESTEP_TCC ((VL53L0X_VcselPeriod) 0)
|
||||
/*!<Target CentreCheck identifier. */
|
||||
#define VL53L0X_SEQUENCESTEP_DSS ((VL53L0X_VcselPeriod) 1)
|
||||
/*!<Dynamic Spad Selection function Identifier. */
|
||||
#define VL53L0X_SEQUENCESTEP_MSRC ((VL53L0X_VcselPeriod) 2)
|
||||
/*!<Minimum Signal Rate Check function Identifier. */
|
||||
#define VL53L0X_SEQUENCESTEP_PRE_RANGE ((VL53L0X_VcselPeriod) 3)
|
||||
/*!<Pre-Range check Identifier. */
|
||||
#define VL53L0X_SEQUENCESTEP_FINAL_RANGE ((VL53L0X_VcselPeriod) 4)
|
||||
/*!<Final Range Check Identifier. */
|
||||
|
||||
#define VL53L0X_SEQUENCESTEP_NUMBER_OF_CHECKS 5
|
||||
/*!<Number of Sequence Step Managed by the API. */
|
||||
|
||||
/** @} VL53L0X_define_SequenceStepId_group */
|
||||
|
||||
|
||||
/* MACRO Definitions */
|
||||
/** @defgroup VL53L0X_define_GeneralMacro_group General Macro Defines
|
||||
* General Macro Defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
/* Defines */
|
||||
#define VL53L0X_SETPARAMETERFIELD(Dev, field, value) \
|
||||
PALDevDataSet(Dev, CurrentParameters.field, value)
|
||||
|
||||
#define VL53L0X_GETPARAMETERFIELD(Dev, field, variable) \
|
||||
variable = PALDevDataGet(Dev, CurrentParameters).field
|
||||
|
||||
|
||||
#define VL53L0X_SETARRAYPARAMETERFIELD(Dev, field, index, value) \
|
||||
PALDevDataSet(Dev, CurrentParameters.field[index], value)
|
||||
|
||||
#define VL53L0X_GETARRAYPARAMETERFIELD(Dev, field, index, variable) \
|
||||
variable = PALDevDataGet(Dev, CurrentParameters).field[index]
|
||||
|
||||
|
||||
#define VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, field, value) \
|
||||
PALDevDataSet(Dev, DeviceSpecificParameters.field, value)
|
||||
|
||||
#define VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, field) \
|
||||
PALDevDataGet(Dev, DeviceSpecificParameters).field
|
||||
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT97(Value) \
|
||||
(uint16_t)((Value>>9)&0xFFFF)
|
||||
#define VL53L0X_FIXPOINT97TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<9)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT88(Value) \
|
||||
(uint16_t)((Value>>8)&0xFFFF)
|
||||
#define VL53L0X_FIXPOINT88TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<8)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT412(Value) \
|
||||
(uint16_t)((Value>>4)&0xFFFF)
|
||||
#define VL53L0X_FIXPOINT412TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<4)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT313(Value) \
|
||||
(uint16_t)((Value>>3)&0xFFFF)
|
||||
#define VL53L0X_FIXPOINT313TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<3)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT08(Value) \
|
||||
(uint8_t)((Value>>8)&0x00FF)
|
||||
#define VL53L0X_FIXPOINT08TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<8)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT53(Value) \
|
||||
(uint8_t)((Value>>13)&0x00FF)
|
||||
#define VL53L0X_FIXPOINT53TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<13)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT102(Value) \
|
||||
(uint16_t)((Value>>14)&0x0FFF)
|
||||
#define VL53L0X_FIXPOINT102TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<12)
|
||||
|
||||
#define VL53L0X_MAKEUINT16(lsb, msb) (uint16_t)((((uint16_t)msb)<<8) + \
|
||||
(uint16_t)lsb)
|
||||
|
||||
/** @} VL53L0X_define_GeneralMacro_group */
|
||||
|
||||
/** @} VL53L0X_globaldefine_group */
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif /* _VL53L0X_DEF_H_ */
|
||||
@@ -1,257 +0,0 @@
|
||||
/*******************************************************************************
|
||||
Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
/**
|
||||
* Device specific defines. To be adapted by implementer for the targeted
|
||||
* device.
|
||||
*/
|
||||
|
||||
#ifndef _VL53L0X_DEVICE_H_
|
||||
#define _VL53L0X_DEVICE_H_
|
||||
|
||||
#include "vl53l0x_types.h"
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_DevSpecDefines_group VL53L0X cut1.1 Device Specific Defines
|
||||
* @brief VL53L0X cut1.1 Device Specific Defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_DeviceError_group Device Error
|
||||
* @brief Device Error code
|
||||
*
|
||||
* This enum is Device specific it should be updated in the implementation
|
||||
* Use @a VL53L0X_GetStatusErrorString() to get the string.
|
||||
* It is related to Status Register of the Device.
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_DeviceError;
|
||||
|
||||
#define VL53L0X_DEVICEERROR_NONE ((VL53L0X_DeviceError) 0)
|
||||
/*!< 0 NoError */
|
||||
#define VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ((VL53L0X_DeviceError) 1)
|
||||
#define VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ((VL53L0X_DeviceError) 2)
|
||||
#define VL53L0X_DEVICEERROR_NOVHVVALUEFOUND ((VL53L0X_DeviceError) 3)
|
||||
#define VL53L0X_DEVICEERROR_MSRCNOTARGET ((VL53L0X_DeviceError) 4)
|
||||
#define VL53L0X_DEVICEERROR_SNRCHECK ((VL53L0X_DeviceError) 5)
|
||||
#define VL53L0X_DEVICEERROR_RANGEPHASECHECK ((VL53L0X_DeviceError) 6)
|
||||
#define VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK ((VL53L0X_DeviceError) 7)
|
||||
#define VL53L0X_DEVICEERROR_TCC ((VL53L0X_DeviceError) 8)
|
||||
#define VL53L0X_DEVICEERROR_PHASECONSISTENCY ((VL53L0X_DeviceError) 9)
|
||||
#define VL53L0X_DEVICEERROR_MINCLIP ((VL53L0X_DeviceError) 10)
|
||||
#define VL53L0X_DEVICEERROR_RANGECOMPLETE ((VL53L0X_DeviceError) 11)
|
||||
#define VL53L0X_DEVICEERROR_ALGOUNDERFLOW ((VL53L0X_DeviceError) 12)
|
||||
#define VL53L0X_DEVICEERROR_ALGOOVERFLOW ((VL53L0X_DeviceError) 13)
|
||||
#define VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD ((VL53L0X_DeviceError) 14)
|
||||
|
||||
/** @} end of VL53L0X_DeviceError_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_CheckEnable_group Check Enable list
|
||||
* @brief Check Enable code
|
||||
*
|
||||
* Define used to specify the LimitCheckId.
|
||||
* Use @a VL53L0X_GetLimitCheckInfo() to get the string.
|
||||
* @{
|
||||
*/
|
||||
|
||||
#define VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE 0
|
||||
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE 1
|
||||
#define VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP 2
|
||||
#define VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD 3
|
||||
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC 4
|
||||
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE 5
|
||||
|
||||
#define VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS 6
|
||||
|
||||
/** @} end of VL53L0X_CheckEnable_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_GpioFunctionality_group Gpio Functionality
|
||||
* @brief Defines the different functionalities for the device GPIO(s)
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_GpioFunctionality;
|
||||
|
||||
#define VL53L0X_GPIOFUNCTIONALITY_OFF \
|
||||
((VL53L0X_GpioFunctionality) 0) /*!< NO Interrupt */
|
||||
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW \
|
||||
((VL53L0X_GpioFunctionality) 1) /*!< Level Low (value < thresh_low) */
|
||||
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH \
|
||||
((VL53L0X_GpioFunctionality) 2) /*!< Level High (value > thresh_high) */
|
||||
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT \
|
||||
((VL53L0X_GpioFunctionality) 3)
|
||||
/*!< Out Of Window (value < thresh_low OR value > thresh_high) */
|
||||
#define VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY \
|
||||
((VL53L0X_GpioFunctionality) 4) /*!< New Sample Ready */
|
||||
|
||||
/** @} end of VL53L0X_GpioFunctionality_group */
|
||||
|
||||
|
||||
/* Device register map */
|
||||
|
||||
/** @defgroup VL53L0X_DefineRegisters_group Define Registers
|
||||
* @brief List of all the defined registers
|
||||
* @{
|
||||
*/
|
||||
#define VL53L0X_REG_SYSRANGE_START 0x000
|
||||
/** mask existing bit in #VL53L0X_REG_SYSRANGE_START*/
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_MASK 0x0F
|
||||
/** bit 0 in #VL53L0X_REG_SYSRANGE_START write 1 toggle state in
|
||||
* continuous mode and arm next shot in single shot mode */
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_START_STOP 0x01
|
||||
/** bit 1 write 0 in #VL53L0X_REG_SYSRANGE_START set single shot mode */
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT 0x00
|
||||
/** bit 1 write 1 in #VL53L0X_REG_SYSRANGE_START set back-to-back
|
||||
* operation mode */
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK 0x02
|
||||
/** bit 2 write 1 in #VL53L0X_REG_SYSRANGE_START set timed operation
|
||||
* mode */
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_TIMED 0x04
|
||||
/** bit 3 write 1 in #VL53L0X_REG_SYSRANGE_START set histogram operation
|
||||
* mode */
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_HISTOGRAM 0x08
|
||||
|
||||
|
||||
#define VL53L0X_REG_SYSTEM_THRESH_HIGH 0x000C
|
||||
#define VL53L0X_REG_SYSTEM_THRESH_LOW 0x000E
|
||||
|
||||
|
||||
#define VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG 0x0001
|
||||
#define VL53L0X_REG_SYSTEM_RANGE_CONFIG 0x0009
|
||||
#define VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD 0x0004
|
||||
|
||||
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO 0x000A
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_DISABLED 0x00
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_LOW 0x01
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_HIGH 0x02
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_OUT_OF_WINDOW 0x03
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY 0x04
|
||||
|
||||
#define VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH 0x0084
|
||||
|
||||
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR 0x000B
|
||||
|
||||
/* Result registers */
|
||||
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS 0x0013
|
||||
#define VL53L0X_REG_RESULT_RANGE_STATUS 0x0014
|
||||
|
||||
#define VL53L0X_REG_RESULT_CORE_PAGE 1
|
||||
#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN 0x00BC
|
||||
#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_RTN 0x00C0
|
||||
#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF 0x00D0
|
||||
#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_REF 0x00D4
|
||||
#define VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF 0x00B6
|
||||
|
||||
/* Algo register */
|
||||
|
||||
#define VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM 0x0028
|
||||
|
||||
#define VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS 0x008a
|
||||
|
||||
/* Check Limit registers */
|
||||
#define VL53L0X_REG_MSRC_CONFIG_CONTROL 0x0060
|
||||
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_MIN_SNR 0X0027
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW 0x0056
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH 0x0057
|
||||
#define VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT 0x0064
|
||||
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_SNR 0X0067
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW 0x0047
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH 0x0048
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT 0x0044
|
||||
|
||||
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_HI 0X0061
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_LO 0X0062
|
||||
|
||||
/* PRE RANGE registers */
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x0050
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0051
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0052
|
||||
|
||||
#define VL53L0X_REG_SYSTEM_HISTOGRAM_BIN 0x0081
|
||||
#define VL53L0X_REG_HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT 0x0033
|
||||
#define VL53L0X_REG_HISTOGRAM_CONFIG_READOUT_CTRL 0x0055
|
||||
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x0070
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0071
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0072
|
||||
#define VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS 0x0020
|
||||
|
||||
#define VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP 0x0046
|
||||
|
||||
|
||||
#define VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N 0x00bf
|
||||
#define VL53L0X_REG_IDENTIFICATION_MODEL_ID 0x00c0
|
||||
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID 0x00c2
|
||||
|
||||
#define VL53L0X_REG_OSC_CALIBRATE_VAL 0x00f8
|
||||
|
||||
|
||||
#define VL53L0X_SIGMA_ESTIMATE_MAX_VALUE 65535
|
||||
/* equivalent to a range sigma of 655.35mm */
|
||||
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH 0x032
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 0x0B0
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_1 0x0B1
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_2 0x0B2
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_3 0x0B3
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_4 0x0B4
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_5 0x0B5
|
||||
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT 0xB6
|
||||
#define VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD 0x4E /* 0x14E */
|
||||
#define VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET 0x4F /* 0x14F */
|
||||
#define VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE 0x80
|
||||
|
||||
/*
|
||||
* Speed of light in um per 1E-10 Seconds
|
||||
*/
|
||||
|
||||
#define VL53L0X_SPEED_OF_LIGHT_IN_AIR 2997
|
||||
|
||||
#define VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV 0x0089
|
||||
|
||||
#define VL53L0X_REG_ALGO_PHASECAL_LIM 0x0030 /* 0x130 */
|
||||
#define VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT 0x0030
|
||||
|
||||
/** @} VL53L0X_DefineRegisters_group */
|
||||
|
||||
/** @} VL53L0X_DevSpecDefines_group */
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
/* _VL53L0X_DEVICE_H_ */
|
||||
|
||||
|
||||
@@ -1,101 +0,0 @@
|
||||
|
||||
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include "vl53l0x_def.h"
|
||||
|
||||
//******************************** IOCTL definitions
|
||||
#define VL53L0X_IOCTL_INIT _IO('p', 0x01)
|
||||
#define VL53L0X_IOCTL_XTALKCALB _IOW('p', 0x02, unsigned int)
|
||||
#define VL53L0X_IOCTL_OFFCALB _IOW('p', 0x03, unsigned int)
|
||||
#define VL53L0X_IOCTL_STOP _IO('p', 0x05)
|
||||
#define VL53L0X_IOCTL_SETXTALK _IOW('p', 0x06, unsigned int)
|
||||
#define VL53L0X_IOCTL_SETOFFSET _IOW('p', 0x07, int8_t)
|
||||
#define VL53L0X_IOCTL_GETDATAS _IOR('p', 0x0b, VL53L0X_RangingMeasurementData_t)
|
||||
#define VL53L0X_IOCTL_PARAMETER _IOWR('p', 0x0d, struct stmvl53l0x_parameter)
|
||||
typedef enum {
|
||||
OFFSET_PAR = 0,
|
||||
XTALKRATE_PAR = 1,
|
||||
XTALKENABLE_PAR = 2,
|
||||
GPIOFUNC_PAR = 3,
|
||||
LOWTHRESH_PAR = 4,
|
||||
HIGHTHRESH_PAR = 5,
|
||||
DEVICEMODE_PAR = 6,
|
||||
INTERMEASUREMENT_PAR = 7,
|
||||
} parameter_name_e;
|
||||
/*
|
||||
* IOCTL parameter structs
|
||||
*/
|
||||
struct stmvl53l0x_parameter {
|
||||
uint32_t is_read; //1: Get 0: Set
|
||||
parameter_name_e name;
|
||||
int32_t value;
|
||||
int32_t value2;
|
||||
int32_t status;
|
||||
};
|
||||
|
||||
static void help(void)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"Usage: vl53l0x_parameter Parameter [Value]\n"
|
||||
" Parameter as <0: OFFSET, 1: XTALKRATE, 2: XTAKEENABLE, 3: GPIOFUNC, \n"
|
||||
" 4: LOW_THRESHOLD, 5: HIGH_THRESHOLD,\n"
|
||||
" 6: DEVICE_MODE, 7: INTERMEASUREMENT_MS\n"
|
||||
" 8: REFERENCESPADS_PAR, 9: REFCALIBRATION_PAR\n"
|
||||
" Value is optional for writting value to requested Parameter\n"
|
||||
);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
int fd;
|
||||
struct stmvl53l0x_parameter parameter;
|
||||
char *end;
|
||||
|
||||
if (argc < 2) {
|
||||
help();
|
||||
exit(1);
|
||||
}
|
||||
|
||||
fd = open("/dev/stmvl53l0x_ranging",O_RDWR );
|
||||
if (fd <= 0) {
|
||||
fprintf(stderr,"Error open stmvl53l0x_ranging device: %s\n", strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
|
||||
parameter.name = strtoul(argv[1], &end, 10);
|
||||
if (*end) {
|
||||
help();
|
||||
exit(1);
|
||||
}
|
||||
if (argc == 2) {
|
||||
parameter.is_read = 1;
|
||||
fprintf(stderr, "To get VL53L0 parameter index:0x%x\n",
|
||||
parameter.name);
|
||||
|
||||
} else {
|
||||
parameter.is_read = 0;
|
||||
parameter.value = strtoul(argv[2],&end,10);
|
||||
if (*end) {
|
||||
help();
|
||||
exit(1);
|
||||
}
|
||||
fprintf(stderr, "To set VL53L0 parameter index:0x%x, as value:%d\n",
|
||||
parameter.name, parameter.value);
|
||||
}
|
||||
// to access requested register
|
||||
ioctl(fd, VL53L0X_IOCTL_PARAMETER,¶meter);
|
||||
fprintf(stderr," VL53L0 parameter value:%d, error status:%d\n",
|
||||
parameter.value, parameter.status);
|
||||
|
||||
//close(fd);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1,88 +0,0 @@
|
||||
|
||||
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "vl53l0x_types.h"
|
||||
|
||||
/*
|
||||
* IOCTL register data structs
|
||||
*/
|
||||
struct stmvl53l0x_register {
|
||||
uint32_t is_read; //1: read 0: write
|
||||
uint32_t reg_index;
|
||||
uint32_t reg_bytes;
|
||||
uint32_t reg_data;
|
||||
int32_t status;
|
||||
};
|
||||
//******************************** IOCTL definitions
|
||||
#define VL53L0X_IOCTL_REGISTER _IOWR('p', 0x0c, struct stmvl53l0x_register)
|
||||
|
||||
static void help(void)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"Usage: vl53l0x_reg REG_ADDR REG_BYTES [REG_DATA]\n"
|
||||
" REG_ADDR is VL6180's register address using 0xxx format: \n"
|
||||
" REG_BYTES is the register bytes(1, 2 or 4) \n"
|
||||
" REG_DATA is optional for writting data to requested REG_ADDR\n"
|
||||
);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
int fd;
|
||||
struct stmvl53l0x_register reg;
|
||||
char *end;
|
||||
|
||||
if (argc < 3) {
|
||||
help();
|
||||
exit(1);
|
||||
}
|
||||
|
||||
fd = open("/dev/stmvl53l0x_ranging",O_RDWR );
|
||||
if (fd <= 0) {
|
||||
fprintf(stderr,"Error open stmvl53l0x_ranging device: %s\n", strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
|
||||
reg.reg_index = strtoul(argv[1], &end, 16);
|
||||
if (*end) {
|
||||
help();
|
||||
exit(1);
|
||||
}
|
||||
reg.reg_bytes = strtoul(argv[2],&end,10);
|
||||
if (*end) {
|
||||
help();
|
||||
exit(1);
|
||||
}
|
||||
if (argc == 3) {
|
||||
reg.is_read = 1;
|
||||
fprintf(stderr, "To read VL53L0 register index:0x%x, bytes:%d\n",
|
||||
reg.reg_index, reg.reg_bytes);
|
||||
|
||||
} else {
|
||||
reg.is_read = 0;
|
||||
reg.reg_data = strtoul(argv[3],&end,16);
|
||||
if (*end) {
|
||||
help();
|
||||
exit(1);
|
||||
}
|
||||
fprintf(stderr, "To write VL53L0 register index:0x%x, bytes:%d as value:0x%x\n",
|
||||
reg.reg_index, reg.reg_bytes, reg.reg_data);
|
||||
}
|
||||
// to access requested register
|
||||
ioctl(fd, VL53L0X_IOCTL_REGISTER,®);
|
||||
fprintf(stderr," VL53L0 register data:0x%x, error status:%d\n",
|
||||
reg.reg_data, reg.status);
|
||||
|
||||
//close(fd);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1,389 +0,0 @@
|
||||
|
||||
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <signal.h>
|
||||
#include "vl53l0x_def.h"
|
||||
|
||||
#define MODE_RANGE 0
|
||||
#define MODE_XTAKCALIB 1
|
||||
#define MODE_OFFCALIB 2
|
||||
#define MODE_HELP 3
|
||||
#define MODE_PARAMETER 6
|
||||
|
||||
|
||||
//******************************** IOCTL definitions
|
||||
#define VL53L0X_IOCTL_INIT _IO('p', 0x01)
|
||||
#define VL53L0X_IOCTL_XTALKCALB _IOW('p', 0x02, unsigned int)
|
||||
#define VL53L0X_IOCTL_OFFCALB _IOW('p', 0x03, unsigned int)
|
||||
#define VL53L0X_IOCTL_STOP _IO('p', 0x05)
|
||||
#define VL53L0X_IOCTL_SETXTALK _IOW('p', 0x06, unsigned int)
|
||||
#define VL53L0X_IOCTL_SETOFFSET _IOW('p', 0x07, int8_t)
|
||||
#define VL53L0X_IOCTL_GETDATAS _IOR('p', 0x0b, VL53L0X_RangingMeasurementData_t)
|
||||
#define VL53L0X_IOCTL_PARAMETER _IOWR('p', 0x0d, struct stmvl53l0x_parameter)
|
||||
|
||||
//modify the following macro accoring to testing set up
|
||||
#define OFFSET_TARGET 100//200
|
||||
#define XTALK_TARGET 600//400
|
||||
#define NUM_SAMPLES 20//20
|
||||
|
||||
typedef enum {
|
||||
OFFSET_PAR = 0,
|
||||
XTALKRATE_PAR = 1,
|
||||
XTALKENABLE_PAR = 2,
|
||||
GPIOFUNC_PAR = 3,
|
||||
LOWTHRESH_PAR = 4,
|
||||
HIGHTHRESH_PAR = 5,
|
||||
DEVICEMODE_PAR = 6,
|
||||
INTERMEASUREMENT_PAR = 7,
|
||||
REFERENCESPADS_PAR = 8,
|
||||
REFCALIBRATION_PAR = 9,
|
||||
} parameter_name_e;
|
||||
/*
|
||||
* IOCTL parameter structs
|
||||
*/
|
||||
struct stmvl53l0x_parameter {
|
||||
uint32_t is_read; //1: Get 0: Set
|
||||
parameter_name_e name;
|
||||
int32_t value;
|
||||
int32_t value2;
|
||||
int32_t status;
|
||||
};
|
||||
|
||||
static void help(void)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"Usage: vl53l0x_test [-c] [-h]\n"
|
||||
" -h for usage\n"
|
||||
" -c for crosstalk calibration\n"
|
||||
" -o for offset calibration\n"
|
||||
" -O <low> <high> for testing interrupt threshold (out mode)\n"
|
||||
" -L <low> for testing interrupt threshold (low mode)\n"
|
||||
" -H <high> for testing interrupt threshold (high mode)\n"
|
||||
" default for ranging\n"
|
||||
);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
static int loop_break=0;
|
||||
|
||||
void sig_handler(int signum)
|
||||
{
|
||||
if(signum == SIGINT)
|
||||
{
|
||||
loop_break=1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
int fd;
|
||||
unsigned long data;
|
||||
VL53L0X_RangingMeasurementData_t range_datas;
|
||||
struct stmvl53l0x_parameter parameter;
|
||||
int flags = 0;
|
||||
int mode = MODE_RANGE;
|
||||
unsigned int targetDistance=0;
|
||||
int i = 0;
|
||||
int rc = 0;
|
||||
unsigned int low_threshold = 0, high_threshold = 0;
|
||||
int configure_int_thresholds = 0;
|
||||
int gpio_functionnality_threshold = 0;
|
||||
|
||||
|
||||
if (signal(SIGINT, sig_handler) == SIG_ERR)
|
||||
fprintf(stderr, "Can't catch SIGINT");
|
||||
|
||||
|
||||
/* handle (optional) flags first */
|
||||
while (1+flags < argc && argv[1+flags][0] == '-') {
|
||||
switch (argv[1+flags][1]) {
|
||||
case 'c': mode= MODE_XTAKCALIB; break;
|
||||
case 'h': mode= MODE_HELP; break;
|
||||
case 'o': mode = MODE_OFFCALIB; break;
|
||||
case 'O':
|
||||
if (argc < 4) {
|
||||
fprintf(stderr, "USAGE: %s <low_threshold_value> <high_threshold_value>\n", argv[0]);
|
||||
exit(1);
|
||||
}
|
||||
low_threshold = atoi(argv[2]);
|
||||
high_threshold = atoi(argv[3]);
|
||||
configure_int_thresholds = 1;
|
||||
|
||||
fprintf(stderr, "Configuring Low threshold = %u, High threhold = %u\n", low_threshold, high_threshold);
|
||||
break;
|
||||
case 'L':
|
||||
if (argc < 3) {
|
||||
fprintf(stderr, "USAGE: %s <low_threshold_value> \n", argv[0]);
|
||||
exit(1);
|
||||
}
|
||||
low_threshold = atoi(argv[2]);
|
||||
configure_int_thresholds = 2;
|
||||
|
||||
fprintf(stderr, "Configuring Low threshold = %u, High threhold = %u\n", low_threshold, high_threshold);
|
||||
break;
|
||||
case 'H':
|
||||
if (argc < 3) {
|
||||
fprintf(stderr, "USAGE: %s <high_threshold_value>\n", argv[0]);
|
||||
exit(1);
|
||||
}
|
||||
high_threshold = atoi(argv[2]);
|
||||
configure_int_thresholds = 3;
|
||||
|
||||
fprintf(stderr, "Configuring Low threshold = %u, High threhold = %u\n", low_threshold, high_threshold);
|
||||
break;
|
||||
default:
|
||||
fprintf(stderr, "Error: Unsupported option "
|
||||
"\"%s\"!\n", argv[1+flags]);
|
||||
help();
|
||||
exit(1);
|
||||
}
|
||||
flags++;
|
||||
}
|
||||
if (mode == MODE_HELP)
|
||||
{
|
||||
help();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
fd = open("/dev/stmvl53l0x_ranging",O_RDWR | O_SYNC);
|
||||
if (fd <= 0)
|
||||
{
|
||||
fprintf(stderr,"Error open stmvl53l0x_ranging device: %s\n", strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
//make sure it's not started
|
||||
if (ioctl(fd, VL53L0X_IOCTL_STOP , NULL) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_STOP : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
if (mode == MODE_XTAKCALIB)
|
||||
{
|
||||
unsigned int XtalkInt = 0;
|
||||
uint8_t XtalkEnable = 0;
|
||||
fprintf(stderr, "xtalk Calibrate place black target at %dmm from glass===\n",XTALK_TARGET);
|
||||
// to xtalk calibration
|
||||
targetDistance = XTALK_TARGET;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_XTALKCALB , &targetDistance) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_XTALKCALB : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
// to get xtalk parameter
|
||||
parameter.is_read = 1;
|
||||
parameter.name = XTALKRATE_PAR;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER , ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
XtalkInt = (unsigned int)parameter.value;
|
||||
parameter.name = XTALKENABLE_PAR;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER , ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
XtalkEnable = (uint8_t)parameter.value;
|
||||
fprintf(stderr, "VL53L0 Xtalk Calibration get Xtalk Compensation rate in fixed 16 point as %u, enable:%u\n",XtalkInt,XtalkEnable);
|
||||
|
||||
for(i = 0; i <= NUM_SAMPLES; i++)
|
||||
{
|
||||
usleep(30 *1000); /*100ms*/
|
||||
// to get all range data
|
||||
ioctl(fd, VL53L0X_IOCTL_GETDATAS,&range_datas);
|
||||
}
|
||||
fprintf(stderr," VL53L0 DMAX calibration Range Data:%d, signalRate_mcps:%d\n",range_datas.RangeMilliMeter, range_datas.SignalRateRtnMegaCps);
|
||||
// get rangedata of last measurement to avoid incorrect datum from unempty buffer
|
||||
//to close
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
else if (mode == MODE_OFFCALIB)
|
||||
{
|
||||
int offset=0;
|
||||
uint32_t SpadCount=0;
|
||||
uint8_t IsApertureSpads=0;
|
||||
uint8_t VhvSettings=0,PhaseCal=0;
|
||||
|
||||
|
||||
// to xtalk calibration
|
||||
targetDistance = OFFSET_TARGET;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_OFFCALB , &targetDistance) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_OFFCALB : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
// to get current offset
|
||||
parameter.is_read = 1;
|
||||
parameter.name = OFFSET_PAR;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
offset = (int)parameter.value;
|
||||
fprintf(stderr, "get offset %d micrometer===\n",offset);
|
||||
|
||||
parameter.name = REFCALIBRATION_PAR;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
VhvSettings = (uint8_t) parameter.value;
|
||||
PhaseCal=(uint8_t) parameter.value2;
|
||||
fprintf(stderr, "get VhvSettings is %u ===\nget PhaseCas is %u ===\n", VhvSettings,PhaseCal);
|
||||
|
||||
parameter.name =REFERENCESPADS_PAR;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
SpadCount = (uint32_t)parameter.value;
|
||||
IsApertureSpads=(uint8_t) parameter.value2;
|
||||
fprintf(stderr, "get SpadCount is %d ===\nget IsApertureSpads is %u ===\n", SpadCount,IsApertureSpads);
|
||||
|
||||
|
||||
//to close
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(configure_int_thresholds)
|
||||
{
|
||||
case 1:
|
||||
gpio_functionnality_threshold = VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT;
|
||||
break;
|
||||
case 2:
|
||||
gpio_functionnality_threshold = VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW;
|
||||
break;
|
||||
case 3:
|
||||
gpio_functionnality_threshold = VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH;
|
||||
break;
|
||||
default:
|
||||
gpio_functionnality_threshold = VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY;
|
||||
}
|
||||
|
||||
if (configure_int_thresholds) {
|
||||
|
||||
parameter.is_read = 0;
|
||||
|
||||
|
||||
parameter.name = DEVICEMODE_PAR;
|
||||
parameter.value = VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING;
|
||||
//parameter.value = VL53L0X_DEVICEMODE_CONTINUOUS_RANGING;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER(CONTINOUS_TIMED_RANGING) : %s\n",
|
||||
strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
parameter.name = GPIOFUNC_PAR;
|
||||
parameter.value = gpio_functionnality_threshold;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s, low_threshold = %u\n",
|
||||
strerror(errno),
|
||||
low_threshold);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (configure_int_thresholds != 3)
|
||||
{
|
||||
parameter.name = LOWTHRESH_PAR;
|
||||
parameter.value = low_threshold;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s, low_threshold = %u\n",
|
||||
strerror(errno),
|
||||
low_threshold);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (configure_int_thresholds != 2)
|
||||
{
|
||||
parameter.name = HIGHTHRESH_PAR;
|
||||
parameter.value = high_threshold;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s, high_threshold = %u\n",
|
||||
strerror(errno),
|
||||
high_threshold);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else {
|
||||
parameter.name = DEVICEMODE_PAR;
|
||||
parameter.value = VL53L0X_DEVICEMODE_CONTINUOUS_RANGING;
|
||||
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER(CONTINUOUS_RANGING) : %s\n",
|
||||
strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
parameter.name = GPIOFUNC_PAR;
|
||||
parameter.value = gpio_functionnality_threshold;
|
||||
if (ioctl(fd, VL53L0X_IOCTL_PARAMETER, ¶meter) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_PARAMETER : %s, low_threshold = %u\n",
|
||||
strerror(errno),
|
||||
low_threshold);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
}
|
||||
// to init
|
||||
if (ioctl(fd, VL53L0X_IOCTL_INIT , NULL) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_INIT : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
// get data testing
|
||||
while (1)
|
||||
{
|
||||
usleep(30 *1000); /*100ms*/
|
||||
// to get all range data
|
||||
rc = ioctl(fd, VL53L0X_IOCTL_GETDATAS,&range_datas);
|
||||
if (rc) {
|
||||
fprintf(stderr, "VL53L0X_IOCTL_GETDATAS failed = %d\n", rc);
|
||||
close(fd);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
fprintf(stderr,"Range:%4d, Error:%u, SigRate_mcps:%7d, AmbRate_mcps:%7d\r",
|
||||
range_datas.RangeMilliMeter, range_datas.RangeStatus, range_datas.SignalRateRtnMegaCps, range_datas.AmbientRateRtnMegaCps);
|
||||
if(loop_break)
|
||||
break;
|
||||
}
|
||||
|
||||
fprintf(stderr, "\n");
|
||||
|
||||
fprintf(stderr, "Stop driver\n");
|
||||
|
||||
if (ioctl(fd, VL53L0X_IOCTL_STOP , NULL) < 0) {
|
||||
fprintf(stderr, "Error: Could not perform VL53L0X_IOCTL_STOP : %s\n", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
close(fd);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
Binary file not shown.
@@ -1,69 +0,0 @@
|
||||
/*******************************************************************************
|
||||
Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
#ifndef VL53L0X_TYPES_H_
|
||||
#define VL53L0X_TYPES_H_
|
||||
|
||||
#include <linux/types.h>
|
||||
|
||||
#ifndef NULL
|
||||
#error "TODO review NULL definition or add required include "
|
||||
#define NULL 0
|
||||
#endif
|
||||
/** use where fractional values are expected
|
||||
*
|
||||
* Given a floating point value f it's .16 bit point is (int)(f*(1<<16))*/
|
||||
typedef unsigned int FixPoint1616_t;
|
||||
|
||||
#if !defined(STDINT_H) && !defined(_GCC_STDINT_H) \
|
||||
&& !defined(_STDINT_H) && !defined(_LINUX_TYPES_H)
|
||||
|
||||
#pragma message("Please review type definition of STDINT define for your \
|
||||
platform and add to list above ")
|
||||
|
||||
/*
|
||||
* target platform do not provide stdint or use a different #define than above
|
||||
* to avoid seeing the message below addapt the #define list above or implement
|
||||
* all type and delete these pragma
|
||||
*/
|
||||
|
||||
typedef unsigned int uint32_t;
|
||||
typedef int int32_t;
|
||||
|
||||
typedef unsigned short uint16_t;
|
||||
typedef short int16_t;
|
||||
|
||||
typedef unsigned char uint8_t;
|
||||
|
||||
typedef signed char int8_t;
|
||||
|
||||
#else
|
||||
#include <stdint.h>
|
||||
#endif /* _STDINT_H */
|
||||
|
||||
#endif /* VL53L0X_TYPES_H_ */
|
||||
@@ -1,5 +0,0 @@
|
||||
# demo2
|
||||
add_executable(wonderEcho_demo wonderEcho_demo.cpp)
|
||||
|
||||
# 链接 OpenCV 库
|
||||
target_link_libraries(wonderEcho_demo)
|
||||
@@ -1,157 +0,0 @@
|
||||
/*
|
||||
* @Author: Ilikara 3435193369@qq.com
|
||||
* @Date: 2025-02-11 15:23:11
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-03-10 13:06:56
|
||||
* @FilePath: /2k300_smartcar/wonderEcho_demo/wonderEcho_demo.cpp
|
||||
* @Description:
|
||||
*
|
||||
* Copyright (c) 2025 by ${git_name_email}, All Rights Reserved.
|
||||
*/
|
||||
#include <iostream>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/i2c-dev.h>
|
||||
#include <linux/i2c.h>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
|
||||
std::mutex i2c_mutex; // 互斥锁，用于保护I2C设备访问
|
||||
std::atomic<bool> running{true}; // 控制线程运行状态
|
||||
|
||||
int i2c_read_byte_data(int file, __u8 reg)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
struct i2c_smbus_ioctl_data args;
|
||||
|
||||
args.read_write = I2C_SMBUS_READ;
|
||||
args.command = reg;
|
||||
args.size = I2C_SMBUS_BYTE_DATA;
|
||||
args.data = &data;
|
||||
|
||||
if (ioctl(file, I2C_SMBUS, &args) == -1)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
return data.byte & 0xFF;
|
||||
}
|
||||
|
||||
int i2c_write_i2c_block_data(int file, __u8 reg, __u8 length, const __u8 *values)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
struct i2c_smbus_ioctl_data args;
|
||||
|
||||
if (length > I2C_SMBUS_BLOCK_MAX)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
for (int i = 0; i < length; i++)
|
||||
{
|
||||
data.block[i + 1] = values[i];
|
||||
}
|
||||
data.block[0] = length;
|
||||
|
||||
args.read_write = I2C_SMBUS_WRITE;
|
||||
args.command = reg;
|
||||
args.size = I2C_SMBUS_I2C_BLOCK_DATA;
|
||||
args.data = &data;
|
||||
|
||||
if (ioctl(file, I2C_SMBUS, &args) == -1)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 轮询线程函数
|
||||
void poll_i2c_device(int file)
|
||||
{
|
||||
while (running)
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(i2c_mutex); // 加锁
|
||||
__u8 reg = 0x64;
|
||||
__u8 value = i2c_read_byte_data(file, reg);
|
||||
|
||||
if (value != 0x00)
|
||||
{
|
||||
std::cout << "Value at 0x64 is not 0x00: " << std::hex << (int)value << std::endl;
|
||||
}
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // 1Hz轮询
|
||||
}
|
||||
}
|
||||
|
||||
// 用户输入线程函数
|
||||
void handle_user_input(int file)
|
||||
{
|
||||
while (running)
|
||||
{
|
||||
__u8 data[2];
|
||||
std::cout << "Enter two bytes to send to 0x6e (first byte must be 0x00 or 0xFF): ";
|
||||
std::cin >> std::hex >> (int &)data[0] >> (int &)data[1];
|
||||
|
||||
// 检查第一个字节是否为0x00或0xFF
|
||||
if (data[0] != 0x00 && data[0] != 0xFF)
|
||||
{
|
||||
std::cerr << "First byte must be 0x00 or 0xFF!" << std::endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(i2c_mutex); // 加锁
|
||||
__u8 reg = 0x6e;
|
||||
if (i2c_write_i2c_block_data(file, reg, sizeof(data), data) < 0)
|
||||
{
|
||||
std::cerr << "Failed to write to the i2c device" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "Data sent successfully: " << std::hex << (int)data[0] << " " << (int)data[1] << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
int file;
|
||||
const char *filename = "/dev/i2c-2"; // I2C总线设备文件
|
||||
int addr = 0x34; // I2C设备地址
|
||||
|
||||
// 打开I2C设备
|
||||
if ((file = open(filename, O_RDWR)) < 0)
|
||||
{
|
||||
std::cerr << "Failed to open the i2c bus" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// 设置I2C设备地址
|
||||
if (ioctl(file, I2C_SLAVE, addr) < 0)
|
||||
{
|
||||
std::cerr << "Failed to acquire bus access and/or talk to slave" << std::endl;
|
||||
close(file);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// 创建轮询线程和用户输入线程
|
||||
std::thread poll_thread(poll_i2c_device, file);
|
||||
std::thread input_thread(handle_user_input, file);
|
||||
|
||||
// 等待用户输入线程结束（按Ctrl+C退出）
|
||||
input_thread.join();
|
||||
|
||||
// 停止轮询线程
|
||||
running = false;
|
||||
poll_thread.join();
|
||||
|
||||
// 关闭I2C设备
|
||||
close(file);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,3 +0,0 @@
|
||||
# zebra_demo - 斑马线检测测试
|
||||
add_executable(zebra_demo zebra_demo.cpp)
|
||||
target_link_libraries(zebra_demo common_lib ${OpenCV_LIBS})
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 894 KiB |
@@ -1,729 +0,0 @@
|
||||
/*
|
||||
* 斑马线实时检测 — 龙芯2K0300 小车摄像头版 (含逆透视变换 IPM)
|
||||
* =================================================================
|
||||
*
|
||||
* 【设计目的】
|
||||
* 在车辆行驶过程中实时识别前方斑马线，将检测结果写入标志文件
|
||||
* 供巡线主控模块 (control.cpp) 读取，做出减速/停车等安全决策。
|
||||
*
|
||||
* 【算法来源】
|
||||
* 移植自 Python 开源项目: https://github.com/TomMao23/ZebraCrossing_Detection
|
||||
* 利用斑马线的四个视觉特征做传统图像处理检测:
|
||||
* (1) 梯度一致性 — 黑白交替 → 边缘方向高度集中
|
||||
* (2) 等间隔 — 线条等距分布 (当前未使用)
|
||||
* (3) 多根线 — 一个区域内有大量平行边缘
|
||||
* (4) 宽度大 — 斑马线宽度明显大于车道线
|
||||
*
|
||||
* 【处理流水线 (每帧)】
|
||||
*
|
||||
* 摄像头 320×240 (MJPG, /dev/video0)
|
||||
* ↓
|
||||
* ┌── [IPM] 逆透视变换 ──────────────────────────────┐
|
||||
* │ 前视图 → 鸟瞰图 (BEV, Bird's Eye View) │
|
||||
* │ cv::warpPerspective(frame, bev, iM, 640×640) │
|
||||
* │ 将倾斜的透视角度"拉平"，使斑马线恢复矩形形状 │
|
||||
* │ 矩阵 M 需针对摄像头安装角度/高度单独标定 │
|
||||
* │ 标定方法：拍摄地面矩形标定板 → 四点透视变换 │
|
||||
* └──────────────────────────────────────────────────┘
|
||||
* ↓ if USE_IPM
|
||||
* 640×640 BEV 图 (已校正透视畸变)
|
||||
* ↓ cv::resize
|
||||
* 400×400 正方形 BGR 图
|
||||
* ↓ extractChannel(0) → 只取 B 通道 (利用蓝色通道衰减黄色减速带)
|
||||
* medianBlur 5×5 → 去砖缝/椒盐噪声, 保留梯度大小
|
||||
* ↓
|
||||
* MORPH_OPEN 3×3 iter=4 → 多次腐蚀去窄车道线, 斑马线保留
|
||||
* ↓
|
||||
* MORPH_CLOSE 5×5 iter=3 → 先膨胀填斑马线缺损, 再腐蚀抑制路面箭头碎片
|
||||
* ↓
|
||||
* Canny(30, 90) → 双阈值边缘检测, 利用连通性合并弱边缘
|
||||
* ↓
|
||||
* Sobel 3×3 → 计算每个边缘像素的梯度模值 Amplitude 和方向 theta(0~90°)
|
||||
* ↓ 弱边缘过滤 (Amplitude < 30 → 置零)
|
||||
* 滑动窗口 100×302, 步长 50, 水平范围固定 [39, 341]
|
||||
* ↓
|
||||
* 每个窗口内:
|
||||
* - 统计有效梯度点 (Amplitude > 0, theta ∈ [0, 70°)) 的方向直方图
|
||||
* - 14 个 bin, 每 bin 5° (0°~5°, 5°~10°, ..., 65°~70°)
|
||||
* - 取峰值 bin 的点数
|
||||
* - 峰值 > 1500 → 判为斑马线
|
||||
* ↓
|
||||
* 所有阳性窗口的纵向跨度 = 最终斑马线位置 (紫色框)
|
||||
* ↓
|
||||
* LCD 叠加显示 + 写入 ./zebra_detected 文件
|
||||
*
|
||||
* 【为何需要逆透视变换 (IPM)】
|
||||
* 前视摄像头拍到的斑马线是透视畸变的 (近大远小、倾斜变形)
|
||||
* → 斑马线的平行特征被破坏 → 梯度一致性减弱 → 检测效果下降
|
||||
* IPM 将图像变换为鸟瞰视角 → 斑马线恢复矩形/平行特征
|
||||
* → Sobel 梯度方向集中在同一角度 → 直方图峰值更高 → 检测更准
|
||||
*
|
||||
* 【注意】IPM 矩阵必须针对实车摄像头标定！
|
||||
* 当前矩阵是 Python 项目原始标定值，在 2K0300 小车上可能不适用。
|
||||
* 建议先关闭 IPM 测试 (USE_IPM=0)，确认检测逻辑正确后，
|
||||
* 再用标定板重新标定并开启 IPM。
|
||||
*
|
||||
* 【为何方向限制在 0~70°】
|
||||
* 排除水平停止线干扰:
|
||||
* 停止线 = 横线 → Sobel dx 大 / dy → 0 → 方向接近 0°
|
||||
* 斑马线 = 纵线/斜线 → 方向偏大 (30°~90°)
|
||||
* 实际上 bin 0 也在统计范围但峰值不高, 主要靠阈值过滤
|
||||
*
|
||||
* 【为何窗口固定水平范围 [39, 341]】
|
||||
* 排除道路两侧的行人道/建筑/绿化等干扰
|
||||
* 400 像素宽, 窗口 302 宽, 居中左偏 39px, 右侧留 59px
|
||||
*
|
||||
* 【为何单独用蓝色通道而非灰度图】
|
||||
* BGR 三通道中:
|
||||
* B 通道 = 蓝色分量
|
||||
* 黄色减速带 ≈ R+G (无蓝色) → B 通道响应 ≈ 0 → 完全消除减速带边缘
|
||||
* 白色斑马线 ≈ R+G+B 均等 → B 通道正常响应 → 斑马线边缘保留
|
||||
* 如果用灰度 (0.299R + 0.587G + 0.114B), 黄色减速带会产生强边缘
|
||||
*
|
||||
* 【为何用中值滤波而非高斯/均值滤波】
|
||||
* 中值滤波最大程度保留原始梯度大小 (不模糊边缘), 同时完美消灭人行道砖缝纹理
|
||||
* 高斯/均值会平滑边缘 → 梯度幅度衰减 → 影响判定阈值
|
||||
*
|
||||
* 【为何先开运算再闭运算】
|
||||
* 开运算 (先 Erosion 后 Dilation):
|
||||
* iter=4 次连续腐蚀 → 窄车道线被彻底腐蚀消失
|
||||
* 后续膨胀 → 恢复剩余斑马线的尺寸
|
||||
* 等效: 保留 "宽度 > 某阈值" 的线条 (斑马线)，去除 "窄的" 线条 (车道线)
|
||||
* 闭运算 (先 Dilation 后 Erosion):
|
||||
* 先膨胀 → 填补斑马线内部的微小缺损/裂缝
|
||||
* 后腐蚀 → 恢复原始尺寸，同时消除箭头碎片的虚假边缘
|
||||
*
|
||||
* 【为何用 Canny 而非直接对 Sobel 结果判定】
|
||||
* (1) Canny 双阈值 + 连通性分析 → 孤立噪点被抑制, 只有真正连续的边缘通过
|
||||
* (2) Canny 使一条边缘只保留最细的 1px 脊线 → 斑马线 "多根线" 特征更突出
|
||||
* (直接 Sobel 一根粗边缘产生 N 个梯度点 → 降低了线数的区分度)
|
||||
* (3) Canny 两个阈值让调参范围更大, 比单梯度阈值更鲁棒
|
||||
*
|
||||
* 【2K0300 算力评估 (400×400 分辨率)】
|
||||
* IPM warpPerspective: O(640×640×常数) ≈ 2.5M
|
||||
* 蓝色通道提取: O(160K) ≈ 忽略
|
||||
* medianBlur 5×5: O(160K × 25) ≈ 4M
|
||||
* morphOpen ×4: O(160K × 9 × 8) ≈ 11.5M
|
||||
* morphClose ×3: O(160K × 25 × 6) ≈ 24M
|
||||
* Canny: O(160K × 常数) ≈ 几M
|
||||
* Sobel ×2: O(160K × 9 × 2) ≈ 2.9M
|
||||
* 梯度逐像素: O(160K × 10) ≈ 1.6M
|
||||
* 滑动窗口 (7窗): O(7 × 30200) ≈ 0.2M
|
||||
* ---------------------------------------------------
|
||||
* 总计约 50~55M 操作/帧, 2K0300 0.43 GOPS → 理论 7~10 FPS
|
||||
* 优化: 关闭 IPM 可节省 ~2.5M/帧; 增大 WIN_STEP 减少窗口数
|
||||
*
|
||||
* 用法:
|
||||
* ./zebra_demo 带 LCD 显示 + 检测 (含 IPM)
|
||||
* ./zebra_demo --no-display 无头模式 (只写状态文件, 省算力)
|
||||
* ./zebra_demo --no-ipm 关闭逆透视变换 (使用前视图)
|
||||
*
|
||||
* 跨模块通信:
|
||||
* 写入 ./zebra_detected: "1" = 检测到斑马线, "0" = 未检测到
|
||||
* 控制模块 (control.cpp) 可用 readFlag("./zebra_detected") 读取
|
||||
* 仅在状态变化时写入, 减少文件 I/O
|
||||
*
|
||||
* 退出: 按 Ctrl+C (SIGINT) 或 kill (SIGTERM) 优雅清理资源
|
||||
*/
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <csignal>
|
||||
#include <atomic>
|
||||
|
||||
// Linux 特定: 帧缓冲屏 + mmap + ioctl
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/fb.h>
|
||||
|
||||
// ============================================================
|
||||
// 可调参数 — 与 Python 版算法完全对齐
|
||||
// ============================================================
|
||||
|
||||
// --- 逆透视变换 (IPM) ---
|
||||
// 是否启用 IPM: 0=关闭(使用前视图), 1=开启(使用鸟瞰图)
|
||||
// 启用前必须用标定板重新标定矩阵 M, 否则效果可能比关闭更差!
|
||||
#define USE_IPM 1
|
||||
|
||||
// IPM 输出鸟瞰图尺寸 (与 Python 版 gd.py 一致)
|
||||
const int IPM_SIZE = 640;
|
||||
|
||||
// --- 滑动窗口 ---
|
||||
const int WIN_H = 100;
|
||||
const int WIN_W = 302;
|
||||
const int WIN_STEP = 50;
|
||||
const int WIN_COL_L = 39;
|
||||
const int WIN_COL_R = 341;
|
||||
|
||||
// --- 处理分辨率 ---
|
||||
const int PROC_SIZE = 400;
|
||||
|
||||
// --- 梯度过滤 ---
|
||||
const float GRAD_THRESHOLD = 30.0f;
|
||||
|
||||
// --- 方向直方图 ---
|
||||
const int HIST_BIN_SIZE = 5;
|
||||
const int HIST_ANGLE_MAX = 70;
|
||||
const int HIST_BIN_COUNT = 14;
|
||||
|
||||
// --- 判定阈值 ---
|
||||
const int ZEBRA_THRESHOLD = 1500;
|
||||
|
||||
// --- 摄像头 ---
|
||||
const int CAM_WIDTH = 320;
|
||||
const int CAM_HEIGHT = 240;
|
||||
|
||||
// ============================================================
|
||||
// IPM 逆透视变换矩阵
|
||||
//
|
||||
// Python 源码中的变换逻辑:
|
||||
// M = 原图 → BEV 的 3×3 透视变换矩阵
|
||||
// iM = M.inv() = BEV → 原图的逆矩阵
|
||||
//
|
||||
// xy = 640×640 BEV 坐标网格 (px, py)
|
||||
// ixy = perspectiveTransform(xy, iM) → 每个 BEV 像素在原图中的坐标
|
||||
// remap(frame, mapx, mapy) → 采样原图得到 BEV 图像
|
||||
//
|
||||
// C++ 等价实现:
|
||||
// iM = M.inv()
|
||||
// warpPerspective(frame, bev, iM, Size(640,640), INTER_LINEAR)
|
||||
//
|
||||
// warpPerspective 的矩阵含义是 dst→src 映射, 恰好就是 iM
|
||||
//
|
||||
// 【标定方法】
|
||||
// 1. 在小车前方地面放置一个已知尺寸的矩形标定板 (如 A4 纸或棋盘格)
|
||||
// 2. 拍摄一张包含整个矩形的前视图
|
||||
// 3. 获取矩形的 4 个角点在原图中的像素坐标 pts_src
|
||||
// 4. 在 BEV 中定义矩形应有的 4 个角点坐标 pts_dst
|
||||
// 5. M = cv::getPerspectiveTransform(pts_src, pts_dst)
|
||||
// 6. 将 M 填入下方矩阵中
|
||||
//
|
||||
// 当前矩阵是 Python 项目的原始标定值，需替换为实车标定值!
|
||||
// ============================================================
|
||||
|
||||
// Python 原始矩阵 M (前视图 → BEV), 来源: gd.py:255-257
|
||||
// 元素按 (row, col) = (0,0) (0,1) (0,2)
|
||||
// (1,0) (1,1) (1,2)
|
||||
// (2,0) (2,1) (2,2)
|
||||
static const double g_M_data[3][3] = {
|
||||
{-1.86073726e-01, -5.02678929e-01, 4.72322899e+02},
|
||||
{-1.39150388e-02, -1.50260445e+00, 1.00507430e+03},
|
||||
{-1.77785988e-05, -1.65517173e-03, 1.00000000e+00}
|
||||
};
|
||||
|
||||
/**
|
||||
* 计算逆矩阵 (BEV → 前视图), 用于 warpPerspective
|
||||
* 在启动时调用一次, 结果全局缓存
|
||||
*/
|
||||
static cv::Mat build_ipm_inverse_matrix()
|
||||
{
|
||||
cv::Mat M(3, 3, CV_64F);
|
||||
for (int r = 0; r < 3; ++r)
|
||||
for (int c = 0; c < 3; ++c)
|
||||
M.at<double>(r, c) = g_M_data[r][c];
|
||||
|
||||
// iM = M.inv(): BEV 坐标 → 原始图像坐标
|
||||
// warpPerspective 接受 dst→src 映射, 即 iM
|
||||
cv::Mat iM = M.inv();
|
||||
return iM;
|
||||
}
|
||||
|
||||
// 全局变量: 在 main() 中计算一次, 之后每帧复用
|
||||
static cv::Mat g_iM;
|
||||
|
||||
// ============================================================
|
||||
// 全局状态 — 优雅退出机制
|
||||
// ============================================================
|
||||
|
||||
static std::atomic<bool> g_running{true};
|
||||
|
||||
static void signal_handler(int)
|
||||
{
|
||||
g_running.store(false);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// apply_ipm — 逆透视变换: 前视图 → 鸟瞰图 (BEV)
|
||||
//
|
||||
// 输入: 原始摄像头帧 (320×240 或任意分辨率)
|
||||
// 预计算的逆矩阵 g_iM (BEV→原图)
|
||||
// 输出: 640×640 鸟瞰图 (消除透视畸变, 斑马线恢复矩形)
|
||||
//
|
||||
// 等价于 Python 版:
|
||||
// remap(frame, mapx, mapy, INTER_LINEAR) // 预计算映射表
|
||||
//
|
||||
// 为什么选 640×640 输出?
|
||||
// 1. BEV 鸟瞰需要比原图更大的空间来容纳"拉平"后的路面
|
||||
// 2. 640 是 2 的幂次方的近似值, 对 resize 和后续操作友好
|
||||
// 3. Python 版使用此尺寸, 算法参数基于此调优
|
||||
//
|
||||
// 如果不开启 IPM, 此函数会被跳过, 直接用前视图进入处理管线
|
||||
// ============================================================
|
||||
static cv::Mat apply_ipm(const cv::Mat &frame)
|
||||
{
|
||||
cv::Mat bev;
|
||||
cv::warpPerspective(frame, bev, g_iM,
|
||||
cv::Size(IPM_SIZE, IPM_SIZE),
|
||||
cv::INTER_LINEAR);
|
||||
// warpPerspective 使用 INTER_LINEAR 双线性插值,
|
||||
// 比 INTER_NEAREST 最近邻更平滑, 避免产生锯齿状伪边缘
|
||||
return bev;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// preprocess — 图像预处理 (蓝色通道 + 滤波 + 形态学)
|
||||
//
|
||||
// 输入: BGR 彩色图 (400×400)
|
||||
// 输出: 预处理后的单通道图 (实际是蓝色通道处理结果)
|
||||
//
|
||||
// 五步处理:
|
||||
// [1] extractChannel(bgr, blue, 0)
|
||||
// 取出 B 通道 — 关键设计!
|
||||
// 黄色减速带在 B 通道中亮度极低 (黄色≈R+G, 无B)
|
||||
// 白色斑马线在 B 通道中亮度正常 (白色≈R=G=B)
|
||||
// → B 通道天然过滤减速带, 保留斑马线
|
||||
//
|
||||
// [2] medianBlur(blue, 5)
|
||||
// 中值滤波核大小=5
|
||||
// 替代像素值为邻域中值 → 椒盐噪声/砖缝等独立噪点直接抹除
|
||||
// 与均值滤波不同: 中值保持边缘处的实际梯度值不衰减
|
||||
//
|
||||
// [3] morphologyEx(MORPH_OPEN, 3×3, iter=4)
|
||||
// 开运算 = Erode ×4 → Dilate ×4
|
||||
// 3×3 矩形核较小, 但 4 次迭代累积效果强:
|
||||
// - 窄车道线 (宽度 < 8px) → 在连续腐蚀中被完全消除
|
||||
// - 宽斑马线 (宽度 >> 8px) → 腐蚀后仍有残留 → 膨胀恢复
|
||||
// 本质: 保留"粗线条", 删除"细线条"
|
||||
//
|
||||
// [4] morphologyEx(MORPH_CLOSE, 5×5, iter=3)
|
||||
// 闭运算 = Dilate ×3 → Erode ×3
|
||||
// 5×5 较大核, 3 次迭代:
|
||||
// - 先膨胀填平斑马线内部的小缺口/裂缝 (缺损斑马线恢复)
|
||||
// - 后腐蚀恢复原始尺寸, 同时消除路面箭头碎片
|
||||
// 注意: 如果闭运算过强, 会把相邻的细斑马线粘成一片 → iter=3 是经验平衡点
|
||||
// ============================================================
|
||||
static cv::Mat preprocess(const cv::Mat &bgr)
|
||||
{
|
||||
cv::Mat blue;
|
||||
|
||||
// [1] 提取 B 通道 (蓝底色 + 减弱黄减速带)
|
||||
cv::extractChannel(bgr, blue, 0);
|
||||
|
||||
// [2] 中值滤波 5×5 → 去砖缝纹理/椒盐噪声/虚假小梯度点
|
||||
cv::medianBlur(blue, blue, 5);
|
||||
|
||||
// [3] 开运算: 3×3 矩形核, 4 次迭代 → 去除细窄车道线
|
||||
cv::Mat kernel_open = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
|
||||
cv::morphologyEx(blue, blue, cv::MORPH_OPEN, kernel_open, cv::Point(-1, -1), 4);
|
||||
|
||||
// [4] 闭运算: 5×5 矩形核, 3 次迭代 → 填补缺损 + 抑制箭头碎片
|
||||
cv::Mat kernel_close = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
|
||||
cv::morphologyEx(blue, blue, cv::MORPH_CLOSE, kernel_close, cv::Point(-1, -1), 3);
|
||||
|
||||
return blue;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// compute_gradient — Sobel 边缘梯度模值和方向
|
||||
//
|
||||
// 输入: Canny 边缘检测结果 (单通道, 像素值 0 或 255)
|
||||
// 输出: Amplitude — 梯度模值 (浮点, 弱边缘 < 30 置零)
|
||||
// theta — 梯度方向 0~90° (浮点)
|
||||
//
|
||||
// 方向定义:
|
||||
// 对每个像素 (r, c) 计算 Sobel 梯度:
|
||||
// gx = ∂I/∂c → 水平方向的变化率
|
||||
// gy = ∂I/∂r → 垂直方向的变化率
|
||||
//
|
||||
// Amplitude = sqrt(gx² + gy²) → 边缘强度
|
||||
// theta = atan2(|gy|, |gx|) → 边缘方向 (0~90°)
|
||||
//
|
||||
// 其中 atan2 取 abs 意味着不区分 "从左到右" 和 "从右到左" 的边缘,
|
||||
// 也不区分 "白→黑" 和 "黑→白" 的方向, 统一归到 0~90°。
|
||||
// 这对斑马线检测是合适的 — 我们只关心边缘的方向一致性, 不关心极性。
|
||||
//
|
||||
// 方向参考:
|
||||
// 0° = 纯水平边缘 (dy→0, dx 大)
|
||||
// 45° = 135° 角斜线
|
||||
// 90° = 纯垂直边缘 (dx→0, dy 大)
|
||||
// 斑马线纵线 ≈ 70~90°, 斜线 ≈ 30~60°
|
||||
//
|
||||
// 弱边缘过滤:
|
||||
// Canny 之后仍有少量噪声梯度 (<30)
|
||||
// 直接置零 → 后续直方图统计不参与
|
||||
// ============================================================
|
||||
static void compute_gradient(const cv::Mat &canny,
|
||||
cv::Mat &litude, cv::Mat &theta)
|
||||
{
|
||||
cv::Mat dx, dy;
|
||||
|
||||
// Sobel 算子 3×3:
|
||||
// dx = [-1 0 1; -2 0 2; -1 0 1] (x方向导数)
|
||||
// dy = [-1 -2 -1; 0 0 0; 1 2 1] (y方向导数)
|
||||
// CV_32F = 32 位浮点, 保留梯度方向亚度数精度
|
||||
cv::Sobel(canny, dx, CV_32F, 1, 0, 3);
|
||||
cv::Sobel(canny, dy, CV_32F, 0, 1, 3);
|
||||
|
||||
// 初始化输出矩阵为全零
|
||||
amplitude = cv::Mat::zeros(canny.size(), CV_32F);
|
||||
theta = cv::Mat::zeros(canny.size(), CV_32F);
|
||||
|
||||
// 逐像素计算模值和方向
|
||||
// 逐像素循环对于 400×400 = 160K 像素来说不是瓶颈,
|
||||
// 瓶颈在 medianBlur 和 morphologyEx 的核运算上
|
||||
for (int r = 0; r < canny.rows; ++r)
|
||||
{
|
||||
for (int c = 0; c < canny.cols; ++c)
|
||||
{
|
||||
float gx = dx.at<float>(r, c);
|
||||
float gy = dy.at<float>(r, c);
|
||||
|
||||
float amp = std::sqrt(gx * gx + gy * gy);
|
||||
float ang = std::atan2(std::abs(gy), std::abs(gx) + 1e-10f)
|
||||
* 180.0f / static_cast<float>(CV_PI);
|
||||
|
||||
// 弱边缘置零 (不参与后续方向统计)
|
||||
amplitude.at<float>(r, c) = (amp >= GRAD_THRESHOLD) ? amp : 0.0f;
|
||||
theta.at<float>(r, c) = ang;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// is_zebra_window — 对单个滑动窗口做斑马线分类
|
||||
//
|
||||
// 输入: 窗口内的梯度模值图 (amp_win) 和方向图 (ang_win)
|
||||
// 输出: true = 斑马线, false = 背景
|
||||
//
|
||||
// 判定逻辑:
|
||||
// [1] 扫描窗口内每个像素
|
||||
// - 跳过 amp ≤ 0 的像素 (弱边缘/噪声/平滑区域)
|
||||
// - 跳过 ang ≥ 70° 的像素 (方向在统计范围外)
|
||||
//
|
||||
// [2] 统计方向直方图 hist[0..13]
|
||||
// hist[k] = 方向在 [k*5, k*5+5) 的有效梯度点数量
|
||||
// e.g. hist[0] = 方向 0~4.999° 的点数
|
||||
//
|
||||
// [3] 取峰值 bin 的点数 = peak
|
||||
// peak 反映了窗口内 "某个特定方向上" 有多少条平行的边缘线
|
||||
//
|
||||
// [4] peak > ZEBRA_THRESHOLD (1500) → 斑马线
|
||||
// 斑马线 = 同一方向上大量平行边缘 → peak 极高
|
||||
// 普通路面 = 边缘方向杂乱 → 峰值被分散到不同 bin → 各 bin 点数低
|
||||
//
|
||||
// 在 IPM 开启时, 鸟瞰图将斑马线恢复为垂直矩形
|
||||
// → 所有边缘方向集中在 90° 附近 → hist[13] 极高 → 检测更灵敏
|
||||
// ============================================================
|
||||
static bool is_zebra_window(const cv::Mat &_win, const cv::Mat &ang_win)
|
||||
{
|
||||
int hist[HIST_BIN_COUNT] = {0};
|
||||
|
||||
for (int r = 0; r < amp_win.rows; ++r)
|
||||
{
|
||||
for (int c = 0; c < amp_win.cols; ++c)
|
||||
{
|
||||
float amp = amp_win.at<float>(r, c);
|
||||
if (amp <= 0.0f)
|
||||
continue;
|
||||
|
||||
float ang = ang_win.at<float>(r, c);
|
||||
if (ang < 0.0f || ang >= HIST_ANGLE_MAX)
|
||||
continue;
|
||||
|
||||
int bin = static_cast<int>(ang) / HIST_BIN_SIZE;
|
||||
if (bin >= 0 && bin < HIST_BIN_COUNT)
|
||||
hist[bin]++;
|
||||
}
|
||||
}
|
||||
|
||||
int peak = 0;
|
||||
for (int i = 0; i < HIST_BIN_COUNT; ++i)
|
||||
if (hist[i] > peak)
|
||||
peak = hist[i];
|
||||
|
||||
return peak > ZEBRA_THRESHOLD;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 帧缓冲显示 — 将 OpenCV BGR Mat 转为 RGB565 格式直写 /dev/fb0
|
||||
//
|
||||
// 硬件: SPI LCD 通过 /dev/fb0 (RGB565 格式)
|
||||
//
|
||||
// RGB565 编码:
|
||||
// 16 位: [R4 R3 R2 R1 R0] [G5 G4 G3] [G2 G1 G0] [B4 B3 B2 B1 B0]
|
||||
// R 取 高 5 位 (>>3, &0xF8)
|
||||
// G 取 高 6 位 (>>2, &0xFC)
|
||||
// B 取 高 5 位 (>>3)
|
||||
// ============================================================
|
||||
|
||||
static uint16_t convertRGBToRGB565(uint8_t r, uint8_t g, uint8_t b)
|
||||
{
|
||||
return ((r & 0xF8) << 8) |
|
||||
((g & 0xFC) << 3) |
|
||||
(b >> 3);
|
||||
}
|
||||
|
||||
static void display_on_fb(const cv::Mat &bgr, int /* fb_fd */,
|
||||
uint16_t *fb_buf, int scr_w, int scr_h)
|
||||
{
|
||||
cv::Mat display;
|
||||
cv::resize(bgr, display, cv::Size(scr_w, scr_h));
|
||||
|
||||
for (int y = 0; y < scr_h; ++y)
|
||||
{
|
||||
for (int x = 0; x < scr_w; ++x)
|
||||
{
|
||||
cv::Vec3b pixel = display.at<cv::Vec3b>(y, x);
|
||||
fb_buf[y * scr_w + x] = convertRGBToRGB565(
|
||||
pixel[2], pixel[1], pixel[0]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// main — 初始化 + 主检测循环 + 清理
|
||||
// ============================================================
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
// ---- 命令行参数解析 ----
|
||||
bool no_display = false;
|
||||
bool use_ipm = (USE_IPM != 0); // 默认跟随编译宏
|
||||
|
||||
for (int i = 1; i < argc; ++i)
|
||||
{
|
||||
if (std::strcmp(argv[i], "--no-display") == 0)
|
||||
no_display = true;
|
||||
else if (std::strcmp(argv[i], "--no-ipm") == 0)
|
||||
use_ipm = false;
|
||||
}
|
||||
|
||||
// ---- 注册信号处理 ----
|
||||
std::signal(SIGINT, signal_handler);
|
||||
std::signal(SIGTERM, signal_handler);
|
||||
|
||||
// ---- 预计算 IPM 逆矩阵 (启用时) ----
|
||||
if (use_ipm)
|
||||
{
|
||||
g_iM = build_ipm_inverse_matrix();
|
||||
printf("IPM 逆透视变换: 已启用 (%dx%d BEV)\n", IPM_SIZE, IPM_SIZE);
|
||||
printf(" 注意: 矩阵未针对实车标定, 效果可能不佳\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("IPM 逆透视变换: 已关闭 (使用前视图)\n");
|
||||
}
|
||||
|
||||
// ========================================================
|
||||
// 第 1 步: 打开 USB 摄像头
|
||||
// ========================================================
|
||||
cv::VideoCapture cap;
|
||||
|
||||
cap.open(0, cv::CAP_V4L2);
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
cap.open(0);
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
std::cerr << "无法打开摄像头 /dev/video0" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, CAM_WIDTH);
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, CAM_HEIGHT);
|
||||
cap.set(cv::CAP_PROP_FOURCC,
|
||||
cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
|
||||
printf("摄像头已打开 %dx%d\n",
|
||||
(int)cap.get(cv::CAP_PROP_FRAME_WIDTH),
|
||||
(int)cap.get(cv::CAP_PROP_FRAME_HEIGHT));
|
||||
|
||||
// ========================================================
|
||||
// 第 2 步: 打开帧缓冲 /dev/fb0 → mmap 映射到用户空间
|
||||
// ========================================================
|
||||
int fb_fd = -1;
|
||||
uint16_t *fb_buf = nullptr;
|
||||
int scr_w = 0, scr_h = 0;
|
||||
|
||||
if (!no_display)
|
||||
{
|
||||
fb_fd = open("/dev/fb0", O_RDWR);
|
||||
if (fb_fd < 0)
|
||||
{
|
||||
std::cerr << "警告: 无法打开 /dev/fb0，切换为无头模式" << std::endl;
|
||||
no_display = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo) < 0)
|
||||
{
|
||||
std::cerr << "无法获取屏幕信息" << std::endl;
|
||||
close(fb_fd);
|
||||
no_display = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
scr_w = vinfo.xres;
|
||||
scr_h = vinfo.yres;
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual
|
||||
* vinfo.bits_per_pixel / 8;
|
||||
|
||||
fb_buf = (uint16_t *)mmap(nullptr, fb_size,
|
||||
PROT_READ | PROT_WRITE,
|
||||
MAP_SHARED, fb_fd, 0);
|
||||
if (fb_buf == MAP_FAILED)
|
||||
{
|
||||
std::cerr << "无法映射帧缓冲区" << std::endl;
|
||||
close(fb_fd);
|
||||
no_display = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("屏幕分辨率: %dx%d\n", scr_w, scr_h);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ========================================================
|
||||
// 第 3 步: 主循环
|
||||
// ========================================================
|
||||
cv::Mat frame, bev, proc, gray, canny, amplitude, theta, result;
|
||||
int frame_count = 0;
|
||||
int zebra_frames = 0;
|
||||
bool was_zebra = false;
|
||||
|
||||
printf("\n开始斑马线检测 (按 Ctrl+C 退出)...\n");
|
||||
|
||||
while (g_running.load())
|
||||
{
|
||||
// ---- 3a. 摄像头采集一帧 ----
|
||||
if (!cap.read(frame) || frame.empty())
|
||||
{
|
||||
usleep(5000);
|
||||
continue;
|
||||
}
|
||||
|
||||
// ---- 3b. [IPM] 逆透视变换: 前视图 → 鸟瞰图 ----
|
||||
// 将倾斜透视拉平为俯瞰视角, 让斑马线恢复矩形平行特征
|
||||
// 未启用 IPM 时直接使用原始帧
|
||||
if (use_ipm)
|
||||
{
|
||||
bev = apply_ipm(frame);
|
||||
}
|
||||
else
|
||||
{
|
||||
bev = frame; // 使用前视图
|
||||
}
|
||||
|
||||
// ---- 3c. 尺寸归一化: BEV/原图 → 400×400 ----
|
||||
cv::resize(bev, proc, cv::Size(PROC_SIZE, PROC_SIZE));
|
||||
|
||||
// ---- 3d. 预处理: 蓝色通道 + 中值滤波 + 形态学 ----
|
||||
gray = preprocess(proc);
|
||||
|
||||
// ---- 3e. Canny 边缘检测 (双阈值 30/90) ----
|
||||
cv::Canny(gray, canny, 30, 90, 3);
|
||||
|
||||
// ---- 3f. 梯度模值与方向 ----
|
||||
compute_gradient(canny, amplitude, theta);
|
||||
|
||||
// ---- 3g. 滑动窗口逐窗检测 ----
|
||||
int img_h = proc.rows;
|
||||
bool detected = false;
|
||||
int z_ymin = img_h;
|
||||
int z_ymax = 0;
|
||||
|
||||
for (int top = 0; top <= img_h - WIN_H; top += WIN_STEP)
|
||||
{
|
||||
cv::Rect roi(WIN_COL_L, top, WIN_W, WIN_H);
|
||||
|
||||
if (is_zebra_window(amplitude(roi), theta(roi)))
|
||||
{
|
||||
detected = true;
|
||||
if (top < z_ymin) z_ymin = top;
|
||||
if (top + WIN_H > z_ymax) z_ymax = top + WIN_H;
|
||||
}
|
||||
}
|
||||
|
||||
// ---- 3h. 写入状态文件 (仅在状态变化时写) ----
|
||||
if (detected != was_zebra)
|
||||
{
|
||||
std::ofstream ofs("./zebra_detected", std::ios::trunc);
|
||||
ofs << (detected ? "1" : "0");
|
||||
was_zebra = detected;
|
||||
}
|
||||
|
||||
// ---- 3i. 绘制调试叠加图 ----
|
||||
result = proc.clone();
|
||||
|
||||
for (int top = 0; top <= img_h - WIN_H; top += WIN_STEP)
|
||||
{
|
||||
cv::line(result, cv::Point(WIN_COL_L, top),
|
||||
cv::Point(WIN_COL_R, top),
|
||||
cv::Scalar(128, 128, 128), 1);
|
||||
}
|
||||
|
||||
if (detected)
|
||||
{
|
||||
cv::rectangle(result,
|
||||
cv::Rect(WIN_COL_L, z_ymin, WIN_W, z_ymax - z_ymin),
|
||||
cv::Scalar(255, 0, 255), 3);
|
||||
}
|
||||
|
||||
if (detected)
|
||||
{
|
||||
cv::putText(result, "ZEBRA CROSSING!",
|
||||
cv::Point(10, 30),
|
||||
cv::FONT_HERSHEY_SIMPLEX, 0.8,
|
||||
cv::Scalar(0, 0, 255), 2);
|
||||
}
|
||||
|
||||
// ---- 3j. LCD 显示 ----
|
||||
if (!no_display && fb_buf)
|
||||
display_on_fb(result, fb_fd, fb_buf, scr_w, scr_h);
|
||||
|
||||
// ---- 3k. 统计 ----
|
||||
frame_count++;
|
||||
if (detected) zebra_frames++;
|
||||
|
||||
if (frame_count % 30 == 0)
|
||||
{
|
||||
printf("帧 %d | 斑马线: %s | 累计命中: %d/%d | IPM: %s\n",
|
||||
frame_count,
|
||||
detected ? "YES" : "NO ",
|
||||
zebra_frames, frame_count,
|
||||
use_ipm ? "ON" : "OFF");
|
||||
}
|
||||
}
|
||||
|
||||
// ========================================================
|
||||
// 第 4 步: 清理
|
||||
// ========================================================
|
||||
cap.release();
|
||||
|
||||
if (!no_display && fb_buf && fb_buf != MAP_FAILED)
|
||||
{
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo) == 0)
|
||||
{
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual
|
||||
* vinfo.bits_per_pixel / 8;
|
||||
munmap(fb_buf, fb_size);
|
||||
}
|
||||
close(fb_fd);
|
||||
}
|
||||
|
||||
printf("\n退出。共处理 %d 帧，命中 %d 帧 (%.1f%%), IPM: %s\n",
|
||||
frame_count, zebra_frames,
|
||||
frame_count > 0 ? (100.0 * zebra_frames / frame_count) : 0.0,
|
||||
use_ipm ? "ON" : "OFF");
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user