From b6b631cef3a6be667b8d29725b557f524528228f Mon Sep 17 00:00:00 2001 From: spdis Date: Thu, 28 May 2026 15:24:33 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=9D=E5=A7=8B=E6=8F=90=E4=BA=A4=EF=BC=9A?= =?UTF-8?q?=E9=BE=99=E8=8A=AF2K0300=E6=99=BA=E8=83=BD=E8=BD=A6=E5=8D=96?= =?UTF-8?q?=E5=AE=B6Demo=E5=AE=8C=E6=95=B4=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 14 + 2K0300智能车项目代码文档.md | 172 +++++++ CMakeLists.txt | 46 ++ README.md | 256 +++++++++++ cross.cmake | 17 + ctl.sh | 88 ++++ demo1/CMakeLists.txt | 5 + demo1/demo1.cpp | 125 +++++ encoder_demo/CMakeLists.txt | 5 + encoder_demo/encoder_demo.cpp | 22 + framebuffer_demo/CMakeLists.txt | 5 + framebuffer_demo/framebuffer_demo.cpp | 207 +++++++++ image_test/CMakeLists.txt | 5 + image_test/image_test.cpp | 420 +++++++++++++++++ jy62_demo/CMakeLists.txt | 5 + jy62_demo/jy62_demo.cpp | 182 ++++++++ key_demo/CMakeLists.txt | 5 + key_demo/key_demo.cpp | 68 +++ lib/GPIO.h | 44 ++ lib/MotorController.h | 37 ++ lib/PIDController.h | 57 +++ lib/PwmController.h | 40 ++ lib/Timer.h | 28 ++ lib/camera.h | 44 ++ lib/control.h | 18 + lib/encoder.h | 75 +++ lib/frame_buffer.h | 8 + lib/global.h | 37 ++ lib/image_cv.h | 28 ++ lib/serial.h | 61 +++ lib/video.h | 35 ++ lib/vl53l0x.h | 22 + lib/vl53l0x_def.h | 640 ++++++++++++++++++++++++++ lib/vl53l0x_device.h | 257 +++++++++++ lib/vl53l0x_types.h | 69 +++ main/CMakeLists.txt | 4 + main/main.cpp | 149 ++++++ opencv_demo1/CMakeLists.txt | 5 + opencv_demo1/opencv_demo1.cpp | 158 +++++++ opencv_demo2/CMakeLists.txt | 5 + opencv_demo2/opencv_demo2.cpp | 222 +++++++++ opencv_demo3/CMakeLists.txt | 5 + opencv_demo3/opencv_demo3.cpp | 141 ++++++ src/GPIO.cpp | 95 ++++ src/MotorController.cpp | 62 +++ src/PIDController.cpp | 94 ++++ src/PwmController.cpp | 85 ++++ src/Timer.cpp | 36 ++ src/camera.cpp | 284 ++++++++++++ src/control.cpp | 81 ++++ src/encoder.cpp | 82 ++++ src/frame_buffer.cpp | 20 + src/global.cpp | 37 ++ src/image_cv.cpp | 177 +++++++ src/serial.cpp | 40 ++ src/video.cpp | 20 + src/vl53l0x.cpp | 72 +++ start.sh | 5 + udp_receive/CMakeLists.txt | 5 + udp_receive/slider.py | 104 +++++ udp_receive/udp_receive.cpp | 175 +++++++ vl53l0x_demo/CMakeLists.txt | 10 + vl53l0x_demo/vl53l0x_demo.cpp | 44 ++ vl53l0x_setup.sh | 32 ++ vl53l0x_test/Android.mk | 36 ++ vl53l0x_test/Makefile | 15 + vl53l0x_test/vl53l0x_def.h | 640 ++++++++++++++++++++++++++ vl53l0x_test/vl53l0x_device.h | 257 +++++++++++ vl53l0x_test/vl53l0x_parameter | Bin 0 -> 20920 bytes vl53l0x_test/vl53l0x_parameter.c | 101 ++++ vl53l0x_test/vl53l0x_parameter.o | Bin 0 -> 8464 bytes vl53l0x_test/vl53l0x_reg | Bin 0 -> 20912 bytes vl53l0x_test/vl53l0x_reg.c | 88 ++++ vl53l0x_test/vl53l0x_reg.o | Bin 0 -> 8688 bytes vl53l0x_test/vl53l0x_test | Bin 0 -> 21184 bytes vl53l0x_test/vl53l0x_test.c | 389 ++++++++++++++++ vl53l0x_test/vl53l0x_test.o | Bin 0 -> 47784 bytes vl53l0x_test/vl53l0x_types.h | 69 +++ wonderEcho_demo/CMakeLists.txt | 5 + wonderEcho_demo/wonderEcho_demo.cpp | 157 +++++++ 80 files changed, 7153 insertions(+) create mode 100644 .gitignore create mode 100644 2K0300智能车项目代码文档.md create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 cross.cmake create mode 100644 ctl.sh create mode 100644 demo1/CMakeLists.txt create mode 100644 demo1/demo1.cpp create mode 100644 encoder_demo/CMakeLists.txt create mode 100644 encoder_demo/encoder_demo.cpp create mode 100644 framebuffer_demo/CMakeLists.txt create mode 100644 framebuffer_demo/framebuffer_demo.cpp create mode 100644 image_test/CMakeLists.txt create mode 100644 image_test/image_test.cpp create mode 100644 jy62_demo/CMakeLists.txt create mode 100644 jy62_demo/jy62_demo.cpp create mode 100644 key_demo/CMakeLists.txt create mode 100644 key_demo/key_demo.cpp create mode 100644 lib/GPIO.h create mode 100644 lib/MotorController.h create mode 100644 lib/PIDController.h create mode 100644 lib/PwmController.h create mode 100644 lib/Timer.h create mode 100644 lib/camera.h create mode 100644 lib/control.h create mode 100644 lib/encoder.h create mode 100644 lib/frame_buffer.h create mode 100644 lib/global.h create mode 100644 lib/image_cv.h create mode 100644 lib/serial.h create mode 100644 lib/video.h create mode 100644 lib/vl53l0x.h create mode 100644 lib/vl53l0x_def.h create mode 100644 lib/vl53l0x_device.h create mode 100644 lib/vl53l0x_types.h create mode 100644 main/CMakeLists.txt create mode 100644 main/main.cpp create mode 100644 opencv_demo1/CMakeLists.txt create mode 100644 opencv_demo1/opencv_demo1.cpp create mode 100644 opencv_demo2/CMakeLists.txt create mode 100644 opencv_demo2/opencv_demo2.cpp create mode 100644 opencv_demo3/CMakeLists.txt create mode 100644 opencv_demo3/opencv_demo3.cpp create mode 100644 src/GPIO.cpp create mode 100644 src/MotorController.cpp create mode 100644 src/PIDController.cpp create mode 100644 src/PwmController.cpp create mode 100644 src/Timer.cpp create mode 100644 src/camera.cpp create mode 100644 src/control.cpp create mode 100644 src/encoder.cpp create mode 100644 src/frame_buffer.cpp create mode 100644 src/global.cpp create mode 100644 src/image_cv.cpp create mode 100644 src/serial.cpp create mode 100644 src/video.cpp create mode 100644 src/vl53l0x.cpp create mode 100644 start.sh create mode 100644 udp_receive/CMakeLists.txt create mode 100644 udp_receive/slider.py create mode 100644 udp_receive/udp_receive.cpp create mode 100644 vl53l0x_demo/CMakeLists.txt create mode 100644 vl53l0x_demo/vl53l0x_demo.cpp create mode 100644 vl53l0x_setup.sh create mode 100644 vl53l0x_test/Android.mk create mode 100644 vl53l0x_test/Makefile create mode 100644 vl53l0x_test/vl53l0x_def.h create mode 100644 vl53l0x_test/vl53l0x_device.h create mode 100644 vl53l0x_test/vl53l0x_parameter create mode 100644 vl53l0x_test/vl53l0x_parameter.c create mode 100644 vl53l0x_test/vl53l0x_parameter.o create mode 100644 vl53l0x_test/vl53l0x_reg create mode 100644 vl53l0x_test/vl53l0x_reg.c create mode 100644 vl53l0x_test/vl53l0x_reg.o create mode 100644 vl53l0x_test/vl53l0x_test create mode 100644 vl53l0x_test/vl53l0x_test.c create mode 100644 vl53l0x_test/vl53l0x_test.o create mode 100644 vl53l0x_test/vl53l0x_types.h create mode 100644 wonderEcho_demo/CMakeLists.txt create mode 100644 wonderEcho_demo/wonderEcho_demo.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c38ff99 --- /dev/null +++ b/.gitignore @@ -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/ diff --git a/2K0300智能车项目代码文档.md b/2K0300智能车项目代码文档.md new file mode 100644 index 0000000..f79acc3 --- /dev/null +++ b/2K0300智能车项目代码文档.md @@ -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 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 +``` \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..5c90dd5 --- /dev/null +++ b/CMakeLists.txt @@ -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) \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..f67b82e --- /dev/null +++ b/README.md @@ -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` | diff --git a/cross.cmake b/cross.cmake new file mode 100644 index 0000000..2bae9f1 --- /dev/null +++ b/cross.cmake @@ -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) diff --git a/ctl.sh b/ctl.sh new file mode 100644 index 0000000..eefbcf1 --- /dev/null +++ b/ctl.sh @@ -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 diff --git a/demo1/CMakeLists.txt b/demo1/CMakeLists.txt new file mode 100644 index 0000000..843c3c9 --- /dev/null +++ b/demo1/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo1 +add_executable(demo1 demo1.cpp) + +# 链接 OpenCV 库 +target_link_libraries(demo1 common_lib ${OpenCV_LIBS}) \ No newline at end of file diff --git a/demo1/demo1.cpp b/demo1/demo1.cpp new file mode 100644 index 0000000..15c0e82 --- /dev/null +++ b/demo1/demo1.cpp @@ -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 +#include +#include +#include +#include + +#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 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; +} \ No newline at end of file diff --git a/encoder_demo/CMakeLists.txt b/encoder_demo/CMakeLists.txt new file mode 100644 index 0000000..787d347 --- /dev/null +++ b/encoder_demo/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo1 +add_executable(encoder_demo encoder_demo.cpp) + +# 链接 OpenCV 库 +target_link_libraries(encoder_demo common_lib ${OpenCV_LIBS}) \ No newline at end of file diff --git a/encoder_demo/encoder_demo.cpp b/encoder_demo/encoder_demo.cpp new file mode 100644 index 0000000..43783b5 --- /dev/null +++ b/encoder_demo/encoder_demo.cpp @@ -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 +int main() +{ + ENCODER encoder(0, 73); + while (1) + { + std::cout << encoder.pulse_counter_update() << std::endl; + usleep(5000); + } + return 0; +} \ No newline at end of file diff --git a/framebuffer_demo/CMakeLists.txt b/framebuffer_demo/CMakeLists.txt new file mode 100644 index 0000000..4bf4ef0 --- /dev/null +++ b/framebuffer_demo/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo1 +add_executable(framebuffer_demo framebuffer_demo.cpp) + +# 链接 OpenCV 库 +target_link_libraries(framebuffer_demo common_lib ${OpenCV_LIBS}) \ No newline at end of file diff --git a/framebuffer_demo/framebuffer_demo.cpp b/framebuffer_demo/framebuffer_demo.cpp new file mode 100644 index 0000000..ca12a63 --- /dev/null +++ b/framebuffer_demo/framebuffer_demo.cpp @@ -0,0 +1,207 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +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; +} \ No newline at end of file diff --git a/image_test/CMakeLists.txt b/image_test/CMakeLists.txt new file mode 100644 index 0000000..e07e9e8 --- /dev/null +++ b/image_test/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo2 +add_executable(image_test image_test.cpp) + +# 链接 OpenCV 库 +target_link_libraries(image_test common_lib ${OpenCV_LIBS}) \ No newline at end of file diff --git a/image_test/image_test.cpp b/image_test/image_test.cpp new file mode 100644 index 0000000..ec399e2 --- /dev/null +++ b/image_test/image_test.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 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 frame_duration; + +int displayMode = 0; +const int displayModeCount = 7; + +void imgInit() +{ + // 动态设置屏幕分辨率 + screenWidth = vinfo.xres; + screenHeight = vinfo.yres; + + // 计算 newWidth 和 newHeight，确保图像适应屏幕 + double widthRatio = static_cast(screenWidth) / cameraWidth; + double heightRatio = static_cast(screenHeight) / cameraHeight; + double scale = std::min(widthRatio, heightRatio); // 选择较小的比例，确保图像不超出屏幕 + + newWidth = static_cast(cameraWidth * scale); + newHeight = static_cast(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 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(left_line[y] * calc_scale); + scaledRightX = static_cast(right_line[y] * calc_scale); + scaledMidX = static_cast(mid_line[y] * calc_scale); + scaledLeftFilteredX = static_cast(left_line_filtered[y] * calc_scale); + scaledRightFilteredX = static_cast(right_line_filtered[y] * calc_scale); + scaledMidFilteredX = static_cast(mid_line_filtered[y] * calc_scale); + scaledY = static_cast(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(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; +} \ No newline at end of file diff --git a/jy62_demo/CMakeLists.txt b/jy62_demo/CMakeLists.txt new file mode 100644 index 0000000..9790289 --- /dev/null +++ b/jy62_demo/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo1 +add_executable(jy62_demo jy62_demo.cpp) + +# 链接 OpenCV 库 +target_link_libraries(jy62_demo) \ No newline at end of file diff --git a/jy62_demo/jy62_demo.cpp b/jy62_demo/jy62_demo.cpp new file mode 100644 index 0000000..d132887 --- /dev/null +++ b/jy62_demo/jy62_demo.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include + +const size_t PACKET_LENGTH = 11; + +const uint8_t HEADER = 0x55; + +std::atomic accel[3], gyro[3], angle[3]; + +uint8_t calculateChecksum(const std::vector &data) +{ + uint8_t checksum = 0; + for (uint8_t byte : data) + { + checksum += byte; + } + return checksum; +} + +bool parsePacket(const std::vector &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(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 &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 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; +} diff --git a/key_demo/CMakeLists.txt b/key_demo/CMakeLists.txt new file mode 100644 index 0000000..804b4ac --- /dev/null +++ b/key_demo/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo1 +add_executable(key_demo key_demo.cpp) + +# 链接库 +target_link_libraries(key_demo common_lib) \ No newline at end of file diff --git a/key_demo/key_demo.cpp b/key_demo/key_demo.cpp new file mode 100644 index 0000000..47c11d0 --- /dev/null +++ b/key_demo/key_demo.cpp @@ -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 +#include +#include +#include + +#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; +} diff --git a/lib/GPIO.h b/lib/GPIO.h new file mode 100644 index 0000000..8aa2a10 --- /dev/null +++ b/lib/GPIO.h @@ -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 +#include +#include +#include +#include +#include +#include +#include + +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 diff --git a/lib/MotorController.h b/lib/MotorController.h new file mode 100644 index 0000000..e568d5f --- /dev/null +++ b/lib/MotorController.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 diff --git a/lib/PIDController.h b/lib/PIDController.h new file mode 100644 index 0000000..d48279b --- /dev/null +++ b/lib/PIDController.h @@ -0,0 +1,57 @@ +#ifndef PIDCONTROLLER_H +#define PIDCONTROLLER_H + +#include // 用于 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 diff --git a/lib/PwmController.h b/lib/PwmController.h new file mode 100644 index 0000000..32afbab --- /dev/null +++ b/lib/PwmController.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 +#include +#include + +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 diff --git a/lib/Timer.h b/lib/Timer.h new file mode 100644 index 0000000..7fdde33 --- /dev/null +++ b/lib/Timer.h @@ -0,0 +1,28 @@ +#ifndef TIMER_H +#define TIMER_H + +#include +#include +#include +#include +#include + +class Timer +{ +public: + Timer(int interval_ms, std::function task); + ~Timer(); + + void start(); + void stop(); + +private: + void run(); + + int interval; // Interval in milliseconds + std::function task; + std::atomic running; + std::thread worker; +}; + +#endif // TIMER_H diff --git a/lib/camera.h b/lib/camera.h new file mode 100644 index 0000000..d1d9e8d --- /dev/null +++ b/lib/camera.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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 \ No newline at end of file diff --git a/lib/control.h b/lib/control.h new file mode 100644 index 0000000..6d393e2 --- /dev/null +++ b/lib/control.h @@ -0,0 +1,18 @@ +#ifndef CONTROL_H +#define CONTROL_H + +#include + +#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 \ No newline at end of file diff --git a/lib/encoder.h b/lib/encoder.h new file mode 100644 index 0000000..c06cf77 --- /dev/null +++ b/lib/encoder.h @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 diff --git a/lib/frame_buffer.h b/lib/frame_buffer.h new file mode 100644 index 0000000..893750f --- /dev/null +++ b/lib/frame_buffer.h @@ -0,0 +1,8 @@ +#ifndef _FRAME_BUFFER_H +#define _FRAME_BUFFER_H + +#include + +void convertMatToRGB565(const cv::Mat &frame, uint16_t *buffer, int width, int height); + +#endif \ No newline at end of file diff --git a/lib/global.h b/lib/global.h new file mode 100644 index 0000000..c675db8 --- /dev/null +++ b/lib/global.h @@ -0,0 +1,37 @@ +#ifndef GLOBAL_H +#define GLOBAL_H + +#include +#include +#include +#include + +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 PID_rotate; + +extern double target_speed; + +#endif diff --git a/lib/image_cv.h b/lib/image_cv.h new file mode 100644 index 0000000..fe80828 --- /dev/null +++ b/lib/image_cv.h @@ -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 +#include +#include + +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 left_line; // 左边缘列号数组 +extern std::vector right_line; // 右边缘列号数组 +extern std::vector mid_line; // 中线列号数组 +extern std::vector left_line_filtered; // 中线列号数组 +extern std::vector right_line_filtered; // 中线列号数组 +extern std::vector mid_line_filtered; // 中线列号数组 + +extern int line_tracking_height, line_tracking_width; \ No newline at end of file diff --git a/lib/serial.h b/lib/serial.h new file mode 100644 index 0000000..16dc54a --- /dev/null +++ b/lib/serial.h @@ -0,0 +1,61 @@ +#ifndef _SERIAL_H +#define _SERIAL_H + +#include +#include +#include + +// 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 \ No newline at end of file diff --git a/lib/video.h b/lib/video.h new file mode 100644 index 0000000..29dc18e --- /dev/null +++ b/lib/video.h @@ -0,0 +1,35 @@ +#ifndef VIDEO_H_ +#define VIDEO_H_ + +#include +#include +#include +#include +#include +#include +#include + +#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 \ No newline at end of file diff --git a/lib/vl53l0x.h b/lib/vl53l0x.h new file mode 100644 index 0000000..816a9c6 --- /dev/null +++ b/lib/vl53l0x.h @@ -0,0 +1,22 @@ +#ifndef VL53L0X_H +#define VL53L0X_H + +#include +#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 diff --git a/lib/vl53l0x_def.h b/lib/vl53l0x_def.h new file mode 100644 index 0000000..cc04929 --- /dev/null +++ b/lib/vl53l0x_def.h @@ -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) +/*!>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_ */ diff --git a/lib/vl53l0x_device.h b/lib/vl53l0x_device.h new file mode 100644 index 0000000..dc010c2 --- /dev/null +++ b/lib/vl53l0x_device.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_ */ + + diff --git a/lib/vl53l0x_types.h b/lib/vl53l0x_types.h new file mode 100644 index 0000000..e57a7c3 --- /dev/null +++ b/lib/vl53l0x_types.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 +#include + +#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 +#endif /* _STDINT_H */ + +#endif /* VL53L0X_TYPES_H_ */ diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt new file mode 100644 index 0000000..e636278 --- /dev/null +++ b/main/CMakeLists.txt @@ -0,0 +1,4 @@ +# 主程序 +add_executable(smartcar_demo2 main.cpp) + +target_link_libraries(smartcar_demo2 common_lib ${OpenCV_LIBS}) \ No newline at end of file diff --git a/main/main.cpp b/main/main.cpp new file mode 100644 index 0000000..4051aa2 --- /dev/null +++ b/main/main.cpp @@ -0,0 +1,149 @@ +#include +#include +#include +#include +#include +#include + +#include "global.h" +#include "camera.h" +#include "control.h" +#include "Timer.h" +#include "serial.h" +#include "encoder.h" +#include "video.h" + +std::atomic 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(videoWidth) / videoHeight; + // // 根据屏幕大小计算缩放后的宽度和高度 + // if (screenWidth / static_cast(screenHeight) > aspectRatio) + // { + // // 屏幕更宽，以高度为基准 + // newHeight = screenHeight; + // newWidth = static_cast(newHeight * aspectRatio); + // } + // else + // { + // // 屏幕更高，以宽度为基准 + // newWidth = screenWidth; + // newHeight = static_cast(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; +} \ No newline at end of file diff --git a/opencv_demo1/CMakeLists.txt b/opencv_demo1/CMakeLists.txt new file mode 100644 index 0000000..10b9ce6 --- /dev/null +++ b/opencv_demo1/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo3 +add_executable(opencv_demo1 opencv_demo1.cpp) + +# 链接 OpenCV 库 +target_link_libraries(opencv_demo1 ${OpenCV_LIBS}) \ No newline at end of file diff --git a/opencv_demo1/opencv_demo1.cpp b/opencv_demo1/opencv_demo1.cpp new file mode 100644 index 0000000..1383794 --- /dev/null +++ b/opencv_demo1/opencv_demo1.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +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(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; +} \ No newline at end of file diff --git a/opencv_demo2/CMakeLists.txt b/opencv_demo2/CMakeLists.txt new file mode 100644 index 0000000..162b7c2 --- /dev/null +++ b/opencv_demo2/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo2 +add_executable(opencv_demo2 opencv_demo2.cpp) + +# 链接 OpenCV 库 +target_link_libraries(opencv_demo2 ${OpenCV_LIBS}) \ No newline at end of file diff --git a/opencv_demo2/opencv_demo2.cpp b/opencv_demo2/opencv_demo2.cpp new file mode 100644 index 0000000..c110572 --- /dev/null +++ b/opencv_demo2/opencv_demo2.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include // for sort + +using namespace cv; +using namespace std; + +// 定义立方体的顶点 +vector 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> 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 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_(3, 1) << point.x, point.y, point.z); + Mat rotatedPoint = rotationMatrix * pointMat; + + // 透视投影 + float z = rotatedPoint.at(2, 0); + float x = rotatedPoint.at(0, 0) / (z / fov + 1) * scale + offset.x; + float y = rotatedPoint.at(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 &faceVertices, Mat rotationMatrix) +{ + float depth = 0; + for (const auto &vertex : faceVertices) + { + Mat pointMat = (Mat_(3, 1) << vertex.x, vertex.y, vertex.z); + Mat rotatedPoint = rotationMatrix * pointMat; + depth += rotatedPoint.at(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_(3, 3) << 1, 0, 0, + 0, cos(angle), -sin(angle), + 0, sin(angle), cos(angle)); + + Mat rotationMatrixY = (Mat_(3, 3) << cos(angle), 0, sin(angle), + 0, 1, 0, + -sin(angle), 0, cos(angle)); + + Mat rotationMatrixZ = (Mat_(3, 3) << cos(angle), -sin(angle), 0, + sin(angle), cos(angle), 0, + 0, 0, 1); + + Mat rotationMatrix = rotationMatrixZ * rotationMatrixY * rotationMatrixX; + + // 计算每个面的深度并排序 + vector> faceDepths; // {深度, 面索引} + for (size_t i = 0; i < faces.size(); i++) + { + vector 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 &a, const pair &b) + { + return a.first > b.first; // 深度大的先绘制 + }); + + // 按排序后的顺序绘制面 + for (const auto &faceDepth : faceDepths) + { + int faceIndex = faceDepth.second; + vector 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 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(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; +} \ No newline at end of file diff --git a/opencv_demo3/CMakeLists.txt b/opencv_demo3/CMakeLists.txt new file mode 100644 index 0000000..b7b486f --- /dev/null +++ b/opencv_demo3/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo2 +add_executable(opencv_demo3 opencv_demo3.cpp) + +# 链接 OpenCV 库 +target_link_libraries(opencv_demo3 ${OpenCV_LIBS}) \ No newline at end of file diff --git a/opencv_demo3/opencv_demo3.cpp b/opencv_demo3/opencv_demo3.cpp new file mode 100644 index 0000000..3e72014 --- /dev/null +++ b/opencv_demo3/opencv_demo3.cpp @@ -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 +#include +#include +#include +#include +#include + +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 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(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; +} \ No newline at end of file diff --git a/src/GPIO.cpp b/src/GPIO.cpp new file mode 100644 index 0000000..9c120d5 --- /dev/null +++ b/src/GPIO.cpp @@ -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(value.size())); +} diff --git a/src/MotorController.cpp b/src/MotorController.cpp new file mode 100644 index 0000000..871b8cd --- /dev/null +++ b/src/MotorController.cpp @@ -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(output); + + // 设置 PWM 占空比 + updateduty(output); + std::cout << encoderReading << " " << output << std::endl; +} + +void MotorController::updateTarget(int speed) +{ + pidController.setTarget(speed); +} diff --git a/src/PIDController.cpp b/src/PIDController.cpp new file mode 100644 index 0000000..6e1c84f --- /dev/null +++ b/src/PIDController.cpp @@ -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_; +} diff --git a/src/PwmController.cpp b/src/PwmController.cpp new file mode 100644 index 0000000..02e65b4 --- /dev/null +++ b/src/PwmController.cpp @@ -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 +#include + +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(value.size())); +} diff --git a/src/Timer.cpp b/src/Timer.cpp new file mode 100644 index 0000000..0b78115 --- /dev/null +++ b/src/Timer.cpp @@ -0,0 +1,36 @@ +#include "Timer.h" + +Timer::Timer(int interval_ms, std::function 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(); + } + } +} diff --git a/src/camera.cpp b/src/camera.cpp new file mode 100644 index 0000000..1d2d0c3 --- /dev/null +++ b/src/camera.cpp @@ -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(screenWidth) / cameraWidth; + double heightRatio = static_cast(screenHeight) / cameraHeight; + double scale = std::min(widthRatio, heightRatio); // 选择较小的比例，确保图像不超出屏幕 + + newWidth = static_cast(cameraWidth * scale); + newHeight = static_cast(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(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(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(left_line[y] * calc_scale); + scaledRightX = static_cast(right_line[y] * calc_scale); + scaledMidX = static_cast(mid_line[y] * calc_scale); + scaledY = static_cast(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; +} diff --git a/src/control.cpp b/src/control.cpp new file mode 100644 index 0000000..43ee195 --- /dev/null +++ b/src/control.cpp @@ -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); +} diff --git a/src/encoder.cpp b/src/encoder.cpp new file mode 100644 index 0000000..5f605f0 --- /dev/null +++ b/src/encoder.cpp @@ -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; +} \ No newline at end of file diff --git a/src/frame_buffer.cpp b/src/frame_buffer.cpp new file mode 100644 index 0000000..0f152c8 --- /dev/null +++ b/src/frame_buffer.cpp @@ -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(y, x); + buffer[y * width + x] = convertRGBToRGB565(color[2], color[1], color[0]); + } + } +} \ No newline at end of file diff --git a/src/global.cpp b/src/global.cpp new file mode 100644 index 0000000..246eb2c --- /dev/null +++ b/src/global.cpp @@ -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; +} diff --git a/src/image_cv.cpp b/src/image_cv.cpp new file mode 100644 index 0000000..e3e5628 --- /dev/null +++ b/src/image_cv.cpp @@ -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 left_line; // 左边缘列号数组 +std::vector right_line; // 右边缘列号数组 +std::vector mid_line; // 中线列号数组 +std::vector left_line_filtered; // 中线列号数组 +std::vector right_line_filtered; // 中线列号数组 +std::vector 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 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(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; + } + } +} \ No newline at end of file diff --git a/src/serial.cpp b/src/serial.cpp new file mode 100644 index 0000000..8909d38 --- /dev/null +++ b/src/serial.cpp @@ -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; +} \ No newline at end of file diff --git a/src/video.cpp b/src/video.cpp new file mode 100644 index 0000000..2cfc1a2 --- /dev/null +++ b/src/video.cpp @@ -0,0 +1,20 @@ +#include "video.h" + +Video::Video(const std::string &filename, double fps) + : timer(static_cast(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(); +} + \ No newline at end of file diff --git a/src/vl53l0x.cpp b/src/vl53l0x.cpp new file mode 100644 index 0000000..524effd --- /dev/null +++ b/src/vl53l0x.cpp @@ -0,0 +1,72 @@ +#include "vl53l0x.h" +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/start.sh b/start.sh new file mode 100644 index 0000000..c223c16 --- /dev/null +++ b/start.sh @@ -0,0 +1,5 @@ +#!/bin/sh +# SmartCar Demo 启动脚本 (带 GPIO 修复) +DIR="$(dirname "$0")" +export LD_PRELOAD="$DIR/gpio_fix_final.so" +exec "$DIR/smartcar_demo" diff --git a/udp_receive/CMakeLists.txt b/udp_receive/CMakeLists.txt new file mode 100644 index 0000000..0b9238c --- /dev/null +++ b/udp_receive/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo2 +add_executable(udp_receive udp_receive.cpp) + +# 链接 OpenCV 库 +target_link_libraries(udp_receive common_lib ${OpenCV_LIBS}) \ No newline at end of file diff --git a/udp_receive/slider.py b/udp_receive/slider.py new file mode 100644 index 0000000..8b3bf53 --- /dev/null +++ b/udp_receive/slider.py @@ -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() diff --git a/udp_receive/udp_receive.cpp b/udp_receive/udp_receive.cpp new file mode 100644 index 0000000..f002eae --- /dev/null +++ b/udp_receive/udp_receive.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include "MotorController.h" +#include "PwmController.h" +#include "GPIO.h" +#include + +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; +} diff --git a/vl53l0x_demo/CMakeLists.txt b/vl53l0x_demo/CMakeLists.txt new file mode 100644 index 0000000..0fa3e2c --- /dev/null +++ b/vl53l0x_demo/CMakeLists.txt @@ -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) diff --git a/vl53l0x_demo/vl53l0x_demo.cpp b/vl53l0x_demo/vl53l0x_demo.cpp new file mode 100644 index 0000000..821bbc8 --- /dev/null +++ b/vl53l0x_demo/vl53l0x_demo.cpp @@ -0,0 +1,44 @@ +/* + * VL53L0X 激光测距 demo — 持续读取距离并打印 + */ +#include +#include +#include +#include +#include "vl53l0x.h" + +std::atomic 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; +} diff --git a/vl53l0x_setup.sh b/vl53l0x_setup.sh new file mode 100644 index 0000000..560631d --- /dev/null +++ b/vl53l0x_setup.sh @@ -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 diff --git a/vl53l0x_test/Android.mk b/vl53l0x_test/Android.mk new file mode 100644 index 0000000..2a469ee --- /dev/null +++ b/vl53l0x_test/Android.mk @@ -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) + diff --git a/vl53l0x_test/Makefile b/vl53l0x_test/Makefile new file mode 100644 index 0000000..04b68f3 --- /dev/null +++ b/vl53l0x_test/Makefile @@ -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 + diff --git a/vl53l0x_test/vl53l0x_def.h b/vl53l0x_test/vl53l0x_def.h new file mode 100644 index 0000000..cc04929 --- /dev/null +++ b/vl53l0x_test/vl53l0x_def.h @@ -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) +/*!>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_ */ diff --git a/vl53l0x_test/vl53l0x_device.h b/vl53l0x_test/vl53l0x_device.h new file mode 100644 index 0000000..dc010c2 --- /dev/null +++ b/vl53l0x_test/vl53l0x_device.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_ */ + + diff --git a/vl53l0x_test/vl53l0x_parameter b/vl53l0x_test/vl53l0x_parameter new file mode 100644 index 0000000000000000000000000000000000000000..2fb42d4b3fb4ac36070af9fbfa88b0a6cdd6bb0a GIT binary patch literal 20920 zcmeHPeQX@X6`xx>&e!4Clzad|+CWj)m2FzgnRzp_HyioQw<6K4iqFR=DNJxR3z>>&P*z;JPZTOR*1{G; z&8_SfRt0<|$3%UQ74*3NC25-IA|b}m4(hci0szY8i7u0JM5%{J7sxtExE_=_ZIlW` zX#l8)eQmr)Xf-}2&^pHippwtUWd7^pQF`_g+92@3#X%8sxA5j(RFsb}0 zWU&}KZzWLg^bMi{DAyx;w}2T@l*RNl(8GRy-c%z6dt`gp^UFy&KBAIk4XNa=jT;(L zi9jwBXcGNLn^?cCYdg1&eWUrq`L2eH7|x#@9$N@yvWdQ10vTl~;h6l8(Tz(XFNTZ> zWv~u)kt(%8b-8c<43^O*%HHwXvR~bE*?x7!M`PE-q zUsBgVa()^ENSzO}2LZdW431+jhc^ITTSoo|fT?BhuK;-9#12J0e@Yhoia$CDN_c4e{_({(eO zO=t8}CT>{COq%7atd$u}0Zl*+OJ?F$iVf_^CM}a?M$9xD7|ABn)&L5EKqd<&(6XMh zjI5;(8_6`=7VX&5rmqXEyNl@^eVsa3G6$16%gpw5wxu#@v(MO-GDTh48W;ksEBr@P zaQO7`e~i&SdE)!e!O~LL!>9yl=Y*Ia)TQ-{_)rNhw?4vbqy#Tr-%pm{w3ku-*%G|; zeODuK#BiNcT2+G6I|%F45*+8Al2(G_{-V@ag5zGI)KY@e9>ls}2~K+&@%9oN_cWzy zA6tFwceU(Kp%1iUhONwoK2VM=h_J~udF`x^MVCSS$G#6Vu(i6tpF8j5XI%VY7r)=d z8!kTN;@1@Ur`U4#KD6zhK2iaGSuyEj6$cvqEhfur18QE0*F;#nCd58O8CI0}5M=(2 zSmR3ty+uv_mT!W+bBNaz@iid3hOsEePrg`?Cmg&AOGIQ9j+a9+OJf|!G8%NZ0%8_Sl*%5yS z!*-6&MHsgA_>XHCws)kmgH5R{$lKU`PMcWnYytT&*oF(!O;0enAs z_%GU|FRwk}Th6Lx4=4+uE!b=&>0R|0Qex!NDkUjyA4(4DE6>k6GxqddzVtJ6|{RmyBAK#hds^ZXC4RsCCGMzwC6h|?18NlyIl5a zq`eof^!s66a7-}Q8qBo@a}E1shOf0avz0yheBIuOeBDKuhl@S4%4MLNdS{hQf30N( ztc&rfeBC2E^L38~^L6{weBGvH9ZcQKf@i?)E0CXuyb3a|$qPzeTeUE+e79mbGa-i| zU%Wl9?1w$s@}aGRJq_B^z@`RmYG79byBgTl;!O03);3c2e(*8zE3-Xk44SR#SgLt_ zs&QN&F|x+62|tButWf&;14e4p{Kk?J$jGU8H@2$XTetQ``s&rDR&_^TD0+WSs4r5l zuH)SOkw{l)OBDF^t?IT|NB7q4U2XO1lEsiVw5rkWe!Z`~C(_&A9Szs3&8=#C$F}wo zhzo9PRl|`7I@%(7XLlG(-PNjgboE7gIwPUp?LCpsNLQcU*~>*+TGgJ&)<{pJt1Z$S z3x#|2Sg5C7-NfOx5d7vt?>f4>5MHA4A*sonni;`!g^^MRGFcVR5mquisE*-?RV$-r z&E2CW91Rl1!HZv|hJ-oRkh6x#aakih2%0Q{hYql6rq%hiDkvr6;O;wf^xM{#Q3p*+ zeIVMrKH8`j-JeV+%<Ku-g0@j(I7O@c7rBR$Nw z&*$^sP{EPa@Wf2thX^VEe(! zpDY^MuN1$hqgwA6R^pWE^to(DV+cpmUP z;CaCFfad|v1D*#w4|pE%Jm7i2^ML08&jX$ZJP&vt@I2so!1I9T0nY=T2RsjW9`HQi zdBF34=K;?Ho(DV+cpmUP;CaCFfad|v1D*#w4|pE%Jm7iY|J(!9^6&O{N-Dzz7}1+$ z|0%xuR>@OL;w_SIk#W?kUG?<$_CqpW`k&YA42t|l@)Qdhs}z6VUx-nx5IhA;9F=w{ z<`Ms{9}`C9lqM0?0LX;Ur!a%O6rwl?inUxR@#zIZp70jQ(>N)Pf?_49{cE-u&nhWz zOP>7qiR9);ZD?w0YObqOTLS9?jih`XS^=w-?inZ*seo82 ziqCiA6^z!S6R#|+OD7JiRIW!SUd3qLI`M_9+oo= z#9@jZ{=~J5^7MWMmZz=o^s!?9X%6tF1z#C|?gN~+>$3kOm!Cf`F9Tk#|F1|p?s%>I zb>y$-hc`jq2kWGMnRC1Vmu>eQtbpiy_d59qP#@e>;9e)6NqP5rtKyEDziw{^+y_x^ z?)B3E_>DMUuK2HeISz68uKNyp0atn3&PPVdPr2m34R{Uw3c1(eQ&Qe(g|U|)0Wka8GJMbQG#mg!1Lx}f5Ib{F8ghOqkXr3x~2SCm;6p?pMFOuPSBL{pSa}rNE|LtP+{nW z^d#Wr?&){|ab^BGc^PoEpj}eHFwA6pZPtXlE&_2D&ssTabYK9A#rr9AYgmut4HY@K z)*_M72UD3{MoLdunQTrsM#ot^Gdz-l@ZCgUWAnxh<%RTtWICxE*{rcwH`7*jFB`y^ zUp+B8JiHfFN;n-PEr%$DyA}dfp?^bWYJ{1ofwd?NysmHU33W#FNLN_bfpZv1Fg^TW zSE#e2%~`|m=Ku(Pr}ap?w9p>zVfwac_m)sp7qP~AAI21;Req~SJU7aRL)U%;gP-$q z4{>FPQ`}4#mO++__k}oxTo;l!D*2ry4g_W?m(hoebb?cnX8c3vcz9bCG#5+u2iRdFq6NT{}6sN4dE0+@&o$$L*9F_d$6MkKa1ID{n9Dul3 z#R2I$+>1i9i5I4nzZ4~q+dFI-yI27G#_151;6@fRJHi6#jAaHwTRPTS#vls}rbh!q zMsA1&5_{92EU1+gHDhKrhc~`(G0Esq2i`DHL6$~R77JMBI6Si%K7j#qNUq_b1jrM| z0>yR60>wSRTQ!E0acC)H1-LyZz%Yb0@F@$xz8Z$TQnFpYXc2$c@%;g>0iNhQ36v-I zpd73mkwyYho7CI^h)XIP~6={u+mFcc+yIu`@wiN4HU!5!==!FIzI#DiM}i8dy(29x&x%}Ta0)*X9ML)6@HBv?{>}mAX-bg76VGLsh&~GC zQhoY;2Fla4^rPU4dc^OC0_xNF>0AMnr{z>3*h2p$%btQlsXl#I0Ojd!5|q@{9d;1# zQhoaV0Ls%UtSm{i{s=k@1r(LbweX!n_n^7ui6?ptba9W9KAlrrNXPxQBd z;#847ooDHuxxDN@%`fTE{JjpkC_?&dLY`-W078lhQh1U+QE?tG>eD$m78EM&1x-;P zJ)$Q8E^VLAn}>oIGMpYwUDVlw14scb*S$`Lp)>K1f6jir}jw}&zj}*Ywr^h L=SfgfxBh +#include +#include +#include +#include +#include +#include +#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; +} + + diff --git a/vl53l0x_test/vl53l0x_parameter.o b/vl53l0x_test/vl53l0x_parameter.o new file mode 100644 index 0000000000000000000000000000000000000000..61c45a65b16b59d8d065a21a00e6be09aee17745 GIT binary patch literal 8464 zcmbuEO>A6O702&U0$c}D#xR|Z?Kp8p5t^}Q{Gm*2YdrZt6ip`MXM4nW z#>~991{EcIBxa9^|&juOAEw!+l46s0zo)vd?W_a61VH}KLU?td@5 zp-!kjzV~=w_Rp!_N3p#{vHdmd^Vp}GVKo<;+kWqHe4~FW%wzc=yc0eU?}XnX zz1`o7bBKI~cWBHi-Z`c+JAZ%NjjS94pW z_2S6>7jcYug#VX0Me?Q^oxJ)NCP_&?9L*H9a|HAy0TNt#kRS)8qN_<=GsWI-l0l zGo^g-ay~nMrI^2*pDC3u&wJ9*v@Yf+^2Pkjcz%8^o0~7sWs3=Y!NcR(!t@wLH$6MU z_>lI7q-$;6Ty<;BMy0M7n=SoPtLD13#*$v+5o@=pTh0$|Ic?Wj*d6>K&cAwL!C8Bu z?XDnktx98wda8V@MTVNIPDA%T)zqn0rF5TZ`&MWon|jG{_0_`2a3Q64(_d>WIP2-u z`k8fKNxJrb)+IE}iWhkL%mU1-UdxaqwByCvb}R0!w%=!Rd_1k=g=Vv{lxX%ap40K6)X>?%!NHM%0X>==PNuwk9QE!>g??02p_fCU*x`u!IxVGuY`XY&BdN=AKF;|pNC%$q=<_fHP{QxT?k3#oyALss*Bh!HwpQKUs zkDa{)5w&~H+Rr0uZ-WuCwAU5k6H8pYE5nw3jP;A2uGcR(bhf#O`i$N5>R<8hy?(#u z@sN61^!SF+j|$fbqyLN0`?J_b$L^C4MvOj=IWdpZhHIh!w&6DfAM<#h>Ign*^dAX+ z$?$gspEY(QFB$$w=ov@U0q?22Mbtrw)OUzrE!z*90emfh|2TmEl=w3$)ZGhM>#qjr zHv;&t0{Ghj{5Jvow*g$?VCx=9$*Nj*>Z_`d3aMmaJVm!*t_;%cELVo;HpuuKz0Nb? z=cDwxzzt6`qRNwn*|BV)?7w%EOZ*;DR&95IP9wF*C$OWO^_r_z=*Urgkg9T-o;8}~ zdb3*L)0)&R@9a{GtF2nYU1Z=kZ`D<;S#|3)qTM;<1Lg}1%v57;nfZH=AMWb?{p2&7 zf8HQ^-}NroIc?e~z`q4x%HyU)JUCgvWjx?A9`LRmGcNG`+L7ae9XT%WAbo$pcy$2h z4}|uGHjm4D^D^TCmwv%zoZvG5{<%jVqKtjyzlR&Y;L+RDOT-skZdKnM6 z)ORfVF|M0=0hf6Om-7NH*TZcx$rs1Z_OTv%xbX`v{enxs;6Zx19-!A0i+zF1_3#TL z;(Yp_4SWDD^Lf|kdAx^BoeOX|zi$`~>+SOsT%I%F^1K3tGr1DEslYY|V>w83RO z;4&U?j4Nv97ks~V9P~SaOTXYUPH>rjd@tp^v3=yfha11( z(l5C53m&AG`GH>h!|dL{Wqz)k{K)eGT<(__t$cp2Yh5XDsmJ$fc|L$k{awpGKHIY! zaCttwVd?Gj0bJUD(;BaRK7dO-zRzP{VEus0eE}}_7r2~9a5?Yba=pE4jo04q;4&U? z84ozdh5Zh`UpsPKup`F>9;ENL))ZHv*8yCv=lfQEykDfvz5|zj!DXD_GXD>ReeCxh zZv28vzu?j@c#vM^2YQ(waG9TnCVsi!!R7k@m!-GY5V+KTV&#X|5cUhWTvy<>ZSDpx z^8haM1}^98QxOm53tYwnF5>~0`vrWzcI3EVM~(|TNZ-%NvM%Vg#kdMC*TeC!FXVCY z3-bjo{esIl!DaqW3j4@^4>$V-T>1rz_EmCLBLoO&hcI_s`VE>zr#O5SX@RkGFe8`b8@iqoKH zji&1)=^v5j=^vBo_esFroaOQ&|7B*}{@;Zr3%r%=_gFTX;Tv5UlPABm`#A`0{%wfo zJ_BIN>reTWN9Mx>|Hj6CkfW1E_nhGo(?-z|e!eevf8w)wyIAEdr)Q@Vry!{Pzr zr!L-gf$rQtZf#$CzB6}g zb0jD|TIWl3)bt~PCY53o=!Xg_tyC#(QaDG7A|-^NMlESW8;J6ODyooHEuk2;@6F7+ z+ub|VC{m^Vv4dvc%x~U%^XARW?JV+}ueWusQvv}-NnxT^KZB`=Mr6a8J4B&|W6f+9 zv@Bv*vFT8+<(O!Xu#y~?|5&OfI#aMQ)Pr)(iU5Feexh?FA5pRp=``6U373O1rx7VY zlm>uuXlvtbLaOlr!B2Ep+L4!(Gs zgSSfjga=2za69GxhAK}B;EDJF4}X`$iynNH#Lswe2J0e@Yho2M^GS;teVMGqbluEm z(-}RLi5pfjlV&+9Yh^}KKoiiyl9{-bV*OjQNy}uJVKdG8hqKAF)sKw8kja7y)U4+$ zBWvkHMl#LTcXqV2>dV8+Z)SQ&&juYdnFGn3WoCOew5Bp?v&ZO5nWC*~3k(6)75*bC zYWNKBe~i&SdH6d|!qQUM*H8%3=7N|Xl%@5H_@D!qTOVOI?7*Gt`w<6Ddl~s3ci_(7 zyLyQuhU=WtLI+O2LD;4`aGZNenghrEMXAw&<6ffF?7(RcVq3(4)1F4W-GSqtrc@VT zi}(MwfxRAmU)yik%Hiny%Km9>YN`Aj z!&8rX>PyP?N7;P#F7zEdu(t;MvS2*GYQ`Fa%_b{q{c2H(*SE2FeUyEGJgm&~0q~qc ztZ`3CZe~-k`KzGsB;xgDdi1{MK6Ee?DI@GF(Q`nZnBNo5-7*sGpY_CDUg_69o` z_OoxQjbUF8K2guGzrD2`Y(ixb-p7`cT4BDs546wQPrLy3Mv9@WpvTw@b{C4umC)DO zU!G7-E9`S%XUtw^_XcYVSVtZW_%-0y;=t>**ExRb%f8*di+qIb2J|(`gpW>x@5gul zMH>$kwTA=q+4OtAsZ4{uV6QD6D;`$tXD{^DLft-pp*LQ<2(ptPI~m$~0_7)_ahSiC zUc49@-w|Pl|9&t8S$hoh1ZNhM#~RpX`+K&KfBr)6G{A)3BZ~d43%w|NID04rSqsM4 zW^T)t=OmY z@|t2R(<23CFYt5uIZ_|mxg7dbY#1}+>%ji-qOkGG3%y)N)S*Lj=y-XG+swz>;?eOl zspE~Q-jWZ31!do@>|UI^!`B=N@$r51CinGg%MWQsS`KM1gMB}uF%@1_xUHX^>SYDi zRRkGskKwS#Ii{N(_H=vfStRXgo$BTNHXL*H`+D~w?G*Iu>Ob;|=>M7Xy@#){mDgZi z(BG$FC66~1_&$6P>U$vDZ=Dxv7xW$J^Nfc{eH+jB24SvoY_NVbSU(!9AJ}`7 zmFt(;%GP2rw5?DKorQTm`@2_^b3j+W@rttg?FMGRy30=#L-%bihVG9PL)+D2X!YC< zrmkg?ozcRbroXn|T>|E8<+;~GyrT4IEBobLOms3Y_$@GBQn9t|c{!Dhru-2$TR44>l zYtSr<^=-4voYr)sx?Ig<)fFMhj9se0n;FK_g^@yCDxM}R)SfUbL$xxXWy^>O=YvGa zw%Kf1!W>0_h{Pp=8Rc`#6>k}vg{QR`@i3C(5r;Zw_5tA$8T~^59AZ0)NzyxL+)<{lv#{!FL z*5X+a-|5;p+uj89IiPuw;rG#%u7ldWkdMw3=PBgnDOOkCzC(GkHWtLA zDO&*BHzyG$XT;OnT-7*b}S~%<9)vXa>H|k-1ni(Q>-r1zO(iR zGY5AlyQV)i`|02h>y*eONWK~SW9(0{w_?X*_`R|a@)YFyppf_4&vp3^-vhn}d=K~@ z@IByr!1sXf0pA0@2Ye6s9`HTjd%*XA?*ZQfz6X2{_#W^*;CsOLfbRj{1HK1*5BMJN zJ>Yx5_kiyK-vhn}d=K~@@IByr!1sXf0pA0@2Ye6s9`HTzf98P$^6&IFODe+y7}3v2 zdnvAZk*rfp;#IQVEaRvbd)n#m>j!1L^gl1!8Dx1*)+rXUt5*Cyekn$=M${=DW1rMZ zF^~9n{FpFm#+gJE10V$js5(YkfxGg!6xu3$gQd6~Hx8E$$aohwUWfM*sI*q8tp=j}wYq9}J)_?N;ze2m5%IMD-S|94>&J~> zUfQ2-{0i13_pKYB&vsSC7qE${IPL1PmV;2NWgppPM3ivs)o_dxuZH9Ap8_`+9Pq^3 zUoCN%VwXR0?IJ(@zCx9!b?^+Zvi&p%c+*0lia)ml&inP~e}wZ_&dZMhuV(+Rq#kd4 z)^9GsePrqOEK3dIjh|Blfcd!9)mG|v_BqjfZhkrZZ_3#$*uETFiezz9Jc1!$# zN6*1a@Z&ul4G=f%UH^YZ9B$p9eNE#!&GiH;;^U^I{-USfx52=AScg9;xaY9}UYzrk zU`3o?3*zWcx&FuG07D$FyyxOpz)`=~KOK_)gvXvQN&WO5p}0U@@}KeWZ;?1$o}j`I zOX(YcSG%X<3B;Akb+Q|9wIuB*VAy3czBFsXT^Hdvi)XEzHPYV?#q#|Wx;3Q7@rH^V zTx*fY=mV)tpOMlNRwkR%jgdTyXNHDTCPb=+SFK#NqB@h_pG+rpBbzn0>1Nu>Ze#sf zW60DKBSS;mK*Ukgfzxuamcm`l0kCY)Obs(L)xQ+^pswrdx}zJ~^tL-=x(+p$c!KG% zd+v;G=xBAf@S8aRf;Y6@)-E-)$GVxmzH?(sv{T=>ZrxpNJ$g^HrLzsS^7}mExe-1P zy6__s`GnVch|5CU>?Q`mlIHSFA#NtmRV1!Pep`tPf%(a0^g$z?;8%%kgbPR#$+SL_ zGrgT;0L&-Swl(E8G9~591LL2y$g<(ex!7g!Z)K6E$_c{J< z!1m{f&XGWQ@?I+B{Eb0|Ya!z|1Ig2Q6DUurAf+e$P7|;a3iu5~@^mf*%2THxoCk~a zJwkfmswVa?_{Jdl=j1sUC{Hm#D&!^1)T!Jkj?geJ4^sL^lB!euGh+&euSBQU&30@=p6D z%m$!`3MfzGr}G3*o?gO6N22*B=us#*<>`9@C{MQ# zqoi_LOXyB0IpyiQ11L`m|4aY7feTsXaxHzI(EVp#d#O%zKgi-9CwV%bHp~0rh!0qJ zqB_xE1Bz2c@^p@+`{jz#ewtsBqxpLgWRZpBSwWs-A^<|_5~TDbd7`gEi&LJ?zg-a_ z(q57j1(G9r6kw-3oileu-29?eUcXd71_jhb_D@LrCnAD_`VlqJ7nPA7hm8F +#include +#include +#include +#include +#include +#include + +#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; +} + + diff --git a/vl53l0x_test/vl53l0x_reg.o b/vl53l0x_test/vl53l0x_reg.o new file mode 100644 index 0000000000000000000000000000000000000000..ee749a2df3b6e916139fb56a93c593248deca5cf GIT binary patch literal 8688 zcmbuFPi!1l9mn6!+99Nykd&qbYT!8}Wen+N*KyL+D$?3sCqY`ZmgCYOiZ+|wvAtrw zYj(%!7DZwsLP5fnkP5Vi9&$)fC5prasS*M^6$lP)=>b6w960pQ0}=)G&;yG3{bqh& z`}MaT3B!|S-=qjvx8DoZ4Q|mro-%A}fd!G4@H#pGq28iD<>mE&=8=!mctLyh@?j4$YXXM$x z@%%g9syS?a_r|@E)$0Yb@vq;E(B;2Cb~1Z=-V2A!<;@p2pT_(_Vy<1DyU|`7q07${ z&1J;$62-#)`1xw8=ieZExPP9;gB06sitQaQ=9z!5dFEDXX5)=}{_60$$GN=eZF+kO zJ?}Q@-T6_RqvX@uq@0c4J7fx*|G2jmV`O8(vEe7!m{c}?=GZ_DGCl9LFPUdDRKJa* zYa_@nif?qye`|8h|2@U`Qzs|HYIxO%b^ zE^Q@NX)e$lGu>{JiPmz^wEa&lO{!O^9LGERxH#12*nFi%nVvpX%54W+Z`OkDc&>ZA zd%{M+j~}n`9MW@q!GATuRyy`-rLhu3h9Z}J^EgTI6GX9h!b-T(iQS!_ni{u$snu#O zOthSCqdwC!p>d&c^sxzVwFK7V>-#GcKb%I2cH4o08Gq`oz4QrA+c z%>J}FM(l`i?jFauc;MiL>)uZWikSx)v4sK;q+Q}uid*r(Oo??b?O|nl%)3+kbMY@l zGM(7?L>(~y+}uf!HrwZ|`#f!SHkc+$JGC^QT&jJH?mfpoa(y7u_4@^f&NY`dA8~J^ zaa_lKzn>QS*VK&R=zh@LRs3Pm|5foXi2gmrPm2B;)v%+hN$9bmAYl;^l z{h-lyrp1o0{C-*Z`^wG}VrNG2i^6}XIODY06Ft=*X|q=&bBPG{xcjh~z*iDDuhUL; zt`onX_D@_VWAF|Ii0)Hie-%j8^PvE~y;AZOp3>LCgvluj%O(~Z$+0s;wK2Gr< zPak7E7^ROq-<_fBQM%63HN~38=*shisYrO*l+Tx*oSZ0?pFDT&;`Cg3Zep@DT{fMt zMu(J{kB&eSbnBs6qLaw*d27mLde&@}8?9=Ek8@JDqa)4CFSqN>aGpWfT4|Vis~R>a zAtxt3cs|8ImKtmFNkR`Xmj?bh0L4Ec_w*eaHRzAZJv#6&0!Sa91N4PV_Z$yS9B>^E zxQ+*W%Z}s)zFRvwFWAv}fhXyQ6Y{kQoPS_+8}2!;=nGBq0@r@Qb)4Y3{wKs4?4$mD zT>OG-zu?+0c#>Y%2YTxkfyUs-1CMhu;O~JUK-}(pOmgG+**^9| z9~Zyi+Ap~F3!bFc`vH1OvA7Lf?}zV;i0k=TX9Bpc=gXqueD^zgaJ|0#{lbm)?)eF> z&lzxiUV-a6fa|(}>-Dvn+0Qa5m&7r3qixL)5sDEk9)-^ax- zxb_RK{emay-5RiK=&i(o4%`Qj{%z^Gp000jUC+Cs=X&zLDbT>R{sR%PeRofTYyH7g zJb~>Wl)2o2pKurtA8_>Uc@D1a4~d@R;R_91>mPUYZv5a{Kj*}A2*#-g*YRI+^ltp% z+WuuH9yfk)tzUNR-|xf&uFubFj@~^#!Sy)$AKkTdt523dST))3BJN5ZE&)FMK1lNASb)4Y3{;w$esDB?9 zzu?+0xb_R4q}TO<-o|NeeS+)y+>-eDU2%Pb>-XhVCtv*Aft`SB{f1-T-E-ht|Bhon z`t7D<0Itu6cOAWZK7ecc?>X_f=L5Lbzwg+0-*<5RUVyu~ath#j9l_n&yL!FfK2Y&s zzk%y`z;!&}`h5rAtsR{g?C89}lk~%~-sqsW0Mdi&{d~lW1)LXOc;CUbUvM2KxUT<@ zvXA=rae3dtwO?@U7d%O?>jS;zT%884>vK%v*Y7*H-v1XI{V~TrxYn1P`nc~4xZYRb zdVhlJI)LlCf$Q~}QSo5Cz;!&}Iv#NSzJTx6j?N2qbY9>|dc7Z@x6WGe5M1wvMW;TI z581ci+Ap|{6I|E7q3omneO&y4Yro*yFL;t(*9UrCA8=itP~z9`3%K6@LsEC1=Kd9+ z=x|9jCfUw&OJU`T$?`8dqka*icF?F~!=M|QEdSNVWUq8OCfjbsH`UhCQqZJl%~lv> z=`THx&|iY8-yi``3l_`s?aERhZvXGjl7$pq?sqI3E$|1mA4#3!yY1h*;GTac%WGc% zkiPrTcyLpGnBd`o|b!be^P$I*@oNr=LWX z{(l4V;`QUXn2+`2*tKs$rZC8##&l(2?xQh(Z|M2FP4wCkGOvs9SA63%T`T^#=1%i3 NNcwf*->tDZq-7}+^>2Fup-J-Z$jFQ4cX?qz{k?W(1I*}>iyv1xJ z)J$bl*l;M9a-OL7F;kE0pOdDEmI*P2c2IAzA{aoqJkfDdjwm5SI!x9{!u24}>5l1w zOO!f*dWf~6Qq)&`OUM)5E^$02Dd}Mm^?Xv#C-sPCB`~7YKkOSGHPRkXj*qAcb(mEC zD`YVnd^a6Iy~)>!3ZPt%Xh5(TQIy4W8|WdgZ?Jme?9LG5B4jpG=|V?tMr<~auOB@*hR?qGcz0d;OT z$nTKl3FE6!LVRQ47$qxF4sB!GXluVK=y$QwUa&Z_%=ZxQF4kikmFIK6y7I5*y|n;zN(DubqNP@@*8 zO#l!5=_kNYolGd}aG;&u5hAWgq(<`l9Q;PfuaN5j$$wq)eGWd3 zAKGtr@NwLcpK$Q~l7G_Cu2=G5V0rvQk*BNS7wwcg_^pz!I`}tAe$c^RDft-(pFv!Q zF{T$WV?#8}Qqk6qP>hAv#*=BL>qau!5!Yk!a3~#(cd(X3XFAMM=?L&yG#*aJSZ69` z7zx(0J{e6LER>E%SvVF?87!VKP$3!ZNVkAW5>(K^bF$}|A>l`_16V66@#1RbtrkAkQQB_dTj$=h7Cx<2sI$w$r*#1N<&uv)>zto#;p04| zq+0kmM=5C*KCLfUS7G7fIzVZ$g->f1*7+=a>l#yI;nP}$@<9uqe2e_m7Cx;#$Zxjr zY3)OPn}u(k-(y|us-3&Xv)BE{w4I@x(&Im->>O6jx~65dJ{PMS2lXdh$292kRc85E zpS`@#QGU`<-s~uc9A&?wJk3#d<;y1k$BAP{&GvbDpQHR_zI-2>#NGiNcg2np7|$!a zT&!eEg?q8VvRaFpRl?=fEL`qq@1soSho;Q?AoBt8cKy4_E1Tv(A@Q5a*WOBg#iWyPiLyY`L1P=J|a`w9I(s z134`Wav2yG$~*0G8`;CkyI|v~9fy^twvT5|xoa7=(|xI$VO#e+T+Xn)r%G$t4wdbFynMw9G5TCQ1a0c-DvYz!|C)5nMGg+k{+UlQoSoug{GXZBHHz(KRF3n&WWi*i2 zKwb-jtP`)b|I}9QtGNd95!Q#Gt#wTJh-01hrq<=kYWKP(vEf^{D8rzw>FB%cekB*a z+*AtX+~Lbj9IM3G1>@GW*~fZb*z18q+XlMsvW(I_iLK!_{eM)(fZjG&#>4v!xCw9A zcUcMT*bjZ(?^)jr6xu<1FE739;Wm@IJ=x@5Pj)RFFF?K?r~=1QIOf4|EgTc}dSDH@ z>Z$Byr4-uD-Fmrc8`yAP+N*TGtOyMI{wLqBnoPc5>?I|4|D`7Ogc9sY?$x@6Wt6Tl zes*B#Wv%=9a)!R>8kSWcwzcmc+oK%7zC8hbxms!KaUEsbKz0_i-Q{{yA$g1;9HSn< z3>VEow_E0CK_2jNzKZSb7QTY|Z3|hm@c-|Cm(_ZpeQf)C(B2H*w|&ZwA)kBwq8Njw zOHEwHkJz%>7OV?jYC_-jfNywR$WpgV)`8DBHt-v7FLa2vb8y~X?ZDE#+ACn^X=<}K zt8lw_Tx?0Vf$5bCP3#CO zjA{JA!kEf)0@M8$gb%MMfC=z&AEKXd+z8XYp20CyToRb(1Ev}l`TQ(gcGaT9jpG>@GQl^B*@cqJ0<~A1LzPjxd zVSm2EzAT&e6BmU2*UmK|J|3Ib$hH=Y4w`+1x!1i}S(81!$CG`lQpEYbm-+Y&Z@y3A z^GUrR#_ZeYn%uzafwiUQh06-!#r8fb%W&Utyob+mznFL*cuC;>uHc*gkYy9^_VWVo zR14lXw1>XgGAiiCHDdp6F&}>m?eDj=uVp56EzI!q<2`_f<7jZ;xNE<_aj~D{GjYhW zi6eMU;CQe<@3;5+1&&K2gFVN0YdzpAJ_m<_<2BKq>5tp80!PY$LzYb(6=wyG(H0!K z#F3UbXuMoGK3)gjs+{>8^#8!|J@&P!?~E9~y=R;7oWOj1-c8)H@1c_1YiFB!uKhee zmW$b>uGtP>jsOhxV!a3I!91TgL0H>9b||ScJ8*`w+z9eu%{v;CQamll>_iPgi=}+cy`EyD@T}gwNXJ?)eMC&!Yw39B)}R z{e1FM;pZQn$z$t3Be1#4%(%-AWvq{$v154hoWO9e;F}m^*~F0fL}0kaf*~a_Y#kwD z4tyf!f4}ha{yo~$FxKLHcS?-!(a)OP(_x+r1?zkV)>CH$*2ACK=Z7qtSbIMdSW}h9(=lTrtQ2LJRz`O2R76pVqY&Xnsdta zsla}X1G_An*!w;Z*#GO(JoeENyDG6m+;Qv>dmQ@%rv&!rZy zc7}v+v;*I5e-Ze8e#-7AC>O-n`<}p8Wx@C8DdDf~k~}_)kDad7?ul{^%o`%UI|Yu+ zT`qBbaOgdO?+OP#D06&Z7w=b0|80L);QP_Zy#M-73ViO8pcz+q{vFDA9zJQ0s|Vf@ zc)lz6<{Bu=W?b!hN8p%h!4a1@w#fT}FA`S`5(mZAUB?BEizn>k24&~C@%y8bM+Khy zPvre_^n~!sXD<7-+Aw(zf_l6jHrJ&m`$U`fI@*+Fv+wMS_TII7w2^S_`T@j|Z7px^6ZV?G9_o(&Gu&@ww9H7j7nk?aobNxvZQ`|B zsRs9;B!BEM=VN>hudv>C_9?k5K4~g=eSGI9rq8+ULw}O-FZgD>fj*86`dp0f0patX zeVp%m?qe~&6X9Nou2Vh)9_ZoqGu(S?aPO_by|)&IdvELi5W*~{tj}gW8#7tY8_@Uu z-yT%n1bV~o4k|Ys9nV62S$zcEEmLn~z9V4&mvB4|$7DG0 zzW*a7t4$t}RlZj;i5YMN;OM_Lt89k*<;Cykbg-vEdm7l(piK?zYG79byIPp-vtdJf zR;*ZdG4Wr{RjE*`u|!=LTR1;fu|ZE8skC~>?C>|>r|lb~)Y)xnOFXG|q7;Wrm1zGSGQ z74XS_YF33h{t-Ks4!5ZdwaaQ(G{P`JTGilRR=q@>k>VhAvkUsVP^{CC9Uj0Q#j4^R zEz#Ca_^)}ZS_imIMfGNNM&~TG22mlC`vZ9b$lq#@{s(K3!wwceAn;e?f9sd1t2$Dh zi3B_gFd}L^fzJTcYiFdcokr%^oQSb*PAc6_fg-)bxIU;D9qPbZ6_lc32(K9_G9=rp zig&^=cf`|b!brlvwyR&OTR6Y2VzpkovZ}F8Z)jW@RIPd&c)V4~cr(;QEEG1>weWkV z3eOl?A>u-5bw;GUU2RFm+tt=sD3!YT=9|A@yQ>@hb+=ae>y}dEL&O{O*ZY@OH&)k^ zp6HIc+6Eiliby3l-VSa{@!6xoBr;$ss4dYAFa&cKsfjpD3^kPE(O`7ovymkf1Tb{c zUus~vf3(Kj9nkCjD_}M+^T>hwqV~AjEESfX z{1oF;SO(tTY0o}qEe5}+J=xz?*kMOkx#x#yuXaUkBZW5CvrpG$)r|pvqrX9I2}NTN zzp!43Fk?I%m&{lIp(vWd#jPRQY6-~eq0ZmF*2aOyJR&uu;|VpAjIJ}1+@ll6i060D z4ni~T!u7qQ3-brF^Z(879wNRMhbZU7|B}>mO)a@G0ItPnjT_$0<(h$(y_d^nfWmb} zZU@jb&{u)}3g}6otNU{~&BfR`plv`gO%#N=zFE&)n_MpUm=eyp3V!j}E}uVdk#cN= zP{g{M;7~`3@hGKwN#XyiYz-X2QKBAYc?ygzui30TQW|hyiDi^S+g(x)pY!t+7^?;z zQBW%VhrpPU1;Ffv090ar**)2)qF*+zht+4jhRo0>?v~ z3qOIezM4l%|E;WTv+~&R$4C9Z{lr+sw+|Hm7hL)eCVXqin#5Bq;ADh7`leBp9ir!V^vxlD zhl^?Za3N3p$+ArSd`#-ow}I6D=Q+`za}s|}mdSr7W%>8AUbOb*{X+XZQ471|ceq~` zrSInHc^|dwr1U(Io<}3BRAZi8=W)ns_Ccaf^i ztC+W7?%cTxJsx$jcfPlRln0?D5Y4m?fmNgg-UXxY_wD==M)7LrLo~~{wDX5CidQ>- zIHS0>^GC2k@lXOEM^hZx~I%8Mk)+Xh!30=ey14({?_*R3M+%+WBK~-vXLC zW>pF70?=HLU(RTqFohvg0$;S#`e5fzU=%-g{uSo@weu&kgq+uQ{v`HdLH?C2TaZt) zK5yl}AzR8$o3XQ0vQ*} z? zopJ1m=>y}c5T2tT4{s9;h@X?(&cOINFXf$a=;jU_7|+)L-vw`VIOA#|@F&2y(7Hzb zt>*lJ`z_oKe3iFt|LK(S&V3lRa=v@OdmOk!1@4F7+&|$N;FrVM&Kb|oNqKv#jP(#- zzK=rnZ%Mv$zlDn=PlA$;ffML>c<0$UzosCc<<|}Jj|ceZH(KxM{gE3uAAID%vqJJK z9Q+TV{c?!^S7p2rtY(x~;AJPrdo=$d+jYKwvjO;M$LY5%vR$8}-G`+eI#1DiGmlI8 zphLb}@|zv}-N3hg=TG>1rF_C6|2E1i1LN?7l+QTiVNqo&8MRV)K{*`_&rTY!sf9Pp z!pU?h-PzIt+5E;6db(YQO)NT$6zo?KiR-Pg_}WlRkEG+tlpgBbz{2o~W(;0$jCdC< zT(qFDklqsQi0YwaGPF@QI?~CFtR)$0H}ptnd;3OEv6OU>Oxr~1by|F{8KAWpu>_07 z;|YCj(g@wjj9ANT)PX8pzXe}0)vH$obR9}IQxT>IZeQVFUR!0a;oENj3$7mY>KbXG zCQ#4xWpyi;`s;Lfzq6sbQE&7wt*b_>eAA9_s*`t$cJ=W2hPZc!*c`+r4zHi;ZJ~}x z%t*>DL~PZ%5rH>Vsp0%?AvPht8;OnM+_1z}$@es|abR#$aR~!97+DG1l0>2%dS}W2 z$L0HsE;V;Bu^Heyo!B^fB&PG&!WU~rtm%m;Y+s_UO{K)PCwylVn-bp#g>R=~W8;P_ zHip=E#m?gUwb)p?4jZS)c)Cjr{h0R(7ZKE827eB$g@M5;% z^)IcRoes4!Z)-=V7hc?LW8TQd4p0^}ofI|jW@ZZajNxLkphF#aCxi+zmx!gAH*IWy z-(;K*y4PrvlcOyH@>F8pd}uRoesb|vL-58iv=mQ!xjo20H-t6tJM+SXZHGx{T?qcZ zJYK)RsQ|7?M560vpghrQgtD5;QXc7baNs=@>C^Q!P@Ysl%wIZh5Mv=^@Sc$L={g)J zPjo(^`$oDyBRg|J>MY!Ik-mAoE^iFi2s7qhRirQ3>_*7oJtpbXbv;m?cA;}1;e9G* zNuTZqtAK~1q)*rXKzaJLAm*<$XO?e;9NfT}_~||YC{NR+B=w)#A-Wps@t%~*bUy)< zCsmLE_$8aQLI!9JtCK;JC6OMnHbVyWssD6e1eB+95{D?|@l*Le z$XNC1{s|~g0TPrHePvCLLcyv}_gO%B@?oVVQT!3>Nyy-PxEO<&=sKA8gLBGLndnZ? z!8uO)bl_eGywUX=3vr!ss4$9+Qhvl4&SCnTsH zl6Q{(5vaiSD@Z_6(qH7@qbw#XrPviU;`f|S&@vgn_GQ~!SfRN4ZU literal 0 HcmV?d00001 diff --git a/vl53l0x_test/vl53l0x_test.c b/vl53l0x_test/vl53l0x_test.c new file mode 100644 index 0000000..4385798 --- /dev/null +++ b/vl53l0x_test/vl53l0x_test.c @@ -0,0 +1,389 @@ + + +#include +#include +#include +#include +#include +#include +#include +#include +#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 for testing interrupt threshold (out mode)\n" + " -L for testing interrupt threshold (low mode)\n" + " -H 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 \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 \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 \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; +} + + diff --git a/vl53l0x_test/vl53l0x_test.o b/vl53l0x_test/vl53l0x_test.o new file mode 100644 index 0000000000000000000000000000000000000000..b89e03532811278edda9ffe144bf0fe11a78e029 GIT binary patch literal 47784 zcmeI5Z){vwdY>=b7#?4^E;0MxYPwLJic7Y8a_n*cM}WQKO(YYP(H^z(`!fMTJ5i z)P)NOs9pEGcg{n-`i*Ya6U22t^aAAEdFFTD|L5EpN}_&k_wd)-+uAaYw#+|+W?AW9 zHqJZZLw|nSpWT(+nN2Ny++Ge#-A`rltADAzE6eUb)wWc!uYcHfZ?^i`lkKZI7u!1{ zKbbAH$4f^$<8ynQTKu^Esfl>q;!^vm?B49uGyU1}%NM)jSLwNUZP%K`_H*}Uhmy_q zJL7fFwJmnX2s+|*rKxyb$g<(7{*T(rXWoc*-st}BM0{L&Be(a%T_1JlHfyhSFV!w~ zFHOd;m*e??_}Cu5cEzvF@$13(b@#>YRWZhY8e{B?el5P|VoY5hUTi=2!**i0k=NeE zro6Hj+eemb7fYvBEw-P!b0B+r*GHvu|6*;H*JD-8u`Mh8=6`;@{cX$b#hA-4w9hQJ z{YCa%)RH6m#kOp?E9Sc#W6rjnE^&F0%~#2a(?tByCXORF)xlYM$IS)HtJt;V!B8=$|aG>&j;4+(X$ktZ7!`oaXEAmAL+TKl)ajwUrq=_xbG5$DfNn;*8iFZ4B8x z-@9$G!>{jretz+uF;{+%fWYw6i+; zyP8E`_bzrY!)9mkSF>qq)vA4qkJ-$9_1!VDl51Rh=bmXlwDjKV-AjMI^QJjpJ@Jp) zSx+n1N46aImgSc}YPWbTpQ~#Y^V)oB@t(YWq|5e*H(qP3$A2IFzv29s7VkZ^b+IGX z=V!kcTmV_?QATsoy8aJeCF|;>$nl0 z&Bs4)8|%8K(Z}V@Eynkqwx9a`Ag^DvYoAv>QT@6)Mpov^X0eri{#CwTIhj5e{J%0+ zAGF>4UTxKt@l{=T$5-lyB+{_sfFf73n|neRRC>-^f`ef3S(*?s-jcW(HT zcyGTn`FiQL_}=wOoTI|Mys_`c&s&S7#oMx>kH7ZuLHj)GZQ6Ua&QjwusOL7Ht9zP1 zcm7{{e|%5;tQ6ndQk$;0@7Q_#>|KhVy-V@4cWEkq_73qg`C|*^BbDvpskyCN=6X(y z*DDM4@U4fYo{ZmSp1eIgG!qV2YvH)5;&(^cu_IyjSiMrK9iOj<`b@2|FjJkI4yEex zdN^90u5?GU!|7TJ9~}w)x)b@l4W@akg!#GhR3%Kt@81sfa_vZ* zxN<$LpFVmt9IjQ5h9h(3g@s2Rd89?ZW1|DZzcM&5ybI@_Fy4`Y0|VdKJ-Yh<*EP}! zV>g9uWwl5XtsaeKTgX>Wh>NHamqIu^dm^@=_u(*KjSC}`7xHPS94k-GRkkw(v303l zVdxtJV}7lL195CCVW?a$Z(l#XF@%NLBge{f2cr4$qf_$>5#}ZNgn2#s^{@MCWSNDZ zR`~kqaCA2A1xG7!0aI#w{qdQn4p!>6xEI2#B@paYdu3#%yigg8N!`GWG0g2AoG(wu z!H8+w%hM}Y?OPa#Q&K-(tC-^Ei$7r=EBz@AKDvK&-~LA*J2*bN?;E>^#t#hakE?l4 zcWPX#_VUX%hPd_ISYr7SPJ{QCw0HBGbp~6tm-2ng5|&i+%6~$8`}Xe}Wwi6_ZeFf? zc8?AXj1C+Ohs(2baem`|)flrZAII(M`{F1zT-oM!aQ28B$gjlL298eNMD9#uksYj8 z=fiYu_NhvZrOZqJ|M4$VfjxtR+e2x%T0M4TpcZdtd-gvT>ecGp)J!>UKeegeO%I3C z=AO-cy}i9#ySu}-Ox!|K%T>{FBc zC#8;a+0Vt3HfJAC=(!#}{3O6Vc4TYZv&N~_KD$Mn@egO>(U#To#~a@s%1_&}vr>xQ zoUh7CQo4teeoXY+lm7hb{AA_Z*OLB4(GMlQB=+~^d{x#je1GC6gvU4O@o3BD?N5f> zF_xdN%Fc`a$(+XyNIa8Ce_8ZL5|2&3>6pu(wPo*%o$n;RR_r{LxNCh*{(k;F)>-T? zCjB%6l0VO~?KujuPof8|E5<7pA>|7AP|1s&e$zABr z62C6?|03DBE_VJ)(myRr^}i*3kM!kQ(wA<_PgcJDkE9tUc#&y4(^yfuClJrlD{l}6%lX?4A(tjZO ziKO2q`ooD&2%k%KhPY>@^uCkyXGH(qq`xNmrxSlu*6A~e_Xz)u#253&8#VnWi60XF zeBv|0e>?HIaI@Hv$>K_GIx-#dEc?B9J?~X+M3ZHISit|NfWKD2--!I~xR|&v^Iqmh ziJwePdGGQc3hew@0e`Q6|JMS3wSa$Az*ooq-Ksua1^n&;{&NL9;|B!xlbTIa*-T|_ zJ{#_7%jT-p`SHnGrTpz|)9_$V{MHx0zG6>$bU9s7&-<*r>vdwnIpk1-m*ydbpmuL-<(BfK`Wm$`ZC@Vi=G=Fl*Ve|ViMN{ZJhnXGJc~T5UVGuI+4!E}M|TYj zkH=5+2X~K-j}Gh_-aVcz)Tb-8T6Xxmwb^=gzAwD)fV>AG~@p%e$mThzTCvM;suKJ0jvHJM8An*<8 zbsnbEiRtr_rMkhj4sfj-T>CX6_B+$_CZ7Dl)h}HA!i)6UFZ3bB!2`SmNRP|u(dzTJ z?R-rGz|}8Yj2lf!L?skB_8?(*LdI>58Uctd3CO77>2iM zNAtpt<^?a(>pY+jjqp~!!%Hh9owPL`EwB8eLdvW0c{0n?FI?kxYhx# zb%SfahQ&VpYU0T+T>Zk;FT6q7MaJ`!((2zuR2{*ZR*F=;5kwe1499ku%!% z_Tj2O58UdO z+ez~SZ`F?Gg&oZcUZj5@<--H|07#E;owu{DKKXm~lowq6!Zl8~*8iN?XFWFYS zXkPFlz0ME%K%>$lT<2%Z)hG9n`UO|NaE%kL^*-?Y(QXja^&v{oLfB%B3U%18z*ZRLA_NjjpPwNY= ze&OmDUZmIhpx64qwLY(=_;r23b^e!JzW)9NSN$cYzt8myuJ^AWJH5Yu!F3MdI?r&e z8~i~Ri=D!?zn3K*`U}^1;2IBH*B881JDL}EG%t9OUgrmWkov%Ney+OuSPtxmaP}X!_BE8NJ z`aoaPBV6Za$koT+zu@W@u5rS({(Hqf^>5;7eZkc)T>Zj}^jaVES|7O9XC%e1>kF>) zKkD-J_b<5W`TKFq73&4Aa|QpR^cSvmfNS00+OI<{{>P&a`UTf`;2IB{yjU;rR_$nB z*wMV;MS7hF^g0i4orgtNpP#oLq`8HwU%18z*ZQ9n`_#XQC%;Fm|T=l1&{U@yc>42;Lg0ufO5XVQjzCV1|>F;-XxazMtz2(dS zSN#W0@AHLg{2wO0+4lLuRlg<;jIF!BX~Cp9fotF4Iwx?Q&n~ggd^Yjq7p{Kc>K9(5 z*ZD*rELV5Hbw1aofUTb6P7l|52AA&_oF1Rz;zy;m3Y$Am49cT z@u1gu;JV)7t=iGNu%mgwi}b$dRtNOKp11?9^Y(lyh^}|I&chj}_v;<5`g5-Sc}>&0 zgKJ)Jtpi;9dtU6Pr&$wEe&OmDu72S~daXbD;A(HDaIOCZS5Lp*;abl(oqo#sfUADl z)yKyQSN%Iqzs}i*>w3T9^nSg=bzQ)9eZjTPaP13R=i#cvlb&X9jR&srz;(UDTeYKk zVMp_V7wLTutv%?2>y_s^Z`WOY)|)V`3%L4)Yn*Vc|A%6q`Zw|97p{Kc>K9(5*ZQCj zma9A9TA$AJ@1<-%^y?9>>!sxMlg>U|^*u>%bM=c(57+h8=k$Jk!F3MdI?r&e8(ixQ z*Zyvkc<3)&Zk;FT6;v^+z9KEdGFN{byV~{rZJ# zJ?lyJL9I`RP5`d@SP^!o!`>-nM6 z`~3l~`p)!2Oq*LDD_r&b`)PZ>F-h?85w7}vXW#DyaL>%#kkkA90j}!^uImx5eSvEq z;W|HiB_8GnuJOP%9=Prg@K)_;Uf9vR;6-}hbE`l4U{Blu*ZCYt^|5)FaC*4T!-UiO z{Q<7}IahzbKfpCFxYhx#{hb&4^tXwp{Q<6i;p!J&q}TeR53ZJW3fKCdaP{>216=EQ z(&_#F09XAfS05iMT=l1&-mhP{&N*Dy3ta04*E++szb{HW^cSx2z%?GYu3vbob~G>S zXkPFlz3++53Ho49+yU45x!~%P*DlRDT>Zi|PPo?pRk2U~n|SgISHE!e3op`Zeb5J2 z8#{$-ecnv*+y3U)7hLCm+2y;@`GBkbva>(!^l;UG==76L4_Ezt=~sQ&-1^txdjGxO z>HYeJ>zu=Ny}-3EaP1>p=O;)!%nw}SfonW)UBB>F?Py-u(Y)YAdYw=7!S&Wo;X0q& zTzxi~Fs&oF`h{zpaIOCiu}}S*c=8KZzi{;nFVbs$&NsIFWj#QGdJS& zQ_enI^<%C+K32HuXPo}a&OTh%;~5vvq|?LI{#j?g$LZl}|D4nN^$yo{0oU~f*FM6v z?{J;B^AZp92G@At8V_98JG@mpniqC7FL;sO&xo}LeQ>?yJLT+Sn;v>;JB+r++^H z*Lq%cdcU8*RsVsjkB=3u`gN&Jw!clf9N?HYoySN)i)zyCZA*Sz3b2e|h4kl3fcO+4w~>KCql;YE6_KlWWU&q_S>7q0QZH6FOGUwEr_ zG%xIEUhpEl?}?2D`e0Ao0WUdbr>9+gwirmy;p!Kzal*C!FN%HY-^7y+u72U_7ha^- z`k)V%t2^K&$L#c_6ql|qxX%A2r=N7T;Htlz^fp(0P7l}n<2z38*B4yp5U%qK*Sf*A z&T#GTyAlumg=;);jR&sl3*M?7%?mr47raQX^MgLPUfC(UB>YOMkKP~QIuF;KzU*wl zRo|I@g{sxf-yh+c7hLNA*Z!^%`}DVor?}zj7p{KcMS872`rv9~r*N(R+7!R8U%1xu zey5*uKH#cf=j!8Qg{yv>)BE)c*ExsldVy=*;96(6_IHQGLx15K4_xDc>-vSaYDe?J zj^+g~()*rRJJAQ%ThDcVMqPcjnlQ~dT>Zi|PPo>8OzcztCZ7Dl)h}HA!i)4;AN0X; zbq8GQGm+xg^##}YKjrk3&OTiAFSzb%twyUzB+0 zFI?k+YdmmWU+`A#XkOUSyx>K8ogehU^~z4+IzKPDd6;n5z;zy8ar&~;!&SfJ>fdep zw2t7K7hLNA*Z#gK_UUgEPk!O*7p{KcMS872`rv9~r*N(Rva9El?i#q(^Rm-VIXztU z@4EWE=n;-SBAjR&srz;*q?TeYKkVMp_V7wLUZ ztexnC>#gTHKlj|;5L#YevC}l?aPT>Zk;FT6;v^+6vjS9id* zJ|V@g_eZ$S|3;_x>kF>>K35+fD_r$MPS5YMwlv@yTuyd6>hxcAufbKH{@bwp_h)W* zdbsN6o&CS%^l**;=>k1m_0JaQ;i^CF^!y(Y78cz98N2aINpJm~G>{Is+Q01VKY}wXAt)sE(c9nA|~r1xvc z>W@Cy6L-LMyoJzVvjcQiy+x1DyH_C2`f1=l*jwZCh`KK*Ut z$uC^}!qqRlNU!xrA1qgQz_tEsQ~bK$!nL0FJH6j;;i_Nf>f>XDtA5PcpLFrVRX^eE zf6eLP8yvIKX{Y!516AKR)j6@3)xJI)!UqaIFLUOOn@Vu}^=Sc=8KZ zzi{;nFVbuM(T9RKwf<*ZJ)d;fz_p&|oZjybaMi!!>f>XDtNu-=A8_%&b-iDA@k}~B zTHxCo;8o18GlGFSB1g`oYyZR5B{j{#(nipK_ z0N4Ir7W?$Ki6_5s^$S!gT?w1K1U*OtDxX#ZuiHG@tYdmm` z2d?`Ayj44z7j`r+c#+=s+}eXaxL$d#^Vy&3qwhE14~d--r}z5N-s-m7Y^8My z*Sz3b2e|fkLhRGuCZ7Dl)h}HA!i)4;fAqm}bq8GQKjZ4@_XoJvv+neMe}Jq0>7=*% z`&i+sKkw{My7=L$zu@fea(cM#53f4C-yh(*j^Mf;;o29t_7Sf0vn25_KX8o)uJOQi ze}K1YNAtpt<^?a(`<`2S&+ zu62NGf3Jys`rE{lU%2{(t6zALUh9uOSg!7XYyGdgdiwnVuJ!Cp|B0RT%kK|x)psSm z?hkO)uXFY%UHov>Z*=y*?(}foA9|eL?+63VvAGpQ?*LdK% zKfqhHqj_OR^MV)Ybw1Gt*DE`P>wNBT^DyDAf$KaBJH6i@;Hn>U_1|Om(>jG~UU01g zT>E=S?9<;Sp8UeqFI@e?i}YH5^ug8APT^YrX;)9bKftw~^G@&g2e|4_xcc~5;i^CB z>`%KK;Hp3C^pj2xSN#PS&t9j8Z*j;u z{le8RyhyM0K_6Ug>=dr`c_+nh_jkYE;ksU~IK5x*aMfRP`X^jWaMgd{>gm@PT;~w3 z^9*YyQ&)sE(c9nA|~q}TaDA6&2O6t43V^8Ylt@#jas zY{E3>aPIyS zu5rROFSypTPwZ3ACZ7Dl)h}HA!i)4;PxQgn(oW%8&mAuQUvbyK^?u#&^#1b#THYo#*L4cl^$yoMz_o60?booxL%-k}4_xDc>;408)sE(c9nA|~q}O>sA6#$k6t44d zDAmW-SH)ce*ZxjOJ=w3|S_in+6Rvr|wGMFY@3h22f8iPrT;qXjf8nj#(Y&yudBKbH z+F$g+^~z4++TRndKEozV`x0FJ!Zl8~*8gd-PyL&C@(WkLaP(rbOt2g}tRaIMc` zir?mc%<17e|EHaP(&^!{~xc2vw*r&fuJo$yIU%2{(7wNVB=!2_`ox-*Lmt8&m=P$U{^L?kEaz5ayzv}Aa zV}+~!dVzho>d&X&|JEL3+3(`v_wN~B>-dt>d;V4dzvTF%&JMqiPVK+z^xpmz$48u< zYmR&U^#Y!y--n}q*A(!z1^m7O9twD=fcF&eZ3Vo)fbT8fBL#e{fKL?gnF2mvz)uwL z#RC4U8YW zI-BPt$zL5x8sq7oxaDNd@!QjR{;+A`?;fp}C$mip^;+X`hO=5_uDq#UIZ@9xO_%HC zY}4ezLbfTVwQ7S*RgWI69E*Zu)p}*qp8bzKv`{ZkeLJZtGvkMA<)i-ny?5Qpd5Ge+ z_?4FT%BB73>^|SD$he;%wEy$ZSU(^=zO}A#a_zas;~nwA{G{7X#{VV#i-~tMWGmmt z#4j(H4S(3@TeDeoy~eB0ACA|WP3x;a@PEw@K@<=_TKgSrx61#*%MHa+3e57iU;WMX zrcz|#1H;vJh@f=C;q6R z*qelwzpWdtC%jwL-Y&V%-ay9V`Lx)&;<>$l*6ZzM?R`Yq*$3@=mVH4Iqi&5?v+VQn R + +#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 +#endif /* _STDINT_H */ + +#endif /* VL53L0X_TYPES_H_ */ diff --git a/wonderEcho_demo/CMakeLists.txt b/wonderEcho_demo/CMakeLists.txt new file mode 100644 index 0000000..4d7592a --- /dev/null +++ b/wonderEcho_demo/CMakeLists.txt @@ -0,0 +1,5 @@ +# demo2 +add_executable(wonderEcho_demo wonderEcho_demo.cpp) + +# 链接 OpenCV 库 +target_link_libraries(wonderEcho_demo) \ No newline at end of file diff --git a/wonderEcho_demo/wonderEcho_demo.cpp b/wonderEcho_demo/wonderEcho_demo.cpp new file mode 100644 index 0000000..9858885 --- /dev/null +++ b/wonderEcho_demo/wonderEcho_demo.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +std::mutex i2c_mutex; // 互斥锁，用于保护I2C设备访问 +std::atomic 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 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 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; +}