初始提交：龙芯2K0300智能车卖家Demo完整代码
This commit is contained in:
spdis
+14
@@ -0,0 +1,14 @@
|
|||||||
|
.vscode/*
|
||||||
|
!.vscode/settings.json
|
||||||
|
!.vscode/tasks.json
|
||||||
|
!.vscode/launch.json
|
||||||
|
!.vscode/extensions.json
|
||||||
|
*.code-workspace
|
||||||
|
|
||||||
|
build/*
|
||||||
|
build2/*
|
||||||
|
build3/*
|
||||||
|
test/
|
||||||
|
|
||||||
|
# Local History for Visual Studio Code
|
||||||
|
.history/
|
||||||
@@ -0,0 +1,172 @@
|
|||||||
|
# 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,46 @@
|
|||||||
|
# 包含交叉编译工具链
|
||||||
|
include(cross.cmake)
|
||||||
|
|
||||||
|
# 设置CMake的最低版本要求
|
||||||
|
cmake_minimum_required(VERSION 3.5.0)
|
||||||
|
|
||||||
|
# 设置 C++ 标准
|
||||||
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
|
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -pthread -Wall") # 对于 C++ 编译器
|
||||||
|
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") # 对于 C 编译器
|
||||||
|
|
||||||
|
# 定义项目名称和版本，并指定使用C和C++语言
|
||||||
|
project(smartcar_demo2 VERSION 0.1.0 LANGUAGES C CXX)
|
||||||
|
|
||||||
|
# 设置OpenCV的安装路径
|
||||||
|
set(OpenCV_DIR /home/ilikara/loongson/opencv-4.11.0/loongson)
|
||||||
|
|
||||||
|
# 查找OpenCV库，确保安装了所需的依赖
|
||||||
|
find_package(OpenCV REQUIRED)
|
||||||
|
|
||||||
|
# 包含OpenCV的头文件路径
|
||||||
|
include_directories(${OpenCV_INCLUDE_DIRS})
|
||||||
|
message(STATUS "OpenCV Include Directories: ${OpenCV_INCLUDE_DIRS}")
|
||||||
|
|
||||||
|
# 包含项目的自定义库路径
|
||||||
|
include_directories(lib)
|
||||||
|
|
||||||
|
# 查找源文件所在的目录
|
||||||
|
aux_source_directory(src DIR_SRCS)
|
||||||
|
|
||||||
|
# 将 src 目录下的源文件编译为静态库
|
||||||
|
add_library(common_lib STATIC ${DIR_SRCS})
|
||||||
|
|
||||||
|
# 添加子目录
|
||||||
|
add_subdirectory(main) # 主程序
|
||||||
|
add_subdirectory(demo1) # demo1
|
||||||
|
add_subdirectory(framebuffer_demo)
|
||||||
|
add_subdirectory(opencv_demo1)
|
||||||
|
add_subdirectory(opencv_demo2)
|
||||||
|
add_subdirectory(opencv_demo3)
|
||||||
|
add_subdirectory(encoder_demo)
|
||||||
|
add_subdirectory(jy62_demo)
|
||||||
|
add_subdirectory(key_demo)
|
||||||
|
add_subdirectory(wonderEcho_demo)
|
||||||
|
add_subdirectory(udp_receive)
|
||||||
|
add_subdirectory(image_test)
|
||||||
@@ -0,0 +1,256 @@
|
|||||||
|
# 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` |
|
||||||
+17
@@ -0,0 +1,17 @@
|
|||||||
|
# cross.cmake文件
|
||||||
|
# 设置为1则表示交叉编译，设置为0则表示x86 gcc编译
|
||||||
|
SET(CROSS_COMPILE 1)
|
||||||
|
|
||||||
|
IF(CROSS_COMPILE)
|
||||||
|
SET(CMAKE_SYSTEM_NAME Linux)
|
||||||
|
set(CMAKE_SYSTEM_PROCESSOR loongson)
|
||||||
|
SET(TOOLCHAIN_DIR "/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6")
|
||||||
|
set(CMAKE_CXX_COMPILER ${TOOLCHAIN_DIR}/bin/loongarch64-linux-gnu-g++)
|
||||||
|
set(CMAKE_C_COMPILER ${TOOLCHAIN_DIR}/bin/loongarch64-linux-gnu-gcc)
|
||||||
|
|
||||||
|
SET(CMAKE_FIND_ROOT_PATH ${TOOLCHAIN_DIR})
|
||||||
|
SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
|
||||||
|
SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
|
||||||
|
SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
|
||||||
|
|
||||||
|
ENDIF(CROSS_COMPILE)
|
||||||
@@ -0,0 +1,88 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
# SmartCar Demo 控制脚本
|
||||||
|
# 用法: sh ctl.sh start | stop | init
|
||||||
|
|
||||||
|
DIR="$(dirname "$0")"
|
||||||
|
PWMCHIP=8
|
||||||
|
PWMPERIOD=50000
|
||||||
|
|
||||||
|
init_pins() {
|
||||||
|
echo "[demo] 初始化引脚..."
|
||||||
|
for pin in 73 12 13 66 67 75 72; do
|
||||||
|
echo $pin > /sys/class/gpio/export 2>/dev/null
|
||||||
|
done
|
||||||
|
echo out > /sys/class/gpio/gpio73/direction 2>/dev/null
|
||||||
|
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 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
|
||||||
|
echo ${PWMPERIOD} > /sys/class/pwm/pwmchip${PWMCHIP}/pwm${ch}/period 2>/dev/null
|
||||||
|
echo 1 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm${ch}/enable 2>/dev/null
|
||||||
|
echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm${ch}/duty_cycle 2>/dev/null
|
||||||
|
done
|
||||||
|
|
||||||
|
echo 0 > /sys/class/pwm/pwmchip1/export 2>/dev/null
|
||||||
|
echo 3000000 > /sys/class/pwm/pwmchip1/pwm0/period 2>/dev/null
|
||||||
|
echo 1 > /sys/class/pwm/pwmchip1/pwm0/enable 2>/dev/null
|
||||||
|
echo 1520000 > /sys/class/pwm/pwmchip1/pwm0/duty_cycle 2>/dev/null
|
||||||
|
|
||||||
|
echo 11 > "$DIR/speed" 2>/dev/null
|
||||||
|
echo 8 > "$DIR/deadband" 2>/dev/null
|
||||||
|
echo 1.5 > "$DIR/steer_gain" 2>/dev/null
|
||||||
|
echo 3.5 > "$DIR/kp" 2>/dev/null
|
||||||
|
echo 0.3 > "$DIR/ki" 2>/dev/null
|
||||||
|
echo 2.0 > "$DIR/kd" 2>/dev/null
|
||||||
|
echo 0.6 > "$DIR/mortor_kp" 2>/dev/null
|
||||||
|
echo 0.2 > "$DIR/mortor_ki" 2>/dev/null
|
||||||
|
echo 0 > "$DIR/mortor_kd" 2>/dev/null
|
||||||
|
echo 40 > "$DIR/foresee" 2>/dev/null
|
||||||
|
echo 0 > "$DIR/start" 2>/dev/null
|
||||||
|
echo 1 > "$DIR/showImg" 2>/dev/null
|
||||||
|
echo "[demo] 引脚初始化完成"
|
||||||
|
}
|
||||||
|
|
||||||
|
do_start() {
|
||||||
|
echo "[demo] 启动..."
|
||||||
|
echo 1 > "$DIR/start"
|
||||||
|
cd "$DIR"
|
||||||
|
LD_PRELOAD="$DIR/gpio_fix_final.so" nohup ./smartcar_demo2 > "$DIR/out.log" 2>&1 &
|
||||||
|
echo "[demo] PID=$!"
|
||||||
|
}
|
||||||
|
|
||||||
|
do_stop() {
|
||||||
|
echo "[demo] 停止..."
|
||||||
|
echo 0 > "$DIR/start" 2>/dev/null
|
||||||
|
killall -9 smartcar_demo2 2>/dev/null
|
||||||
|
sleep 0.3
|
||||||
|
echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm1/duty_cycle 2>/dev/null
|
||||||
|
echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm2/duty_cycle 2>/dev/null
|
||||||
|
echo 0 > /sys/class/gpio/gpio73/value 2>/dev/null
|
||||||
|
echo "[demo] 已停止, PWM已归零"
|
||||||
|
}
|
||||||
|
|
||||||
|
case "${1}" in
|
||||||
|
start)
|
||||||
|
init_pins
|
||||||
|
do_start
|
||||||
|
;;
|
||||||
|
stop)
|
||||||
|
do_stop
|
||||||
|
;;
|
||||||
|
init)
|
||||||
|
init_pins
|
||||||
|
;;
|
||||||
|
*)
|
||||||
|
echo "用法: sh ctl.sh start | stop | init"
|
||||||
|
exit 1
|
||||||
|
;;
|
||||||
|
esac
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# demo1
|
||||||
|
add_executable(demo1 demo1.cpp)
|
||||||
|
|
||||||
|
# 链接 OpenCV 库
|
||||||
|
target_link_libraries(demo1 common_lib ${OpenCV_LIBS})
|
||||||
+125
@@ -0,0 +1,125 @@
|
|||||||
|
/*
|
||||||
|
* @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,5 @@
|
|||||||
|
# demo1
|
||||||
|
add_executable(encoder_demo encoder_demo.cpp)
|
||||||
|
|
||||||
|
# 链接 OpenCV 库
|
||||||
|
target_link_libraries(encoder_demo common_lib ${OpenCV_LIBS})
|
||||||
@@ -0,0 +1,22 @@
|
|||||||
|
/*
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# demo1
|
||||||
|
add_executable(framebuffer_demo framebuffer_demo.cpp)
|
||||||
|
|
||||||
|
# 链接 OpenCV 库
|
||||||
|
target_link_libraries(framebuffer_demo common_lib ${OpenCV_LIBS})
|
||||||
@@ -0,0 +1,207 @@
|
|||||||
|
#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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# demo2
|
||||||
|
add_executable(image_test image_test.cpp)
|
||||||
|
|
||||||
|
# 链接 OpenCV 库
|
||||||
|
target_link_libraries(image_test common_lib ${OpenCV_LIBS})
|
||||||
@@ -0,0 +1,420 @@
|
|||||||
|
/*
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# demo1
|
||||||
|
add_executable(jy62_demo jy62_demo.cpp)
|
||||||
|
|
||||||
|
# 链接 OpenCV 库
|
||||||
|
target_link_libraries(jy62_demo)
|
||||||
@@ -0,0 +1,182 @@
|
|||||||
|
/*
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# demo1
|
||||||
|
add_executable(key_demo key_demo.cpp)
|
||||||
|
|
||||||
|
# 链接库
|
||||||
|
target_link_libraries(key_demo common_lib)
|
||||||
@@ -0,0 +1,68 @@
|
|||||||
|
/*
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
+44
@@ -0,0 +1,44 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2024-10-10 15:02:00
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2024-12-01 03:49:47
|
||||||
|
* @FilePath: /smartcar/lib/GPIO.h
|
||||||
|
* @Description:
|
||||||
|
*
|
||||||
|
* Copyright (c) 2024 by ilikara 3435193369@qq.com, All Rights Reserved.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef GPIO_H
|
||||||
|
#define GPIO_H
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
#include <sstream>
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
class GPIO
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
GPIO(int gpioNum);
|
||||||
|
~GPIO();
|
||||||
|
|
||||||
|
bool setDirection(const std::string &direction);
|
||||||
|
bool setEdge(const std::string &edge);
|
||||||
|
bool setValue(bool value); // 设置 GPIO 输出值
|
||||||
|
bool readValue(); // 读取 GPIO 输入值
|
||||||
|
int getFileDescriptor() const; // 获取 GPIO 文件描述符
|
||||||
|
|
||||||
|
private:
|
||||||
|
int gpioNum;
|
||||||
|
int fd; // 文件描述符
|
||||||
|
std::string gpioPath;
|
||||||
|
|
||||||
|
bool writeToFile(const std::string &path, const std::string &value);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // GPIO_H
|
||||||
@@ -0,0 +1,37 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2024-10-10 14:36:47
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2025-03-21 10:42:00
|
||||||
|
* @FilePath: /smartcar/lib/MotorController.h
|
||||||
|
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||||
|
*/
|
||||||
|
#ifndef MOTOR_CONTROLLER_H
|
||||||
|
#define MOTOR_CONTROLLER_H
|
||||||
|
|
||||||
|
#include "PwmController.h"
|
||||||
|
#include "PIDController.h"
|
||||||
|
#include "GPIO.h"
|
||||||
|
#include "encoder.h"
|
||||||
|
|
||||||
|
class MotorController
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
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_);
|
||||||
|
~MotorController(void);
|
||||||
|
|
||||||
|
void updateSpeed(void);
|
||||||
|
void updateTarget(int speed);
|
||||||
|
void updateduty(double dutyCycle);
|
||||||
|
PIDController pidController;
|
||||||
|
|
||||||
|
private:
|
||||||
|
PwmController pwmController;
|
||||||
|
ENCODER encoder;
|
||||||
|
GPIO directionGPIO;
|
||||||
|
int encoder_dir;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // MOTOR_CONTROLLER_H
|
||||||
@@ -0,0 +1,57 @@
|
|||||||
|
#ifndef PIDCONTROLLER_H
|
||||||
|
#define PIDCONTROLLER_H
|
||||||
|
|
||||||
|
#include <algorithm> // 用于 std::clamp
|
||||||
|
|
||||||
|
enum PIDMode
|
||||||
|
{
|
||||||
|
POSITION, // 位置式 PID
|
||||||
|
INCREMENTAL // 增量式 PID
|
||||||
|
};
|
||||||
|
|
||||||
|
class PIDController
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// 构造函数，初始化 PID 参数
|
||||||
|
PIDController(double kp, double ki, double kd, double target, PIDMode mode = POSITION,
|
||||||
|
double output_limit = 100.0, double integral_limit = 100.0);
|
||||||
|
|
||||||
|
// 更新 PID 控制器
|
||||||
|
double update(double measured_value);
|
||||||
|
|
||||||
|
// 设置新的目标值
|
||||||
|
void setTarget(double target);
|
||||||
|
|
||||||
|
// 设置 PID 参数
|
||||||
|
void setPID(double kp, double ki, double kd);
|
||||||
|
|
||||||
|
// 设置 PID 控制模式（位置式或增量式）
|
||||||
|
void setMode(PIDMode mode);
|
||||||
|
|
||||||
|
// 设置输出和积分的限幅值
|
||||||
|
void setLimits(double output_limit, double integral_limit);
|
||||||
|
|
||||||
|
private:
|
||||||
|
// 位置式 PID 实现
|
||||||
|
double positionPID(double error);
|
||||||
|
|
||||||
|
// 增量式 PID 实现
|
||||||
|
double incrementalPID(double error);
|
||||||
|
|
||||||
|
double kp_; // 比例增益
|
||||||
|
double ki_; // 积分增益
|
||||||
|
double kd_; // 微分增益
|
||||||
|
double target_; // 目标值
|
||||||
|
|
||||||
|
double prev_error_; // 前一次误差
|
||||||
|
double prev_prev_error_; // 前两次误差 (用于增量式)
|
||||||
|
double integral_; // 积分项
|
||||||
|
double prev_output_; // 前一次的控制器输出 (用于增量式)
|
||||||
|
|
||||||
|
PIDMode mode_; // 控制模式 (位置式或增量式)
|
||||||
|
|
||||||
|
double output_limit_; // 输出限幅
|
||||||
|
double integral_limit_; // 积分限幅
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // PIDCONTROLLER_H
|
||||||
@@ -0,0 +1,40 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2025-02-23 09:08:09
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2025-03-20 11:38:33
|
||||||
|
* @FilePath: /smartcar/lib/PwmController.h
|
||||||
|
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||||
|
*/
|
||||||
|
#ifndef PWM_CONTROLLER_H
|
||||||
|
#define PWM_CONTROLLER_H
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <fstream>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
class PwmController
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
PwmController(int pwmchip, int pwmnum, bool polarity = true);
|
||||||
|
~PwmController();
|
||||||
|
|
||||||
|
bool enable();
|
||||||
|
bool disable();
|
||||||
|
bool setPeriod(unsigned int period_ns);
|
||||||
|
bool setDutyCycle(unsigned int duty_cycle_ns);
|
||||||
|
bool setPolarity(bool polarity);
|
||||||
|
bool initialize();
|
||||||
|
int readPeriod();
|
||||||
|
int readDutyCycle();
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::string pwmPath; // PWM设备的路径
|
||||||
|
int pwmchip;
|
||||||
|
int pwmnum;
|
||||||
|
int period;
|
||||||
|
int duty_cycle;
|
||||||
|
bool writeToFile(const std::string &path, const std::string &value);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
+28
@@ -0,0 +1,28 @@
|
|||||||
|
#ifndef TIMER_H
|
||||||
|
#define TIMER_H
|
||||||
|
|
||||||
|
#include <thread>
|
||||||
|
#include <chrono>
|
||||||
|
#include <atomic>
|
||||||
|
#include <functional>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
class Timer
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Timer(int interval_ms, std::function<void()> task);
|
||||||
|
~Timer();
|
||||||
|
|
||||||
|
void start();
|
||||||
|
void stop();
|
||||||
|
|
||||||
|
private:
|
||||||
|
void run();
|
||||||
|
|
||||||
|
int interval; // Interval in milliseconds
|
||||||
|
std::function<void()> task;
|
||||||
|
std::atomic<bool> running;
|
||||||
|
std::thread worker;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // TIMER_H
|
||||||
@@ -0,0 +1,44 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2024-10-10 08:28:56
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2025-01-07 09:35:04
|
||||||
|
* @FilePath: /smartcar/lib/camera.h
|
||||||
|
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||||
|
*/
|
||||||
|
#ifndef CAMERA_H_
|
||||||
|
#define CAMERA_H_
|
||||||
|
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
#include <sstream>
|
||||||
|
#include <iomanip>
|
||||||
|
#include <mutex>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <sys/mman.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <linux/fb.h>
|
||||||
|
#include <algorithm>
|
||||||
|
#include <chrono>
|
||||||
|
|
||||||
|
#include "image_cv.h"
|
||||||
|
#include "PIDController.h"
|
||||||
|
#include "PwmController.h"
|
||||||
|
#include "global.h"
|
||||||
|
#include "frame_buffer.h"
|
||||||
|
#include "serial.h"
|
||||||
|
|
||||||
|
int CameraInit(uint8_t camera_id, double dest_fps, int width, int height);
|
||||||
|
int CameraHandler(void);
|
||||||
|
void streamCapture(void);
|
||||||
|
void cameraDeInit(void);
|
||||||
|
|
||||||
|
extern bool streamCaptureRunning;
|
||||||
|
|
||||||
|
extern double kp;
|
||||||
|
extern double ki;
|
||||||
|
extern double kd;
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,18 @@
|
|||||||
|
#ifndef CONTROL_H
|
||||||
|
#define CONTROL_H
|
||||||
|
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
#include "MotorController.h"
|
||||||
|
#include "global.h"
|
||||||
|
#include "serial.h"
|
||||||
|
|
||||||
|
void ControlInit();
|
||||||
|
void ControlMain();
|
||||||
|
void ControlExit();
|
||||||
|
|
||||||
|
extern double mortor_kp;
|
||||||
|
extern double mortor_ki;
|
||||||
|
extern double mortor_kd;
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,75 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2024-10-11 06:20:04
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2024-12-01 03:48:28
|
||||||
|
* @FilePath: /smartcar/lib/encoder.h
|
||||||
|
* @Description:
|
||||||
|
*
|
||||||
|
* Copyright (c) 2024 by ilikara 3435193369@qq.com, All Rights Reserved.
|
||||||
|
*/
|
||||||
|
#ifndef ENCODER_H
|
||||||
|
#define ENCODER_H
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <sys/mman.h>
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "GPIO.h"
|
||||||
|
|
||||||
|
#define PWM_BASE_ADDR 0x1611B000
|
||||||
|
#define PWM_OFFSET 0x10
|
||||||
|
#define LOW_BUFFER_OFFSET 0x4
|
||||||
|
#define FULL_BUFFER_OFFSET 0x8
|
||||||
|
#define CONTROL_REG_OFFSET 0xC
|
||||||
|
|
||||||
|
#define CNTR_ENABLE_BIT (1 << 0) // 计数器使能
|
||||||
|
#define PULSE_OUT_ENABLE_BIT (1 << 3) // 脉冲输出使能（低有效）
|
||||||
|
#define SINGLE_PULSE_BIT (1 << 4) // 单脉冲控制位
|
||||||
|
#define INT_ENABLE_BIT (1 << 5) // 中断使能
|
||||||
|
#define INT_STATUS_BIT (1 << 6) // 中断状态
|
||||||
|
#define COUNTER_RESET_BIT (1 << 7) // 计数器重置
|
||||||
|
#define MEASURE_PULSE_BIT (1 << 8) // 测量脉冲使能
|
||||||
|
#define INVERT_OUTPUT_BIT (1 << 9) // 输出翻转使能
|
||||||
|
#define DEAD_ZONE_ENABLE_BIT (1 << 10) // 防死区使能
|
||||||
|
|
||||||
|
#define LOW_BUFFER_ADDR (PWM_BASE_ADDR + LOW_BUFFER_OFFSET)
|
||||||
|
#define FULL_BUFFER_ADDR (PWM_BASE_ADDR + FULL_BUFFER_OFFSET)
|
||||||
|
#define CONTROL_REG_ADDR (PWM_BASE_ADDR + CONTROL_REG_OFFSET)
|
||||||
|
|
||||||
|
#define GPIO_PIN 73
|
||||||
|
#define GPIO_PATH "/sys/class/gpio/gpio73/value"
|
||||||
|
|
||||||
|
#define PAGE_SIZE 0x10000
|
||||||
|
|
||||||
|
#define REG_READ(addr) (*(volatile uint32_t *)(addr))
|
||||||
|
#define REG_WRITE(addr, val) (*(volatile uint32_t *)(addr) = (val))
|
||||||
|
|
||||||
|
class ENCODER
|
||||||
|
{
|
||||||
|
|
||||||
|
public:
|
||||||
|
ENCODER(int pwmNum, int gpioNum);
|
||||||
|
~ENCODER();
|
||||||
|
|
||||||
|
double pulse_counter_update(void);
|
||||||
|
|
||||||
|
private:
|
||||||
|
uint32_t base_addr;
|
||||||
|
GPIO directionGPIO;
|
||||||
|
void *low_buffer;
|
||||||
|
void *full_buffer;
|
||||||
|
void *control_buffer;
|
||||||
|
void *map_register(uint32_t physical_address, size_t size);
|
||||||
|
void PWM_Init(void);
|
||||||
|
void reset_counter(void);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,8 @@
|
|||||||
|
#ifndef _FRAME_BUFFER_H
|
||||||
|
#define _FRAME_BUFFER_H
|
||||||
|
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
|
||||||
|
void convertMatToRGB565(const cv::Mat &frame, uint16_t *buffer, int width, int height);
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,37 @@
|
|||||||
|
#ifndef GLOBAL_H
|
||||||
|
#define GLOBAL_H
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
#include <atomic>
|
||||||
|
|
||||||
|
const std::string kp_file = "./kp";
|
||||||
|
const std::string ki_file = "./ki";
|
||||||
|
const std::string kd_file = "./kd";
|
||||||
|
|
||||||
|
const std::string mortor_kp_file = "./mortor_kp";
|
||||||
|
const std::string mortor_ki_file = "./mortor_ki";
|
||||||
|
const std::string mortor_kd_file = "./mortor_kd";
|
||||||
|
|
||||||
|
const std::string start_file = "./start";
|
||||||
|
const std::string showImg_file = "./showImg";
|
||||||
|
const std::string destfps_file = "./destfps";
|
||||||
|
const std::string foresee_file = "./foresee";
|
||||||
|
const std::string saveImg_file = "./saveImg";
|
||||||
|
const std::string speed_file = "./speed";
|
||||||
|
const std::string deadband_file = "./deadband";
|
||||||
|
const std::string steer_gain_file = "./steer_gain";
|
||||||
|
const std::string center_bias_file = "./center_bias";
|
||||||
|
|
||||||
|
// 从文件读取双精度值
|
||||||
|
double readDoubleFromFile(const std::string &filename);
|
||||||
|
|
||||||
|
// 从文件中读取标志
|
||||||
|
bool readFlag(const std::string &filename);
|
||||||
|
|
||||||
|
extern std::atomic<double> PID_rotate;
|
||||||
|
|
||||||
|
extern double target_speed;
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2025-01-04 06:51:37
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2025-03-13 08:05:00
|
||||||
|
* @FilePath: /smartcar/lib/image_main.h
|
||||||
|
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||||
|
*/
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <iostream>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
void image_main();
|
||||||
|
|
||||||
|
extern cv::Mat raw_frame;
|
||||||
|
extern cv::Mat grayFrame;
|
||||||
|
extern cv::Mat binarizedFrame;
|
||||||
|
extern cv::Mat morphologyExFrame;
|
||||||
|
extern cv::Mat track;
|
||||||
|
|
||||||
|
extern std::vector<int> left_line; // 左边缘列号数组
|
||||||
|
extern std::vector<int> right_line; // 右边缘列号数组
|
||||||
|
extern std::vector<int> mid_line; // 中线列号数组
|
||||||
|
extern std::vector<double> left_line_filtered; // 中线列号数组
|
||||||
|
extern std::vector<double> right_line_filtered; // 中线列号数组
|
||||||
|
extern std::vector<double> mid_line_filtered; // 中线列号数组
|
||||||
|
|
||||||
|
extern int line_tracking_height, line_tracking_width;
|
||||||
@@ -0,0 +1,61 @@
|
|||||||
|
#ifndef _SERIAL_H
|
||||||
|
#define _SERIAL_H
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
// class Serial{
|
||||||
|
// public:
|
||||||
|
// Serial();
|
||||||
|
// ~Serial();
|
||||||
|
|
||||||
|
// private:
|
||||||
|
// }
|
||||||
|
|
||||||
|
extern char vofa_buffer[64];
|
||||||
|
extern bool vofa_justfloat(int CH_count);
|
||||||
|
|
||||||
|
enum ImgFormat
|
||||||
|
{
|
||||||
|
Format_Invalid,
|
||||||
|
Format_Mono,
|
||||||
|
Format_MonoLSB,
|
||||||
|
Format_Indexed8,
|
||||||
|
Format_RGB32,
|
||||||
|
Format_ARGB32,
|
||||||
|
Format_ARGB32_Premultiplied,
|
||||||
|
Format_RGB16,
|
||||||
|
Format_ARGB8565_Premultiplied,
|
||||||
|
Format_RGB666,
|
||||||
|
Format_ARGB6666_Premultiplied,
|
||||||
|
Format_RGB555,
|
||||||
|
Format_ARGB8555_Premultiplied,
|
||||||
|
Format_RGB888,
|
||||||
|
Format_RGB444,
|
||||||
|
Format_ARGB4444_Premultiplied,
|
||||||
|
Format_RGBX8888,
|
||||||
|
Format_RGBA8888,
|
||||||
|
Format_RGBA8888_Premultiplied,
|
||||||
|
Format_BGR30,
|
||||||
|
Format_A2BGR30_Premultiplied,
|
||||||
|
Format_RGB30,
|
||||||
|
Format_A2RGB30_Premultiplied,
|
||||||
|
Format_Alpha8,
|
||||||
|
Format_Grayscale8,
|
||||||
|
|
||||||
|
// 以下格式发送时，IMG_WIDTH和IMG_HEIGHT不需要强制指定，设置为-1即可
|
||||||
|
Format_BMP,
|
||||||
|
Format_GIF,
|
||||||
|
Format_JPG,
|
||||||
|
Format_PNG,
|
||||||
|
Format_PBM,
|
||||||
|
Format_PGM,
|
||||||
|
Format_PPM,
|
||||||
|
Format_XBM,
|
||||||
|
Format_XPM,
|
||||||
|
Format_SVG,
|
||||||
|
};
|
||||||
|
extern bool vofa_image(int IMG_ID, int IMG_SIZE, int IMG_WIDTH, int IMG_HEIGHT, ImgFormat IMG_FORMAT, char *image);
|
||||||
|
|
||||||
|
#endif
|
||||||
+35
@@ -0,0 +1,35 @@
|
|||||||
|
#ifndef VIDEO_H_
|
||||||
|
#define VIDEO_H_
|
||||||
|
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <iostream>
|
||||||
|
#include <iomanip>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <linux/fb.h>
|
||||||
|
|
||||||
|
#include "Timer.h"
|
||||||
|
#include "frame_buffer.h"
|
||||||
|
|
||||||
|
class Video
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Video(const std::string &filename, double fps);
|
||||||
|
~Video(void);
|
||||||
|
|
||||||
|
Timer timer;
|
||||||
|
cv::Mat frame;
|
||||||
|
std::mutex frameMutex;
|
||||||
|
private:
|
||||||
|
cv::VideoCapture cap;
|
||||||
|
cv::Mat fbImage;
|
||||||
|
cv::Mat resizedFrame;
|
||||||
|
void streamCapture(void);
|
||||||
|
int screenHeight, screenWidth;
|
||||||
|
int newWidth, newHeight;
|
||||||
|
int fb;
|
||||||
|
uint16_t *fb_buffer;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,22 @@
|
|||||||
|
#ifndef VL53L0X_H
|
||||||
|
#define VL53L0X_H
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#include "vl53l0x_def.h"
|
||||||
|
|
||||||
|
class VL53L0X
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
VL53L0X();
|
||||||
|
~VL53L0X();
|
||||||
|
|
||||||
|
bool init();
|
||||||
|
bool readRange(VL53L0X_RangingMeasurementData_t &data);
|
||||||
|
bool stop();
|
||||||
|
bool isOpen() const { return fd > 0; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
int fd;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,640 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
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_ */
|
||||||
@@ -0,0 +1,257 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
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_ */
|
||||||
|
|
||||||
|
|
||||||
@@ -0,0 +1,69 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
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 <stdint.h>
|
||||||
|
#include <stddef.h>
|
||||||
|
|
||||||
|
#ifndef NULL
|
||||||
|
#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_ */
|
||||||
@@ -0,0 +1,4 @@
|
|||||||
|
# 主程序
|
||||||
|
add_executable(smartcar_demo2 main.cpp)
|
||||||
|
|
||||||
|
target_link_libraries(smartcar_demo2 common_lib ${OpenCV_LIBS})
|
||||||
+149
@@ -0,0 +1,149 @@
|
|||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <string>
|
||||||
|
#include <csignal>
|
||||||
|
#include <atomic>
|
||||||
|
|
||||||
|
#include "global.h"
|
||||||
|
#include "camera.h"
|
||||||
|
#include "control.h"
|
||||||
|
#include "Timer.h"
|
||||||
|
#include "serial.h"
|
||||||
|
#include "encoder.h"
|
||||||
|
#include "video.h"
|
||||||
|
|
||||||
|
std::atomic<bool> running(true);
|
||||||
|
void signalHandler(int signal)
|
||||||
|
{
|
||||||
|
running.store(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(void)
|
||||||
|
{
|
||||||
|
// Video video("test.mp4", 30);
|
||||||
|
|
||||||
|
// // 获取帧缓冲区设备信息
|
||||||
|
// int fb = open("/dev/fb0", O_RDWR);
|
||||||
|
// if (fb == -1)
|
||||||
|
// {
|
||||||
|
// std::cerr << "无法打开帧缓冲区设备" << std::endl;
|
||||||
|
// return -1;
|
||||||
|
// }
|
||||||
|
// struct fb_var_screeninfo vinfo;
|
||||||
|
// ioctl(fb, FBIOGET_VSCREENINFO, &vinfo);
|
||||||
|
|
||||||
|
// // 设置屏幕参数
|
||||||
|
// int screenWidth = 160;
|
||||||
|
// int screenHeight = 128;
|
||||||
|
|
||||||
|
// int newWidth, newHeight;
|
||||||
|
|
||||||
|
// // 创建帧缓冲区
|
||||||
|
// uint16_t *fb_buffer = new uint16_t[screenWidth * screenHeight];
|
||||||
|
|
||||||
|
// int frameCount = 0;
|
||||||
|
|
||||||
|
// cv::Mat frame, resizedFrame, fbImage(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0)); // 帧缓冲区图像
|
||||||
|
|
||||||
|
// int frame_count = 0;
|
||||||
|
|
||||||
|
// video.timer.start();
|
||||||
|
|
||||||
|
// while(1) {
|
||||||
|
// video.frameMutex.lock();
|
||||||
|
// if(newWidth == 0)
|
||||||
|
// {
|
||||||
|
// // 保持视频长宽比
|
||||||
|
// int videoWidth = video.frame.cols;
|
||||||
|
// int videoHeight = video.frame.rows;
|
||||||
|
// double aspectRatio = static_cast<double>(videoWidth) / videoHeight;
|
||||||
|
// // 根据屏幕大小计算缩放后的宽度和高度
|
||||||
|
// if (screenWidth / static_cast<double>(screenHeight) > aspectRatio)
|
||||||
|
// {
|
||||||
|
// // 屏幕更宽，以高度为基准
|
||||||
|
// newHeight = screenHeight;
|
||||||
|
// newWidth = static_cast<int>(newHeight * aspectRatio);
|
||||||
|
// }
|
||||||
|
// else
|
||||||
|
// {
|
||||||
|
// // 屏幕更高，以宽度为基准
|
||||||
|
// newWidth = screenWidth;
|
||||||
|
// newHeight = static_cast<int>(newWidth / aspectRatio);
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // 缩放视频到新尺寸
|
||||||
|
// cv::resize(video.frame, resizedFrame, cv::Size(newWidth, newHeight));
|
||||||
|
// video.frameMutex.unlock();
|
||||||
|
|
||||||
|
// // 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
|
||||||
|
// fbImage.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
|
||||||
|
// cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
|
||||||
|
// resizedFrame.copyTo(fbImage(roi));
|
||||||
|
|
||||||
|
// // 将帧缓冲区图像转换为RGB565格式
|
||||||
|
// convertMatToRGB565(fbImage, fb_buffer, screenWidth, screenHeight);
|
||||||
|
|
||||||
|
// // 写入帧缓冲区
|
||||||
|
// lseek(fb, 0, SEEK_SET);
|
||||||
|
// write(fb, fb_buffer, screenWidth * screenHeight * 2);
|
||||||
|
// std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||||||
|
// }
|
||||||
|
std::signal(SIGINT, signalHandler);
|
||||||
|
|
||||||
|
double dest_fps = readDoubleFromFile(destfps_file);
|
||||||
|
int dest_frame_duration = CameraInit(0, dest_fps, 320, 240);
|
||||||
|
printf("%d\n", dest_frame_duration);
|
||||||
|
if (dest_frame_duration != -1)
|
||||||
|
{
|
||||||
|
streamCaptureRunning = true;
|
||||||
|
std::thread camworker = std::thread(streamCapture);
|
||||||
|
std::cout << "Stream Capture Service started!\n";
|
||||||
|
ControlInit();
|
||||||
|
std::cout << "Control Initialized!\n";
|
||||||
|
|
||||||
|
Timer CameraTimer(dest_frame_duration, std::bind(CameraHandler));
|
||||||
|
Timer MortorTimer(8, std::bind(ControlMain));
|
||||||
|
CameraTimer.start();
|
||||||
|
std::cout << "Camera Service started!\n";
|
||||||
|
MortorTimer.start();
|
||||||
|
std::cout << "Control Service started!\n";
|
||||||
|
|
||||||
|
// 主循环，直到用户输入 Ctrl+C
|
||||||
|
while (running.load())
|
||||||
|
{
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||||
|
target_speed = readDoubleFromFile(speed_file);
|
||||||
|
|
||||||
|
mortor_kp = readDoubleFromFile(mortor_kp_file);
|
||||||
|
mortor_ki = readDoubleFromFile(mortor_ki_file);
|
||||||
|
mortor_kd = readDoubleFromFile(mortor_kd_file);
|
||||||
|
|
||||||
|
kp = readDoubleFromFile(kp_file);
|
||||||
|
ki = readDoubleFromFile(ki_file);
|
||||||
|
kd = readDoubleFromFile(kd_file);
|
||||||
|
// vofa_image(1, 160*120, 160, 120, Format_Grayscale8, (char*)IMG);
|
||||||
|
}
|
||||||
|
std::cout << "Stopping!\n";
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||||
|
|
||||||
|
CameraTimer.stop();
|
||||||
|
std::cout << "Camera Timer stopped!\n";
|
||||||
|
MortorTimer.stop();
|
||||||
|
std::cout << "Control Timer stopped!\n";
|
||||||
|
|
||||||
|
ControlExit();
|
||||||
|
std::cout << "Control Service stopped!\n";
|
||||||
|
|
||||||
|
streamCaptureRunning = false;
|
||||||
|
if (camworker.joinable())
|
||||||
|
{
|
||||||
|
camworker.join();
|
||||||
|
}
|
||||||
|
|
||||||
|
cameraDeInit();
|
||||||
|
std::cout << "Camera Service stopped!\n";
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# demo3
|
||||||
|
add_executable(opencv_demo1 opencv_demo1.cpp)
|
||||||
|
|
||||||
|
# 链接 OpenCV 库
|
||||||
|
target_link_libraries(opencv_demo1 ${OpenCV_LIBS})
|
||||||
@@ -0,0 +1,158 @@
|
|||||||
|
/*
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# demo2
|
||||||
|
add_executable(opencv_demo2 opencv_demo2.cpp)
|
||||||
|
|
||||||
|
# 链接 OpenCV 库
|
||||||
|
target_link_libraries(opencv_demo2 ${OpenCV_LIBS})
|
||||||
@@ -0,0 +1,222 @@
|
|||||||
|
/*
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# demo2
|
||||||
|
add_executable(opencv_demo3 opencv_demo3.cpp)
|
||||||
|
|
||||||
|
# 链接 OpenCV 库
|
||||||
|
target_link_libraries(opencv_demo3 ${OpenCV_LIBS})
|
||||||
@@ -0,0 +1,141 @@
|
|||||||
|
/*
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,95 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2024-10-10 15:02:10
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2024-12-01 03:50:49
|
||||||
|
* @FilePath: /smartcar/src/GPIO.cpp
|
||||||
|
* @Description:
|
||||||
|
*
|
||||||
|
* Copyright (c) 2024 by ${git_name_email}, All Rights Reserved.
|
||||||
|
*/
|
||||||
|
#include "GPIO.h"
|
||||||
|
|
||||||
|
GPIO::GPIO(int gpioNum) : gpioNum(gpioNum), fd(-1)
|
||||||
|
{
|
||||||
|
gpioPath = "/sys/class/gpio/gpio" + std::to_string(gpioNum);
|
||||||
|
|
||||||
|
writeToFile("/sys/class/gpio/export", std::to_string(gpioNum));
|
||||||
|
|
||||||
|
fd = open((gpioPath + "/value").c_str(), O_RDWR);
|
||||||
|
if (fd == -1)
|
||||||
|
{
|
||||||
|
throw std::runtime_error("Failed to open GPIO value file: " + std::string(strerror(errno)));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
GPIO::~GPIO()
|
||||||
|
{
|
||||||
|
if (fd != -1)
|
||||||
|
{
|
||||||
|
close(fd); // 关闭文件描述符
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool GPIO::setDirection(const std::string &direction)
|
||||||
|
{
|
||||||
|
return writeToFile(gpioPath + "/direction", direction);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool GPIO::setEdge(const std::string &edge)
|
||||||
|
{
|
||||||
|
return writeToFile(gpioPath + "/edge", edge);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool GPIO::setValue(bool value)
|
||||||
|
{
|
||||||
|
if (fd == -1)
|
||||||
|
{
|
||||||
|
std::cerr << "GPIO file descriptor is invalid" << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 使用文件描述符写入 GPIO 值 ('1' 或 '0')
|
||||||
|
const char *val_str = value ? "1" : "0";
|
||||||
|
if (write(fd, val_str, 1) != 1)
|
||||||
|
{
|
||||||
|
std::cerr << "Failed to write GPIO value: " << strerror(errno) << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool GPIO::readValue()
|
||||||
|
{
|
||||||
|
if (fd == -1)
|
||||||
|
{
|
||||||
|
std::cerr << "GPIO file descriptor is invalid" << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
char value;
|
||||||
|
lseek(fd, 0, SEEK_SET); // 重置文件偏移量
|
||||||
|
if (read(fd, &value, 1) != 1)
|
||||||
|
{
|
||||||
|
std::cerr << "Failed to read GPIO value: " << strerror(errno) << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return value == '1'; // 如果读取的值为 '1'，则返回 true，否则返回 false
|
||||||
|
}
|
||||||
|
|
||||||
|
int GPIO::getFileDescriptor() const
|
||||||
|
{
|
||||||
|
return fd;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool GPIO::writeToFile(const std::string &path, const std::string &value)
|
||||||
|
{
|
||||||
|
int fd = ::open(path.c_str(), O_WRONLY);
|
||||||
|
if (fd == -1)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
ssize_t n = ::write(fd, value.c_str(), value.size());
|
||||||
|
::close(fd);
|
||||||
|
return (n == static_cast<ssize_t>(value.size()));
|
||||||
|
}
|
||||||
@@ -0,0 +1,62 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2024-10-10 14:36:42
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2025-03-21 10:41:17
|
||||||
|
* @FilePath: /smartcar/src/MotorController.cpp
|
||||||
|
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||||
|
*/
|
||||||
|
#include "MotorController.h"
|
||||||
|
|
||||||
|
MotorController::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_)
|
||||||
|
: pwmController(pwmchip, pwmnum), directionGPIO(gpioNum), pidController(kp, ki, kd, targetSpeed, INCREMENTAL, 80),
|
||||||
|
encoder(encoder_pwmNum, encoder_gpioNum), encoder_dir(encoder_dir_)
|
||||||
|
{
|
||||||
|
pwmController.setPeriod(period_ns); // 设置 PWM 周期
|
||||||
|
directionGPIO.setDirection("out");
|
||||||
|
pwmController.enable(); // 启用 PWM
|
||||||
|
}
|
||||||
|
|
||||||
|
MotorController::~MotorController(void)
|
||||||
|
{
|
||||||
|
pwmController.disable();
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotorController::updateduty(double dutyCycle)
|
||||||
|
{
|
||||||
|
int newduty = pwmController.readPeriod() * abs(dutyCycle) / 100.0;
|
||||||
|
if (newduty != pwmController.readDutyCycle())
|
||||||
|
{
|
||||||
|
pwmController.setDutyCycle(newduty);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 根据 PID 输出设置 GPIO 的方向
|
||||||
|
if (dutyCycle > 0)
|
||||||
|
{
|
||||||
|
directionGPIO.setValue(1); // 正向
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
directionGPIO.setValue(0); // 反向
|
||||||
|
}
|
||||||
|
//std::cout << encoder.pulse_counter_update() << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotorController::updateSpeed(void)
|
||||||
|
{
|
||||||
|
double encoderReading = encoder.pulse_counter_update() * encoder_dir;
|
||||||
|
// std::cout << encoderReading << std::endl;
|
||||||
|
double output = pidController.update(encoderReading);
|
||||||
|
// int dutyCycle = static_cast<int>(output);
|
||||||
|
|
||||||
|
// 设置 PWM 占空比
|
||||||
|
updateduty(output);
|
||||||
|
std::cout << encoderReading << " " << output << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotorController::updateTarget(int speed)
|
||||||
|
{
|
||||||
|
pidController.setTarget(speed);
|
||||||
|
}
|
||||||
@@ -0,0 +1,94 @@
|
|||||||
|
#include "PIDController.h"
|
||||||
|
|
||||||
|
// 构造函数，初始化 PID 参数
|
||||||
|
PIDController::PIDController(double kp, double ki, double kd, double target, PIDMode mode,
|
||||||
|
double output_limit, double integral_limit)
|
||||||
|
: kp_(kp), ki_(ki), kd_(kd), target_(target),
|
||||||
|
prev_error_(0.0), prev_prev_error_(0.0), integral_(0.0),
|
||||||
|
mode_(mode), prev_output_(0.0), output_limit_(output_limit), integral_limit_(integral_limit) {}
|
||||||
|
|
||||||
|
// 更新 PID 控制器
|
||||||
|
double PIDController::update(double measured_value)
|
||||||
|
{
|
||||||
|
// 计算误差
|
||||||
|
double error = target_ - measured_value;
|
||||||
|
|
||||||
|
if (mode_ == POSITION)
|
||||||
|
{
|
||||||
|
// 位置式 PID 计算
|
||||||
|
return positionPID(error);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// 增量式 PID 计算
|
||||||
|
return incrementalPID(error);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 设置新的目标值
|
||||||
|
void PIDController::setTarget(double target)
|
||||||
|
{
|
||||||
|
target_ = target;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 设置 PID 参数
|
||||||
|
void PIDController::setPID(double kp, double ki, double kd)
|
||||||
|
{
|
||||||
|
kp_ = kp;
|
||||||
|
ki_ = ki;
|
||||||
|
kd_ = kd;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 设置 PID 控制模式（位置式或增量式）
|
||||||
|
void PIDController::setMode(PIDMode mode)
|
||||||
|
{
|
||||||
|
mode_ = mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 设置输出和积分的限幅值
|
||||||
|
void PIDController::setLimits(double output_limit, double integral_limit)
|
||||||
|
{
|
||||||
|
output_limit_ = output_limit;
|
||||||
|
integral_limit_ = integral_limit;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 位置式 PID 实现
|
||||||
|
double PIDController::positionPID(double error)
|
||||||
|
{
|
||||||
|
// 计算积分项，进行积分饱和处理
|
||||||
|
integral_ += error;
|
||||||
|
integral_ = std::clamp(integral_, -integral_limit_, integral_limit_);
|
||||||
|
|
||||||
|
// 计算微分项
|
||||||
|
double derivative = (error - prev_error_);
|
||||||
|
|
||||||
|
// 计算输出
|
||||||
|
double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
|
||||||
|
|
||||||
|
// 输出饱和处理
|
||||||
|
output = std::clamp(output, -output_limit_, output_limit_);
|
||||||
|
|
||||||
|
// 存储当前误差，用于下次计算
|
||||||
|
prev_error_ = error;
|
||||||
|
|
||||||
|
return output;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 增量式 PID 实现
|
||||||
|
double PIDController::incrementalPID(double error)
|
||||||
|
{
|
||||||
|
// 增量输出计算公式
|
||||||
|
double delta_output = kp_ * (error - prev_error_) + ki_ * error + kd_ * (error - 2 * prev_error_ + prev_prev_error_);
|
||||||
|
|
||||||
|
// 更新误差历史
|
||||||
|
prev_prev_error_ = prev_error_;
|
||||||
|
prev_error_ = error;
|
||||||
|
|
||||||
|
// 累加增量得到新的输出
|
||||||
|
prev_output_ += delta_output;
|
||||||
|
|
||||||
|
// 输出饱和处理
|
||||||
|
prev_output_ = std::clamp(prev_output_, -output_limit_, output_limit_);
|
||||||
|
|
||||||
|
return prev_output_;
|
||||||
|
}
|
||||||
@@ -0,0 +1,85 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2024-09-17 08:21:50
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2025-03-20 12:03:51
|
||||||
|
* @FilePath: /smartcar/src/PwmController.cpp
|
||||||
|
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||||
|
*/
|
||||||
|
#include "PwmController.h"
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
PwmController::PwmController(int pwmchip, int pwmnum, bool polarity)
|
||||||
|
: pwmchip(pwmchip), pwmnum(pwmnum)
|
||||||
|
{
|
||||||
|
initialize();
|
||||||
|
pwmPath = "/sys/class/pwm/pwmchip" + std::to_string(pwmchip) + "/pwm" + std::to_string(pwmnum) + "/";
|
||||||
|
setPolarity(polarity);
|
||||||
|
}
|
||||||
|
|
||||||
|
PwmController::~PwmController()
|
||||||
|
{
|
||||||
|
disable(); // 析构时自动禁用PWM
|
||||||
|
}
|
||||||
|
|
||||||
|
int PwmController::readPeriod()
|
||||||
|
{
|
||||||
|
return period;
|
||||||
|
}
|
||||||
|
|
||||||
|
int PwmController::readDutyCycle()
|
||||||
|
{
|
||||||
|
return duty_cycle;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 初始化PWM设备
|
||||||
|
bool PwmController::initialize()
|
||||||
|
{
|
||||||
|
std::string exportPath = "/sys/class/pwm/pwmchip" + std::to_string(pwmchip) + "/export";
|
||||||
|
return writeToFile(exportPath, std::to_string(pwmnum));
|
||||||
|
}
|
||||||
|
|
||||||
|
// 启用PWM
|
||||||
|
bool PwmController::enable()
|
||||||
|
{
|
||||||
|
return writeToFile(pwmPath + "enable", std::to_string(1));
|
||||||
|
}
|
||||||
|
|
||||||
|
// 禁用PWM
|
||||||
|
bool PwmController::disable()
|
||||||
|
{
|
||||||
|
return writeToFile(pwmPath + "enable", std::to_string(0));
|
||||||
|
}
|
||||||
|
|
||||||
|
// 设置周期（以纳秒为单位）
|
||||||
|
bool PwmController::setPeriod(unsigned int period_ns)
|
||||||
|
{
|
||||||
|
period = period_ns;
|
||||||
|
return writeToFile(pwmPath + "period", std::to_string(period_ns));
|
||||||
|
}
|
||||||
|
|
||||||
|
// 设置低电平时间（以纳秒为单位）
|
||||||
|
bool PwmController::setDutyCycle(unsigned int duty_cycle_ns)
|
||||||
|
{
|
||||||
|
duty_cycle = duty_cycle_ns;
|
||||||
|
return writeToFile(pwmPath + "duty_cycle", std::to_string(duty_cycle_ns));
|
||||||
|
}
|
||||||
|
|
||||||
|
// 设置极性
|
||||||
|
bool PwmController::setPolarity(bool polarity)
|
||||||
|
{
|
||||||
|
return writeToFile(pwmPath + "polarity", polarity ? "normal" : "inversed");
|
||||||
|
}
|
||||||
|
|
||||||
|
bool PwmController::writeToFile(const std::string &path, const std::string &value)
|
||||||
|
{
|
||||||
|
int fd = open(path.c_str(), O_WRONLY);
|
||||||
|
if (fd == -1)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
ssize_t n = write(fd, value.c_str(), value.size());
|
||||||
|
close(fd);
|
||||||
|
return (n == static_cast<ssize_t>(value.size()));
|
||||||
|
}
|
||||||
@@ -0,0 +1,36 @@
|
|||||||
|
#include "Timer.h"
|
||||||
|
|
||||||
|
Timer::Timer(int interval_ms, std::function<void()> task)
|
||||||
|
: interval(interval_ms), task(task), running(false) {}
|
||||||
|
|
||||||
|
Timer::~Timer()
|
||||||
|
{
|
||||||
|
stop(); // Ensure the timer is stopped in the destructor
|
||||||
|
}
|
||||||
|
|
||||||
|
void Timer::start()
|
||||||
|
{
|
||||||
|
running = true;
|
||||||
|
worker = std::thread(&Timer::run, this);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Timer::stop()
|
||||||
|
{
|
||||||
|
running = false;
|
||||||
|
if (worker.joinable())
|
||||||
|
{
|
||||||
|
worker.join();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Timer::run()
|
||||||
|
{
|
||||||
|
while (running)
|
||||||
|
{
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(interval));
|
||||||
|
if (running)
|
||||||
|
{
|
||||||
|
std::thread(task).detach();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
+284
@@ -0,0 +1,284 @@
|
|||||||
|
#include "camera.h"
|
||||||
|
|
||||||
|
cv::VideoCapture cap;
|
||||||
|
|
||||||
|
double kp = 0;
|
||||||
|
double ki = 0;
|
||||||
|
double kd = 0;
|
||||||
|
|
||||||
|
int screenWidth, screenHeight;
|
||||||
|
int newWidth, newHeight;
|
||||||
|
int fb;
|
||||||
|
// 创建帧缓冲区
|
||||||
|
uint16_t *fb_buffer;
|
||||||
|
PwmController servo(1, 0);
|
||||||
|
|
||||||
|
#define calc_scale 2
|
||||||
|
|
||||||
|
int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
|
||||||
|
{
|
||||||
|
servo.setPeriod(3000000);
|
||||||
|
servo.setDutyCycle(1500000);
|
||||||
|
servo.enable();
|
||||||
|
|
||||||
|
// 打开帧缓冲区设备
|
||||||
|
fb = open("/dev/fb0", O_RDWR);
|
||||||
|
if (fb == -1)
|
||||||
|
{
|
||||||
|
std::cerr << "无法打开帧缓冲区设备" << std::endl;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 获取帧缓冲区设备信息
|
||||||
|
struct fb_var_screeninfo vinfo;
|
||||||
|
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
|
||||||
|
{
|
||||||
|
std::cerr << "无法获取帧缓冲区信息" << std::endl;
|
||||||
|
close(fb);
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 动态设置屏幕分辨率
|
||||||
|
screenWidth = vinfo.xres;
|
||||||
|
screenHeight = vinfo.yres;
|
||||||
|
|
||||||
|
// 计算帧缓冲区大小
|
||||||
|
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
|
||||||
|
|
||||||
|
// 使用 mmap 映射帧缓冲区到内存
|
||||||
|
fb_buffer = (uint16_t *)mmap(NULL, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0);
|
||||||
|
if (fb_buffer == MAP_FAILED)
|
||||||
|
{
|
||||||
|
std::cerr << "无法映射帧缓冲区到内存" << std::endl;
|
||||||
|
close(fb);
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 打开默认摄像头（设备编号 0）
|
||||||
|
cap.open(0, cv::CAP_V4L2);
|
||||||
|
if (!cap.isOpened()) cap.open(0);
|
||||||
|
|
||||||
|
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'));
|
||||||
|
|
||||||
|
// 检查摄像头是否成功打开
|
||||||
|
if (!cap.isOpened())
|
||||||
|
{
|
||||||
|
printf("无法打开摄像头\n");
|
||||||
|
munmap(fb_buffer, fb_size);
|
||||||
|
close(fb);
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
cap.set(cv::CAP_PROP_FRAME_WIDTH, width); // 宽度
|
||||||
|
cap.set(cv::CAP_PROP_FRAME_HEIGHT, height); // 高度
|
||||||
|
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); // 视频流格式
|
||||||
|
cap.set(cv::CAP_PROP_AUTO_EXPOSURE, -1); // 设置自动曝光
|
||||||
|
|
||||||
|
// 获取摄像头实际分辨率
|
||||||
|
int cameraWidth = cap.get(cv::CAP_PROP_FRAME_WIDTH);
|
||||||
|
int cameraHeight = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
|
||||||
|
printf("摄像头分辨率: %d x %d\n", cameraWidth, cameraHeight);
|
||||||
|
|
||||||
|
// 计算 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);
|
||||||
|
printf("自适应分辨率: %d x %d\n", newWidth, newHeight);
|
||||||
|
|
||||||
|
// 计算帧率
|
||||||
|
double fps = cap.get(cv::CAP_PROP_FPS);
|
||||||
|
printf("Camera fps:%lf\n", fps);
|
||||||
|
|
||||||
|
line_tracking_width = newWidth / calc_scale;
|
||||||
|
line_tracking_height = newHeight / calc_scale;
|
||||||
|
|
||||||
|
// 计算每帧的延迟时间（ms）
|
||||||
|
return static_cast<int>(1000.0 / std::min(fps, dest_fps));
|
||||||
|
}
|
||||||
|
|
||||||
|
void cameraDeInit(void)
|
||||||
|
{
|
||||||
|
cap.release();
|
||||||
|
|
||||||
|
// 获取帧缓冲区设备信息
|
||||||
|
struct fb_var_screeninfo vinfo;
|
||||||
|
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
|
||||||
|
{
|
||||||
|
std::cerr << "无法获取帧缓冲区信息" << std::endl;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// 计算帧缓冲区大小
|
||||||
|
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
|
||||||
|
|
||||||
|
// 取消映射
|
||||||
|
munmap(fb_buffer, fb_size);
|
||||||
|
}
|
||||||
|
|
||||||
|
close(fb);
|
||||||
|
}
|
||||||
|
|
||||||
|
int saved_frame_count = 0;
|
||||||
|
bool saveCameraImage(cv::Mat frame, const std::string &directory)
|
||||||
|
{
|
||||||
|
if (frame.empty())
|
||||||
|
{
|
||||||
|
std::cerr << "Save Error: Frame is empty." << std::endl;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 构建文件名
|
||||||
|
std::ostringstream filename;
|
||||||
|
filename << directory << "/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg";
|
||||||
|
|
||||||
|
saved_frame_count++;
|
||||||
|
// 保存图像
|
||||||
|
return cv::imwrite(filename.str(), frame);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::mutex frameMutex;
|
||||||
|
cv::Mat pubframe;
|
||||||
|
bool streamCaptureRunning;
|
||||||
|
void streamCapture(void)
|
||||||
|
{
|
||||||
|
cv::Mat frame;
|
||||||
|
while (streamCaptureRunning)
|
||||||
|
{
|
||||||
|
cap.read(frame);
|
||||||
|
frameMutex.lock();
|
||||||
|
pubframe = frame;
|
||||||
|
frameMutex.unlock();
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ===================================================
|
||||||
|
// PIDController ServoControl(P=1.0, I=0, D=2.0, target=0, 位置式, 输出限幅=1,250,000)
|
||||||
|
// 输出单位: 百分之一脉宽周期 (÷100 × period_ns → ns)
|
||||||
|
// 实际等效线性增益: Kp=1.0 起主导, I=0 无积分, D=2.0 微分量抑制过冲
|
||||||
|
// ===================================================
|
||||||
|
double g_steer_deviation = 0; // 全局偏差, 供速度控制用
|
||||||
|
|
||||||
|
PIDController ServoControl(1.0, 0.0, 2.0, 0.0, POSITION, 1250000);
|
||||||
|
int CameraHandler(void)
|
||||||
|
{
|
||||||
|
cv::Mat resizedFrame;
|
||||||
|
|
||||||
|
frameMutex.lock();
|
||||||
|
raw_frame = pubframe;
|
||||||
|
frameMutex.unlock();
|
||||||
|
|
||||||
|
if (raw_frame.empty())
|
||||||
|
{
|
||||||
|
printf("无法捕获图像\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (readFlag(saveImg_file))
|
||||||
|
{
|
||||||
|
if (saveCameraImage(raw_frame, "./image"))
|
||||||
|
{
|
||||||
|
printf("图像%d已保存\n", saved_frame_count);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
printf("图像保存失败\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// ── 1. 视觉巡线 ──────────────────────────────────
|
||||||
|
// image_main() 处理 raw_frame → 80×60 图
|
||||||
|
// 产出: left_line[60], right_line[60], mid_line[60] (EMA 滤波后)
|
||||||
|
{ // 图像计算
|
||||||
|
image_main();
|
||||||
|
}
|
||||||
|
|
||||||
|
// ── 2. 舵机转向控制 ──────────────────────────────
|
||||||
|
// 仅在 start 文件为 1 时执行 (readFlag(start_file))
|
||||||
|
// 否则保持上一次的脉宽 (不做任何转向)
|
||||||
|
if (readFlag(start_file))
|
||||||
|
{
|
||||||
|
// 2a. 前瞻行号换算
|
||||||
|
// foresee 是 newWidth×newHeight (160×120) 坐标系下的行号
|
||||||
|
// calc_scale=2, 除以 2 得到 80×60 (line_tracking) 下的行号
|
||||||
|
int foresee = readDoubleFromFile(foresee_file);
|
||||||
|
int check_row = foresee / calc_scale;
|
||||||
|
|
||||||
|
// 2b. 单行偏差计算 (80×60 坐标系 → 160×120 像素偏差)
|
||||||
|
if (check_row >= 0 && check_row < line_tracking_height && mid_line[check_row] != 255)
|
||||||
|
{
|
||||||
|
double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
|
||||||
|
double bias = readDoubleFromFile(center_bias_file); // 中位偏置(像素), 正=偏右
|
||||||
|
deviation -= bias;
|
||||||
|
double norm = deviation / (newWidth / 2.0); // 归一化到 ±1
|
||||||
|
g_steer_deviation = norm; // 给速度控制用
|
||||||
|
|
||||||
|
// 2c. 死区 (像素)
|
||||||
|
double deadband = readDoubleFromFile(deadband_file);
|
||||||
|
if (std::abs(deviation) < deadband)
|
||||||
|
{
|
||||||
|
servo.setDutyCycle(1500000); // 中位 1.50ms
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// 2d. 比例转向: 像素偏差 → 归一化 → 舵机脉宽
|
||||||
|
double steer_gain = readDoubleFromFile(steer_gain_file);
|
||||||
|
double norm = deviation / (newWidth / 2.0); // 归一化到 ±1
|
||||||
|
double offset = norm * steer_gain * 300000; // 半行程 0.30ms
|
||||||
|
double duty_ns = 1500000.0 + offset;
|
||||||
|
duty_ns = std::clamp(duty_ns, 1200000.0, 1800000.0);
|
||||||
|
servo.setDutyCycle(static_cast<unsigned int>(duty_ns));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// servo.setDutyCycle(1520000);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 显示图片
|
||||||
|
if (readFlag(showImg_file))
|
||||||
|
{
|
||||||
|
cv::Mat fbImage(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0));
|
||||||
|
|
||||||
|
// 缩放视频到新尺寸
|
||||||
|
cv::resize(track, resizedFrame, cv::Size(newWidth, newHeight));
|
||||||
|
// 将单通道的二值化图像转换为三通道的彩色图像
|
||||||
|
cv::Mat coloredResizedFrame;
|
||||||
|
cv::cvtColor(resizedFrame, coloredResizedFrame, cv::COLOR_GRAY2BGR); // 转换为彩色图像
|
||||||
|
|
||||||
|
// 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
|
||||||
|
fbImage.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
|
||||||
|
cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
|
||||||
|
coloredResizedFrame.copyTo(fbImage(roi));
|
||||||
|
|
||||||
|
// 绘制左右边界线和中线
|
||||||
|
int scaledLeftX, scaledRightX, scaledMidX, scaledY;
|
||||||
|
|
||||||
|
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);
|
||||||
|
scaledY = static_cast<int>(y * calc_scale);
|
||||||
|
|
||||||
|
// 绘制左边界（红）
|
||||||
|
cv::line(fbImage(roi), cv::Point(scaledLeftX, scaledY), cv::Point(scaledLeftX, scaledY), cv::Scalar(0, 0, 255), calc_scale);
|
||||||
|
// 绘制右边界（绿）
|
||||||
|
cv::line(fbImage(roi), cv::Point(scaledRightX, scaledY), cv::Point(scaledRightX, scaledY), cv::Scalar(0, 255, 0), calc_scale);
|
||||||
|
// 绘制中线 （蓝）
|
||||||
|
cv::line(fbImage(roi), cv::Point(scaledMidX, scaledY), cv::Point(scaledMidX, scaledY), cv::Scalar(255, 0, 0), calc_scale);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 将帧缓冲区图像转换为RGB565格式
|
||||||
|
convertMatToRGB565(fbImage, fb_buffer, screenWidth, screenHeight);
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -0,0 +1,81 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2024-10-10 09:02:10
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2025-03-21 10:40:10
|
||||||
|
* @FilePath: /smartcar/src/control.cpp
|
||||||
|
* @Description:
|
||||||
|
*
|
||||||
|
* Copyright (c) 2024 by ${git_name_email}, All Rights Reserved.
|
||||||
|
*/
|
||||||
|
#include "control.h"
|
||||||
|
|
||||||
|
#include "GPIO.h"
|
||||||
|
extern double g_steer_deviation; // camera.cpp 输出的归一化偏差
|
||||||
|
|
||||||
|
MotorController *motorController[2] = {nullptr, nullptr};
|
||||||
|
GPIO mortorEN(73);
|
||||||
|
double mortor_kp = 1000;
|
||||||
|
double mortor_ki = 300;
|
||||||
|
double mortor_kd = 0;
|
||||||
|
|
||||||
|
void ControlInit()
|
||||||
|
{
|
||||||
|
mortorEN.setDirection("out");
|
||||||
|
mortorEN.setValue(1);
|
||||||
|
|
||||||
|
GPIO leftIn2(13);
|
||||||
|
leftIn2.setDirection("out");
|
||||||
|
leftIn2.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,
|
||||||
|
mortor_kp, mortor_ki, mortor_kd, 0,
|
||||||
|
encoder_pwmchip[i], encoder_gpioNum[i], encoder_dir[i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ControlMain()
|
||||||
|
{
|
||||||
|
if (readFlag(start_file))
|
||||||
|
{
|
||||||
|
// 弯道减速: 偏差大→速度低, 最低 30%
|
||||||
|
double curve = 1.0 - std::abs(g_steer_deviation) * 0.7;
|
||||||
|
if (curve < 0.3) curve = 0.3;
|
||||||
|
double spd = target_speed * curve;
|
||||||
|
|
||||||
|
for (int i = 0; i < 2; ++i)
|
||||||
|
{
|
||||||
|
motorController[i]->updateduty(spd);
|
||||||
|
}
|
||||||
|
mortorEN.setValue(1);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
for (int i = 0; i < 2; ++i)
|
||||||
|
{
|
||||||
|
motorController[i]->updateduty(0);
|
||||||
|
}
|
||||||
|
mortorEN.setValue(0);
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ControlExit()
|
||||||
|
{
|
||||||
|
for (int i = 0; i < 2; ++i)
|
||||||
|
{
|
||||||
|
delete motorController[i];
|
||||||
|
std::cout << "motor" << i << "deleted\n";
|
||||||
|
}
|
||||||
|
mortorEN.setValue(0);
|
||||||
|
}
|
||||||
@@ -0,0 +1,82 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2024-10-11 06:19:57
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2024-12-01 03:54:06
|
||||||
|
* @FilePath: /smartcar/src/encoder.cpp
|
||||||
|
* @Description:
|
||||||
|
*
|
||||||
|
* Copyright (c) 2024 by ilikara 3435193369@qq.com, All Rights Reserved.
|
||||||
|
*/
|
||||||
|
#include "encoder.h"
|
||||||
|
|
||||||
|
ENCODER::ENCODER(int pwmNum, int gpioNum) : base_addr(PWM_BASE_ADDR + pwmNum * PWM_OFFSET), directionGPIO(gpioNum)
|
||||||
|
{
|
||||||
|
directionGPIO.setDirection("in");
|
||||||
|
|
||||||
|
control_buffer = map_register(base_addr + CONTROL_REG_OFFSET, PAGE_SIZE);
|
||||||
|
low_buffer = map_register(base_addr + LOW_BUFFER_OFFSET, PAGE_SIZE);
|
||||||
|
full_buffer = map_register(base_addr + FULL_BUFFER_OFFSET, PAGE_SIZE);
|
||||||
|
|
||||||
|
printf("Registers mapped successfully\n");
|
||||||
|
|
||||||
|
PWM_Init();
|
||||||
|
}
|
||||||
|
|
||||||
|
ENCODER::~ENCODER()
|
||||||
|
{
|
||||||
|
directionGPIO.~GPIO();
|
||||||
|
munmap(control_buffer, PAGE_SIZE);
|
||||||
|
munmap(low_buffer, PAGE_SIZE);
|
||||||
|
munmap(full_buffer, PAGE_SIZE);
|
||||||
|
}
|
||||||
|
|
||||||
|
void *ENCODER::map_register(uint32_t physical_address, size_t size)
|
||||||
|
{
|
||||||
|
int mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
|
||||||
|
if (mem_fd == -1)
|
||||||
|
{
|
||||||
|
perror("Failed to open /dev/mem");
|
||||||
|
exit(EXIT_FAILURE);
|
||||||
|
}
|
||||||
|
|
||||||
|
void *mapped_addr = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, physical_address & ~(PAGE_SIZE - 1));
|
||||||
|
if (mapped_addr == MAP_FAILED)
|
||||||
|
{
|
||||||
|
perror("Failed to map memory");
|
||||||
|
close(mem_fd);
|
||||||
|
exit(EXIT_FAILURE);
|
||||||
|
}
|
||||||
|
|
||||||
|
close(mem_fd);
|
||||||
|
|
||||||
|
return (void *)((uintptr_t)mapped_addr + (physical_address & (PAGE_SIZE - 1)));
|
||||||
|
}
|
||||||
|
|
||||||
|
void ENCODER::PWM_Init(void)
|
||||||
|
{
|
||||||
|
uint32_t control_reg = 0;
|
||||||
|
|
||||||
|
control_reg |= CNTR_ENABLE_BIT;
|
||||||
|
control_reg |= MEASURE_PULSE_BIT;
|
||||||
|
control_reg |= INT_ENABLE_BIT;
|
||||||
|
|
||||||
|
REG_WRITE(control_buffer, control_reg);
|
||||||
|
|
||||||
|
printf("PWM initialized with control register: 0x%08X\n", control_reg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ENCODER::reset_counter(void)
|
||||||
|
{
|
||||||
|
uint32_t control_reg = REG_READ(control_buffer);
|
||||||
|
control_reg |= COUNTER_RESET_BIT;
|
||||||
|
REG_WRITE(control_buffer, control_reg);
|
||||||
|
}
|
||||||
|
|
||||||
|
double ENCODER::pulse_counter_update(void)
|
||||||
|
{
|
||||||
|
double value = 100000000.0 / REG_READ(full_buffer) / 1024.0 * (directionGPIO.readValue() * 2 - 1);
|
||||||
|
// reset_counter();
|
||||||
|
// printf("Encoder RPS: %8.1lf\n", 100000000.0 / REG_READ(full_buffer) / 1024.0 * (gpio_value * 2 - 1));
|
||||||
|
return value;
|
||||||
|
}
|
||||||
@@ -0,0 +1,20 @@
|
|||||||
|
#include "frame_buffer.h"
|
||||||
|
|
||||||
|
// 将RGB转换为RGB565格式
|
||||||
|
uint16_t convertRGBToRGB565(uint8_t r, uint8_t g, uint8_t b)
|
||||||
|
{
|
||||||
|
return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 将Mat图像转换为RGB565格式
|
||||||
|
void convertMatToRGB565(const cv::Mat &frame, uint16_t *buffer, int width, int height)
|
||||||
|
{
|
||||||
|
for (int y = 0; y < height; y++)
|
||||||
|
{
|
||||||
|
for (int x = 0; x < width; x++)
|
||||||
|
{
|
||||||
|
cv::Vec3b color = frame.at<cv::Vec3b>(y, x);
|
||||||
|
buffer[y * width + x] = convertRGBToRGB565(color[2], color[1], color[0]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,37 @@
|
|||||||
|
#include "global.h"
|
||||||
|
|
||||||
|
double target_speed;
|
||||||
|
|
||||||
|
// 从文件读取双精度值
|
||||||
|
double readDoubleFromFile(const std::string &filename)
|
||||||
|
{
|
||||||
|
std::ifstream file(filename);
|
||||||
|
double value = 0.0;
|
||||||
|
if (file.is_open())
|
||||||
|
{
|
||||||
|
file >> value; // 读取文件中的值
|
||||||
|
file.close();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
std::cerr << "Failed to open " << filename << std::endl;
|
||||||
|
}
|
||||||
|
return value;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 从文件中读取标志
|
||||||
|
bool readFlag(const std::string &filename)
|
||||||
|
{
|
||||||
|
std::ifstream file(filename);
|
||||||
|
int flag = 0;
|
||||||
|
if (file.is_open())
|
||||||
|
{
|
||||||
|
file >> flag; // 读取文件中的更新标志
|
||||||
|
file.close();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
std::cerr << "Failed to open " << filename << std::endl;
|
||||||
|
}
|
||||||
|
return flag;
|
||||||
|
}
|
||||||
@@ -0,0 +1,177 @@
|
|||||||
|
/*
|
||||||
|
* @Author: ilikara 3435193369@qq.com
|
||||||
|
* @Date: 2025-01-04 06:50:56
|
||||||
|
* @LastEditors: ilikara 3435193369@qq.com
|
||||||
|
* @LastEditTime: 2025-03-13 08:15:10
|
||||||
|
* @FilePath: /2k300_smartcar/src/image_cv.cpp
|
||||||
|
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||||
|
*/
|
||||||
|
#include "image_cv.h"
|
||||||
|
|
||||||
|
cv::Mat raw_frame;
|
||||||
|
cv::Mat grayFrame;
|
||||||
|
cv::Mat binarizedFrame;
|
||||||
|
cv::Mat morphologyExFrame;
|
||||||
|
cv::Mat track;
|
||||||
|
|
||||||
|
std::vector<int> left_line; // 左边缘列号数组
|
||||||
|
std::vector<int> right_line; // 右边缘列号数组
|
||||||
|
std::vector<int> mid_line; // 中线列号数组
|
||||||
|
std::vector<double> left_line_filtered; // 中线列号数组
|
||||||
|
std::vector<double> right_line_filtered; // 中线列号数组
|
||||||
|
std::vector<double> mid_line_filtered; // 中线列号数组
|
||||||
|
|
||||||
|
int line_tracking_width;
|
||||||
|
int line_tracking_height;
|
||||||
|
|
||||||
|
cv::Mat image_binerize(cv::Mat &frame)
|
||||||
|
{
|
||||||
|
cv::Mat output;
|
||||||
|
cv::Mat binarizedFrame;
|
||||||
|
cv::Mat hsvImage;
|
||||||
|
cv::cvtColor(frame, hsvImage, cv::COLOR_BGR2HSV);
|
||||||
|
|
||||||
|
std::vector<cv::Mat> hsvChannels;
|
||||||
|
cv::split(hsvImage, hsvChannels);
|
||||||
|
|
||||||
|
cv::threshold(hsvChannels[0], binarizedFrame, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
|
||||||
|
cv::threshold(hsvChannels[1], output, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
|
||||||
|
|
||||||
|
cv::bitwise_or(output, binarizedFrame, output);
|
||||||
|
|
||||||
|
return output;
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat find_road(cv::Mat &frame)
|
||||||
|
{
|
||||||
|
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2));
|
||||||
|
cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel);
|
||||||
|
|
||||||
|
cv::Mat mask = cv::Mat::zeros(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1);
|
||||||
|
|
||||||
|
cv::Point seedPoint(line_tracking_width / 2, line_tracking_height - 10);
|
||||||
|
|
||||||
|
cv::circle(morphologyExFrame, seedPoint, 10, 255, -1);
|
||||||
|
|
||||||
|
cv::Scalar newVal(128);
|
||||||
|
|
||||||
|
cv::Scalar loDiff = cv::Scalar(20);
|
||||||
|
cv::Scalar upDiff = cv::Scalar(20);
|
||||||
|
|
||||||
|
cv::floodFill(morphologyExFrame, mask, seedPoint, newVal, 0, loDiff, upDiff, 8);
|
||||||
|
|
||||||
|
cv::Mat outputImage = cv::Mat::zeros(line_tracking_width, line_tracking_height, CV_8UC1);
|
||||||
|
|
||||||
|
mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage);
|
||||||
|
|
||||||
|
return outputImage;
|
||||||
|
}
|
||||||
|
|
||||||
|
void image_main()
|
||||||
|
{
|
||||||
|
cv::Mat resizedFrame;
|
||||||
|
|
||||||
|
cv::resize(raw_frame, resizedFrame, cv::Size(line_tracking_width, line_tracking_height));
|
||||||
|
|
||||||
|
binarizedFrame = image_binerize(resizedFrame);
|
||||||
|
|
||||||
|
track = find_road(binarizedFrame);
|
||||||
|
|
||||||
|
left_line.clear();
|
||||||
|
right_line.clear();
|
||||||
|
mid_line.clear();
|
||||||
|
left_line_filtered.clear();
|
||||||
|
right_line_filtered.clear();
|
||||||
|
mid_line_filtered.clear();
|
||||||
|
|
||||||
|
left_line.resize(line_tracking_height, -1);
|
||||||
|
right_line.resize(line_tracking_height, -1);
|
||||||
|
mid_line.resize(line_tracking_height, -1);
|
||||||
|
left_line_filtered.resize(line_tracking_height, -1);
|
||||||
|
right_line_filtered.resize(line_tracking_height, -1);
|
||||||
|
mid_line_filtered.resize(line_tracking_height, -1);
|
||||||
|
|
||||||
|
uchar(*IMG)[line_tracking_width] = reinterpret_cast<uchar(*)[line_tracking_width]>(track.data);
|
||||||
|
|
||||||
|
for (int i = 0; i < line_tracking_height; ++i)
|
||||||
|
{
|
||||||
|
int max_start = -1;
|
||||||
|
int max_end = -1;
|
||||||
|
int current_start = -1;
|
||||||
|
int current_length = 0;
|
||||||
|
int max_length = 0;
|
||||||
|
|
||||||
|
for (int j = 0; j < line_tracking_width; ++j)
|
||||||
|
{
|
||||||
|
if (IMG[i][j])
|
||||||
|
{
|
||||||
|
if (current_length == 0)
|
||||||
|
{
|
||||||
|
current_start = j;
|
||||||
|
current_length = 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
current_length++;
|
||||||
|
}
|
||||||
|
if (current_length >= max_length)
|
||||||
|
{
|
||||||
|
max_length = current_length;
|
||||||
|
max_start = current_start;
|
||||||
|
max_end = j;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
current_length = 0;
|
||||||
|
current_start = -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (max_length > 0)
|
||||||
|
{
|
||||||
|
left_line[i] = max_start;
|
||||||
|
right_line[i] = max_end;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
left_line[i] = -1;
|
||||||
|
right_line[i] = -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
double a = 0.4;
|
||||||
|
for (int row = line_tracking_height - 1; row >= 10; --row)
|
||||||
|
{
|
||||||
|
if (left_line[row] == -1 && right_line[row] == -1)
|
||||||
|
{
|
||||||
|
mid_line[row] = mid_line[row + 1];
|
||||||
|
if (mid_line[row] > line_tracking_width / 2)
|
||||||
|
{
|
||||||
|
right_line[row] = line_tracking_width - 1;
|
||||||
|
left_line[row] = mid_line[row + 1];
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
left_line[row] = 0;
|
||||||
|
right_line[row] = mid_line[row + 1];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
mid_line[row] = (left_line[row] + right_line[row]) / 2;
|
||||||
|
}
|
||||||
|
if (row == line_tracking_height - 1)
|
||||||
|
{
|
||||||
|
left_line_filtered[row] = left_line[row];
|
||||||
|
right_line_filtered[row] = right_line[row];
|
||||||
|
mid_line_filtered[row] = mid_line[row];
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
left_line_filtered[row] = a * left_line[row] + (1 - a) * left_line_filtered[row + 1];
|
||||||
|
right_line_filtered[row] = a * right_line[row] + (1 - a) * right_line_filtered[row + 1];
|
||||||
|
// mid_line_filtered[row] = a * mid_line[row] + (1 - a) * mid_line_filtered[row + 1];
|
||||||
|
mid_line_filtered[row] = (left_line_filtered[row] + right_line_filtered[row]) / 2.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,40 @@
|
|||||||
|
#include "serial.h"
|
||||||
|
|
||||||
|
char vofa_buffer[64];
|
||||||
|
|
||||||
|
bool vofa_justfloat(int CH_count)
|
||||||
|
{
|
||||||
|
const unsigned char tail[4]{0x00, 0x00, 0x80, 0x7f};
|
||||||
|
std::string path = "/dev/ttyS0";
|
||||||
|
std::ofstream file(path);
|
||||||
|
if (!file.is_open())
|
||||||
|
{
|
||||||
|
std::cerr << "Error opening file: " << path << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
file.write((char *)vofa_buffer, CH_count * 4);
|
||||||
|
file.write((char *)tail, sizeof(tail));
|
||||||
|
file.close();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool vofa_image(int IMG_ID, int IMG_SIZE, int IMG_WIDTH, int IMG_HEIGHT, ImgFormat IMG_FORMAT, char *image)
|
||||||
|
{
|
||||||
|
int preFrame[7] = {0, 0, 0, 0, 0, 0x7F800000, 0x7F800000};
|
||||||
|
preFrame[0] = IMG_ID; // 此ID用于标识不同图片通道
|
||||||
|
preFrame[1] = IMG_SIZE; // 图片数据大小
|
||||||
|
preFrame[2] = IMG_WIDTH; // 图片宽度
|
||||||
|
preFrame[3] = IMG_HEIGHT; // 图片高度
|
||||||
|
preFrame[4] = IMG_FORMAT; // 图片格式
|
||||||
|
std::string path = "/dev/ttyS0";
|
||||||
|
std::ofstream file(path);
|
||||||
|
if (!file.is_open())
|
||||||
|
{
|
||||||
|
std::cerr << "Error opening file: " << path << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
file.write((char *)preFrame, sizeof(preFrame));
|
||||||
|
file.write((char *)image, IMG_SIZE);
|
||||||
|
file.close();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
@@ -0,0 +1,20 @@
|
|||||||
|
#include "video.h"
|
||||||
|
|
||||||
|
Video::Video(const std::string &filename, double fps)
|
||||||
|
: timer(static_cast<int>(1000 / fps), std::bind(&Video::streamCapture, this)), cap(filename)
|
||||||
|
{
|
||||||
|
streamCapture();
|
||||||
|
}
|
||||||
|
|
||||||
|
Video::~Video(void)
|
||||||
|
{
|
||||||
|
timer.stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Video::streamCapture(void)
|
||||||
|
{
|
||||||
|
frameMutex.lock();
|
||||||
|
cap.read(frame);
|
||||||
|
frameMutex.unlock();
|
||||||
|
}
|
||||||
|
|
||||||
@@ -0,0 +1,72 @@
|
|||||||
|
#include "vl53l0x.h"
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <cstring>
|
||||||
|
#include <cerrno>
|
||||||
|
#include <cstdio>
|
||||||
|
|
||||||
|
#define VL53L0X_IOCTL_INIT _IO('p', 0x01)
|
||||||
|
#define VL53L0X_IOCTL_STOP _IO('p', 0x05)
|
||||||
|
#define VL53L0X_IOCTL_GETDATA _IOR('p', 0x0b, VL53L0X_RangingMeasurementData_t)
|
||||||
|
|
||||||
|
VL53L0X::VL53L0X() : fd(-1) {}
|
||||||
|
|
||||||
|
VL53L0X::~VL53L0X()
|
||||||
|
{
|
||||||
|
if (fd > 0)
|
||||||
|
{
|
||||||
|
stop();
|
||||||
|
close(fd);
|
||||||
|
fd = -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool VL53L0X::init()
|
||||||
|
{
|
||||||
|
fd = open("/dev/stmvl53l0x_ranging", O_RDWR | O_SYNC);
|
||||||
|
if (fd <= 0)
|
||||||
|
{
|
||||||
|
fprintf(stderr, "[VL53L0X] open failed: %s\n", strerror(errno));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
ioctl(fd, VL53L0X_IOCTL_STOP, nullptr);
|
||||||
|
|
||||||
|
if (ioctl(fd, VL53L0X_IOCTL_INIT, nullptr) < 0)
|
||||||
|
{
|
||||||
|
fprintf(stderr, "[VL53L0X] init failed: %s\n", strerror(errno));
|
||||||
|
close(fd);
|
||||||
|
fd = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
fprintf(stderr, "[VL53L0X] init OK\n");
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool VL53L0X::readRange(VL53L0X_RangingMeasurementData_t &data)
|
||||||
|
{
|
||||||
|
if (fd <= 0)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
if (ioctl(fd, VL53L0X_IOCTL_GETDATA, &data) < 0)
|
||||||
|
{
|
||||||
|
fprintf(stderr, "[VL53L0X] read failed: %s\n", strerror(errno));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool VL53L0X::stop()
|
||||||
|
{
|
||||||
|
if (fd <= 0)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
if (ioctl(fd, VL53L0X_IOCTL_STOP, nullptr) < 0)
|
||||||
|
{
|
||||||
|
fprintf(stderr, "[VL53L0X] stop failed: %s\n", strerror(errno));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
# SmartCar Demo 启动脚本 (带 GPIO 修复)
|
||||||
|
DIR="$(dirname "$0")"
|
||||||
|
export LD_PRELOAD="$DIR/gpio_fix_final.so"
|
||||||
|
exec "$DIR/smartcar_demo"
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# demo2
|
||||||
|
add_executable(udp_receive udp_receive.cpp)
|
||||||
|
|
||||||
|
# 链接 OpenCV 库
|
||||||
|
target_link_libraries(udp_receive common_lib ${OpenCV_LIBS})
|
||||||
@@ -0,0 +1,104 @@
|
|||||||
|
'''
|
||||||
|
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()
|
||||||
@@ -0,0 +1,175 @@
|
|||||||
|
/*
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,10 @@
|
|||||||
|
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)
|
||||||
@@ -0,0 +1,44 @@
|
|||||||
|
/*
|
||||||
|
* 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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,32 @@
|
|||||||
|
#!/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
|
||||||
@@ -0,0 +1,36 @@
|
|||||||
|
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)
|
||||||
|
|
||||||
@@ -0,0 +1,15 @@
|
|||||||
|
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
|
||||||
|
|
||||||
@@ -0,0 +1,640 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
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_ */
|
||||||
@@ -0,0 +1,257 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
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_ */
|
||||||
|
|
||||||
|
|
||||||
Binary file not shown.
@@ -0,0 +1,101 @@
|
|||||||
|
|
||||||
|
|
||||||
|
#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.
@@ -0,0 +1,88 @@
|
|||||||
|
|
||||||
|
|
||||||
|
#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.
@@ -0,0 +1,389 @@
|
|||||||
|
|
||||||
|
|
||||||
|
#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.
@@ -0,0 +1,69 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
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_ */
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
# demo2
|
||||||
|
add_executable(wonderEcho_demo wonderEcho_demo.cpp)
|
||||||
|
|
||||||
|
# 链接 OpenCV 库
|
||||||
|
target_link_libraries(wonderEcho_demo)
|
||||||
@@ -0,0 +1,157 @@
|
|||||||
|
/*
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user