C路: VL53L0X 用户态I2C直驱 + 高速模式 29.6fps
- 搬运 ST API C 源码到 lib/vl53l0x/, 替换 I2C 层为用户态 i2c-dev - 新增 vl53l0x_platform_user.cpp: WriteMulti/ReadMulti 等全平台接口 - 重写 vl53l0x.cpp: start时unbind内核驱动, 单次测距, 无后台轮询 - 高速模式 20000μs timing budget, 模型缓存零污染(41-43ms) - LiDAR 每5帧一读(~20ms), 隔帧模型推理, fps 25.1→29.6 (+18%) - 同事的LiDAR避障集成: 12个配置参数, 中线变形绕行, 刹车注释 - nice -10 + sched_yield 优先级优化
This commit is contained in:
spdis
@@ -38,6 +38,8 @@ These source files exist but are **excluded from build**:
|
|||||||
|
|
||||||
- `vl53l0x.cpp` — laser ranging module, hardware not connected
|
- `vl53l0x.cpp` — laser ranging module, hardware not connected
|
||||||
- `zebra_detect.cpp` — classic zebra-crossing detection (replaced by Mild model)
|
- `zebra_detect.cpp` — classic zebra-crossing detection (replaced by Mild model)
|
||||||
|
- `PIDController.cpp` — unused (motor is open-loop direct drive)
|
||||||
|
- `serial.cpp` — Vofa/serial image streaming, unused
|
||||||
|
|
||||||
## Device-side operation
|
## Device-side operation
|
||||||
|
|
||||||
@@ -52,7 +54,7 @@ sh ctl.sh stop # stop and zero PWM
|
|||||||
|
|
||||||
`ctl.sh` sets default config values by writing to files like `./speed`, `./kp`, `./steer_gain`, etc. The running binary reads these files at startup and optionally re-reads them every 7 frames (when `./debug` = 1).
|
`ctl.sh` sets default config values by writing to files like `./speed`, `./kp`, `./steer_gain`, etc. The running binary reads these files at startup and optionally re-reads them every 7 frames (when `./debug` = 1).
|
||||||
|
|
||||||
`start.sh` is an alternative launcher using `LD_PRELOAD=./gpio_fix_final.so` for the GPIO 74 workaround.
|
`start.sh` is an alternative launcher using `LD_PRELOAD=./gpio_fix_final.so` for the GPIO 74 workaround. **Note**: `start.sh` references the binary as `smartcar_demo` but the actual executable is `smartcar_demo1` — this is a known bug, use `ctl.sh` instead.
|
||||||
|
|
||||||
## Configuration system
|
## Configuration system
|
||||||
|
|
||||||
@@ -94,7 +96,7 @@ All config is via single-value text files in the working directory:
|
|||||||
|
|
||||||
## Architecture
|
## Architecture
|
||||||
|
|
||||||
### Per-frame pipeline in `CameraHandler()` ([`src/camera.cpp`](file:///D:\PPPProgram\smartcar\smartcar2\src\camera.cpp))
|
### Per-frame pipeline in `CameraHandler()` (`src/camera.cpp:578`)
|
||||||
|
|
||||||
CameraHandler 现已拆分为多个函数，主函数 ~40 行仅做编排：
|
CameraHandler 现已拆分为多个函数，主函数 ~40 行仅做编排：
|
||||||
|
|
||||||
@@ -117,7 +119,7 @@ CameraHandler()
|
|||||||
|
|
||||||
- **Open-loop**: `MotorController::updateduty(spd)` — direct PWM duty cycle, no encoder feedback in normal driving.
|
- **Open-loop**: `MotorController::updateduty(spd)` — direct PWM duty cycle, no encoder feedback in normal driving.
|
||||||
- **Encoder brake**: When `zebra_block=true` (red light or zebra stop), reads `g_enc_speed` from the encoder thread and applies reverse braking proportional to current speed.
|
- **Encoder brake**: When `zebra_block=true` (red light or zebra stop), reads `g_enc_speed` from the encoder thread and applies reverse braking proportional to current speed.
|
||||||
- **Encoder thread** ([`control.cpp:32`](file:///D:\PPPProgram\smartcar\smartcar2\src\control.cpp#L32)): polls GPIO67 (LSB pulse) + GPIO72 (direction) at ~2kHz, 100ms window speed calculation → `g_enc_speed` (pulses/sec). Now includes 500µs sleep to prevent CPU starvation.
|
- **Encoder thread** (`control.cpp:32`): polls GPIO67 (LSB pulse) + GPIO72 (direction) at ~2kHz, 100ms window speed calculation → `g_enc_speed` (pulses/sec). Now includes 500µs sleep to prevent CPU starvation.
|
||||||
- **Curve slowdown**: `speed *= (1.0 - |deviation| × 0.4)`, clamped to ≥ 60%.
|
- **Curve slowdown**: `speed *= (1.0 - |deviation| × 0.4)`, clamped to ≥ 60%.
|
||||||
|
|
||||||
### Model: Mild v12
|
### Model: Mild v12
|
||||||
|
|||||||
+11
-2
@@ -21,6 +21,7 @@ message(STATUS "OpenCV Include Directories: ${OpenCV_INCLUDE_DIRS}")
|
|||||||
# 项目头文件路径
|
# 项目头文件路径
|
||||||
include_directories(src)
|
include_directories(src)
|
||||||
include_directories(lib)
|
include_directories(lib)
|
||||||
|
include_directories(lib/vl53l0x)
|
||||||
|
|
||||||
# 收集 src 下所有 .cpp (自动发现)
|
# 收集 src 下所有 .cpp (自动发现)
|
||||||
aux_source_directory(src DIR_SRCS)
|
aux_source_directory(src DIR_SRCS)
|
||||||
@@ -30,8 +31,16 @@ aux_source_directory(src DIR_SRCS)
|
|||||||
# zebra_detect.cpp — 经典斑马线检测, 已被 Mild 模型替代
|
# zebra_detect.cpp — 经典斑马线检测, 已被 Mild 模型替代
|
||||||
# PIDController.cpp — PID 类未使用 (电机开环直驱)
|
# PIDController.cpp — PID 类未使用 (电机开环直驱)
|
||||||
# serial.cpp — Vofa/串口图传未使用
|
# serial.cpp — Vofa/串口图传未使用
|
||||||
list(FILTER DIR_SRCS EXCLUDE REGEX "(vl53l0x\\.cpp|zebra_detect\\.cpp|PIDController\\.cpp|serial\\.cpp)")
|
list(FILTER DIR_SRCS EXCLUDE REGEX "(zebra_detect\\.cpp|PIDController\\.cpp|serial\\.cpp)")
|
||||||
|
|
||||||
|
# VL53L0X ST API 源码 (纯 C)
|
||||||
|
set(VL53L0X_API_SRCS
|
||||||
|
lib/vl53l0x/vl53l0x_api.c
|
||||||
|
lib/vl53l0x/vl53l0x_api_core.c
|
||||||
|
lib/vl53l0x/vl53l0x_api_calibration.c
|
||||||
|
lib/vl53l0x/vl53l0x_api_ranging.c
|
||||||
|
)
|
||||||
|
|
||||||
# 静态库 + 主程序
|
# 静态库 + 主程序
|
||||||
add_library(common_lib STATIC ${DIR_SRCS})
|
add_library(common_lib STATIC ${DIR_SRCS} ${VL53L0X_API_SRCS})
|
||||||
add_subdirectory(main)
|
add_subdirectory(main)
|
||||||
|
|||||||
@@ -49,6 +49,19 @@ init_pins() {
|
|||||||
echo 10 > "$DIR/brake_scale" 2>/dev/null
|
echo 10 > "$DIR/brake_scale" 2>/dev/null
|
||||||
echo 10000 > "$DIR/brake_max" 2>/dev/null
|
echo 10000 > "$DIR/brake_max" 2>/dev/null
|
||||||
|
|
||||||
|
echo 300 > "$DIR/lidar_thresh" 2>/dev/null
|
||||||
|
echo 50 > "$DIR/lidar_near" 2>/dev/null
|
||||||
|
echo 1200 > "$DIR/lidar_far" 2>/dev/null
|
||||||
|
echo 1 > "$DIR/lidar_near_start" 2>/dev/null
|
||||||
|
echo 4 > "$DIR/lidar_near_end" 2>/dev/null
|
||||||
|
echo 6 > "$DIR/lidar_far_span" 2>/dev/null
|
||||||
|
echo 3 > "$DIR/lidar_min_frames" 2>/dev/null
|
||||||
|
echo 0.4 > "$DIR/lidar_avoid_gain" 2>/dev/null
|
||||||
|
echo 30 > "$DIR/lidar_avoid_range" 2>/dev/null
|
||||||
|
echo 0.4 > "$DIR/lidar_speed" 2>/dev/null
|
||||||
|
echo 30 > "$DIR/lidar_hold_frames" 2>/dev/null
|
||||||
|
echo 1 > "$DIR/lidar_enable" 2>/dev/null
|
||||||
|
|
||||||
echo "[demo] 引脚初始化完成"
|
echo "[demo] 引脚初始化完成"
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
+243
-134
@@ -3,119 +3,98 @@
|
|||||||
## 坐标系统一览
|
## 坐标系统一览
|
||||||
|
|
||||||
```
|
```
|
||||||
Camera 输出 640×480 MJPEG
|
Camera 输出 640×480 MJPEG
|
||||||
│
|
│
|
||||||
CONVERT_RGB=0 + IMREAD_REDUCED_COLOR_4
|
CONVERT_RGB=0 + IMREAD_REDUCED_COLOR_4
|
||||||
→ 1/4 MJPEG 解码 → raw_frame = 160×120
|
→ 1/4 MJPEG 解码 → raw_frame = 160×120
|
||||||
│
|
│
|
||||||
┌───────────┤
|
┌───────────┤
|
||||||
▼ ▼ (raw_frame.cols/rows 决定分母)
|
▼ ▼ (raw_frame.cols/rows 决定分母)
|
||||||
巡线空间 显示空间 模型空间
|
巡线空间 显示空间 模型空间
|
||||||
(line_tracking (newWidth × newHeight) 160×120 (正常)
|
(line_tracking (newWidth × newHeight) 160×120 (正常)
|
||||||
_width × calc_scale = 2 640×480 (回退模式)
|
_width × calc_scale = 2 640×480 (回退模式)
|
||||||
line_tracking newWidth = lt_w × 2
|
line_tracking newWidth = lt_w × 2
|
||||||
_height) newHeight = lt_h × 2
|
_height) newHeight = lt_h × 2
|
||||||
典型值 80×60
|
典型值 80×60
|
||||||
```
|
```
|
||||||
|
|
||||||
**关键换算关系（正常模式 raw_frame = 160×120）：**
|
**关键换算关系（正常模式 raw_frame = 160×120）：**
|
||||||
- 模型空间 → 巡线空间：`col_lt = cx * line_tracking_width / 160`，`row_lt = cy * line_tracking_height / 120`
|
- 模型空间 → 巡线空间：`col_lt = cx * line_tracking_width / 160`，`row_lt = cy * line_tracking_height / 120`
|
||||||
- 巡线空间 → 显示空间：`pixel = value * calc_scale`（calc_scale = 2）
|
- 巡线空间 → 显示空间：`pixel = value * calc_scale`（calc_scale = 2）
|
||||||
- newWidth / newHeight 取决于屏幕物理分辨率，由 `CameraInit` 动态计算
|
- `line_tracking_width / height` = `newWidth / calc_scale`, `newHeight / calc_scale`
|
||||||
- line_tracking_width / height = newWidth/calc_scale, newHeight/calc_scale
|
- newWidth / newHeight 由 `CameraInit` 读取 `/dev/fb0` 屏幕分辨率后动态计算
|
||||||
|
|
||||||
**⚠ 回退模式：** 当 CONVERT_RGB=0 未生效时，raw_frame = 640×480，模型框坐标也在 640×480 空间。
|
**⚠ 回退模式：** 当 CONVERT_RGB=0 未生效时，raw_frame = 640×480，模型框坐标也在 640×480 空间。
|
||||||
此时 LCD 渲染仍硬编码除以 160/120（会出错），但正常运行时不会进入此模式。
|
此时 LCD 渲染的硬编码缩放 `newWidth/160.0f` 会出错，但正常运行时不会进入此模式。
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 主循环 (main.cpp → CameraHandler)
|
## 主循环 (`main.cpp`)
|
||||||
|
|
||||||
```
|
```
|
||||||
main()
|
main()
|
||||||
├── cfg_load_all() # 从工作目录文件读取全部配置
|
├── cfg_load_all() # 从工作目录文件读取全部配置
|
||||||
├── CameraInit(0, dest_fps, 320, 240) # 打开摄像头, LCD, 模型, I2C
|
├── 补读 cone_avoid_gain/range/hold_frames
|
||||||
├── ControlInit() # 初始化双电机 GPIO + PWM
|
├── CameraInit(0) # 打开摄像头, LCD, 模型, I2C, 计算巡线尺寸
|
||||||
|
├── ControlInit() # 初始化双电机 GPIO + PWM + 启动编码器线程
|
||||||
│
|
│
|
||||||
└── while(running):
|
└── while(running):
|
||||||
└── CameraHandler() # ★ 以下逐帧执行
|
├── CameraHandler() # ★ 逐帧执行
|
||||||
|
└── target_speed = g_cfg.speed # (debug 模式下每 7 帧重载)
|
||||||
|
```
|
||||||
|
|
||||||
|
`CameraInit` 现在只接受 `camera_id` 一个参数（之前有 dest_fps, width, height 三个遗留参数已移除）。
|
||||||
|
巡线分辨率由屏幕自适应算法决定：`line_tracking_w = newWidth/2`, `line_tracking_h = newHeight/2`。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## CameraHandler 11 步流水线
|
||||||
|
|
||||||
|
```
|
||||||
|
Step 1 capture_frame() 640×480 MJPEG → IMREAD_REDUCED_COLOR_4
|
||||||
|
取帧+解码 → raw_frame = 160×120 BGR
|
||||||
|
(回退: raw_mat 三通道 → 直用 640×480)
|
||||||
|
|
||||||
|
Step 2 save_image_if_requested() debug=1 && saveImg 文件=1 → 存 ./image/XXXXX.jpg
|
||||||
|
保存图像 (条件)
|
||||||
|
|
||||||
|
Step 3 image_main() raw_frame → resize(lt_w×lt_h) → HSV 双通道Otsu
|
||||||
|
视觉巡线 → floodFill(种子 (lt_w/2, lt_h-10), 半径5)
|
||||||
|
→ 逐行最长连续段搜索 → 丢线补全(row 10~59)
|
||||||
|
输出: left_line[], right_line[], mid_line[]
|
||||||
|
|
||||||
|
Step 4 run_model_inference() 每 2 帧推理一次
|
||||||
|
模型推理 raw_frame (160×120) → model_v10_detect()
|
||||||
|
4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线
|
||||||
|
g_thresh = {0.90, 0.75, 0.80, 0.90}
|
||||||
|
→ g_boxes[16], g_box_count
|
||||||
|
|
||||||
|
Step 5a zebra_process() 去抖计数 + ZNORMAL→ZSTOP(4s)→ZCOOLDOWN(5s)
|
||||||
|
斑马线状态机 触发: 连续5帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee)
|
||||||
|
刹停时 I2C 语音播报
|
||||||
|
|
||||||
|
Step 5b traffic_light_process() TLNORMAL→TLSTOP→TLWAIT_GREEN
|
||||||
|
红绿灯状态机 红灯≥3帧 → 停车; 红灯消失 → 等绿灯; 绿灯≥3帧 → 通行
|
||||||
|
|
||||||
|
Step 5c cone_detect_and_deform() 去抖确认 + 中线变形 + 消失后保持衰减
|
||||||
|
锥桶检测 & 中线变形 cone_speed 减速 + cone_hold_frames 保持
|
||||||
|
|
||||||
|
Step 6 steering_update() foresee/2 → check_row → mid_line[row]×2 - newWidth/2
|
||||||
|
舵机控制 deadband 过滤 → servo duty: 1500000 ± offset ns
|
||||||
|
|
||||||
|
Step 7 motor_update() 开环PWM + 编码器比例刹车 + 弯道减速 + 锥桶减速
|
||||||
|
电机控制
|
||||||
|
|
||||||
|
Step 8 lcd_render() track→BGR→ROI + 边界线(红/绿/蓝) + 检测框 + 状态指示
|
||||||
|
LCD 渲染 RGB565 → /dev/fb0 mmap
|
||||||
|
|
||||||
|
Step 9 fps_log() 每 15 帧输出总帧率 + 分步耗时 ms
|
||||||
|
FPS 统计
|
||||||
```
|
```
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## CameraHandler 9 步流水线
|
## 视觉巡线深度展开 (`image_main`)
|
||||||
|
|
||||||
```
|
|
||||||
┌─────────────────────┐
|
|
||||||
Step 1 │ cap.read(raw_mat) │ 640×480 MJPEG 原始字节流
|
|
||||||
取帧+解码 │ raw_mat 单通道(字节) │ → IMREAD_REDUCED_COLOR_4
|
|
||||||
│ → cv::imdecode │ → raw_frame = 160×120 BGR
|
|
||||||
│ raw_frame = decoded │ (回退: raw_mat 三通道→直用 640×480)
|
|
||||||
└────────┬────────────┘
|
|
||||||
│
|
|
||||||
Step 2 ┌────────▼────────────┐
|
|
||||||
保存图像 (条件) │ g_cfg.debug==1 │ 写入 ./image/image_XXXXX.jpg
|
|
||||||
│ && saveImg==1 → │ (双重条件，缺一不可)
|
|
||||||
│ saveCameraImage() │
|
|
||||||
└────────┬────────────┘
|
|
||||||
│
|
|
||||||
Step 3 ┌────────▼────────────┐
|
|
||||||
视觉巡线 │ image_main() │ raw_frame → resize(lt_w×lt_h)
|
|
||||||
(每帧都跑) │ │ → HSV双通道Otsu → floodFill
|
|
||||||
│ 输出: left_line[] │ → 逐行最长连续段搜索
|
|
||||||
│ right_line[] │ → 丢线补全(仅 row 10~59)
|
|
||||||
│ mid_line[] │ → track (赛道蒙版 128/0)
|
|
||||||
└────────┬────────────┘
|
|
||||||
│
|
|
||||||
Step 4 ┌────────▼────────────┐
|
|
||||||
模型推理 │ model_v10_detect() │ 每 2 帧推理一次
|
|
||||||
(每 2 帧) │ raw_frame (160×120) │ 4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线
|
|
||||||
│ → g_boxes[16] │ g_thresh = [0.80,0.80,0.80,0.75]
|
|
||||||
│ → g_box_count │ 框坐标 ∈ raw_frame 空间
|
|
||||||
└────────┬────────────┘
|
|
||||||
│
|
|
||||||
Step 5 ┌────────▼────────────┐
|
|
||||||
斑马线状态机 │ 遍历 g_boxes │ 仅处理 cls=3，取第一个斑马线框
|
|
||||||
│ 去抖计数+远近判断 │ NORMAL→STOP(4s)→COOLDOWN(5s)
|
|
||||||
│ │ I2C 语音播报 @ 0x34
|
|
||||||
└────────┬────────────┘
|
|
||||||
│
|
|
||||||
Step 6 ┌────────▼────────────┐
|
|
||||||
舵机控制 │ if g_cfg.start: │
|
|
||||||
│ foresee→check_row │ 偏差 = mid_line[row]×2 - newWidth/2
|
|
||||||
│ mid_line[check_row]│ → g_steer_deviation ∈ [-1, 1]
|
|
||||||
│ → deviation │ → servo duty: 1500000 ± offset
|
|
||||||
│ → deadband 过滤 │ clamp [1.2M, 1.8M] ns
|
|
||||||
│ → servo.setDuty()│ mid_val==255 → 跳过(无效行)
|
|
||||||
└────────┬────────────┘
|
|
||||||
│
|
|
||||||
Step 7 ┌────────▼────────────┐
|
|
||||||
电机控制 │ ControlUpdate( │
|
|
||||||
(开环) │ target_speed, │ if zebra_STOP: duty=0 (刹停)+GPIO73=0
|
|
||||||
│ g_zstate==Z_STOP) │ else: speed × curve
|
|
||||||
│ │ curve = 1 - |deviation|×0.4
|
|
||||||
│ │ curve ∈ [0.6, 1.0]
|
|
||||||
│ │ 左右电机同速(无差速)
|
|
||||||
└────────┬────────────┘
|
|
||||||
│
|
|
||||||
Step 8 ┌────────▼────────────┐
|
|
||||||
LCD 渲染 │ if g_lcd_on: │ g_lcd_on = g_cfg.showImg 的缓存
|
|
||||||
│ track→BGR→ROI │ (每10帧刷新一次缓存)
|
|
||||||
│ + 边界线(红) │ 检测框坐标 bx=newWidth/160 缩放
|
|
||||||
│ + 中线(蓝) │ 状态指示 N(绿)/S(红)/C(黄)
|
|
||||||
│ + 检测框标注 │ → RGB565 → /dev/fb0 mmap
|
|
||||||
│ (L1条状:跳过红灯) │
|
|
||||||
└────────┬────────────┘
|
|
||||||
│
|
|
||||||
Step 9 ┌────────▼────────────┐
|
|
||||||
FPS 统计 │ 每 15 帧输出 │ stdout: "fps=XX.X|rd=X vi=X md=X ct=X ms"
|
|
||||||
│ 分步计时+总帧率 │ 15帧平均
|
|
||||||
└─────────────────────┘
|
|
||||||
```
|
|
||||||
|
|
||||||
---
|
|
||||||
|
|
||||||
## 视觉巡线深度展开 (image_main)
|
|
||||||
|
|
||||||
```
|
```
|
||||||
raw_frame (160×120 或 640×480 BGR)
|
raw_frame (160×120 或 640×480 BGR)
|
||||||
@@ -130,7 +109,7 @@ raw_frame (160×120 或 640×480 BGR)
|
|||||||
│
|
│
|
||||||
├── find_road():
|
├── find_road():
|
||||||
│ MORPH_OPEN(2×2 CROSS) → 种子点(lt_w/2, lt_h-10)
|
│ MORPH_OPEN(2×2 CROSS) → 种子点(lt_w/2, lt_h-10)
|
||||||
│ → 种子点处画实心圆(半径10, 255) 防种子落在黑色区域
|
│ → 种子点处画实心圆(半径5, 255) 防种子落在黑色区域
|
||||||
│ → floodFill(loDiff=20, upDiff=20, 8邻域, newVal=128)
|
│ → floodFill(loDiff=20, upDiff=20, 8邻域, newVal=128)
|
||||||
│ → mask 提取 ROI → track (赛道内部=128, 外部=0)
|
│ → mask 提取 ROI → track (赛道内部=128, 外部=0)
|
||||||
│
|
│
|
||||||
@@ -141,28 +120,29 @@ raw_frame (160×120 或 640×480 BGR)
|
|||||||
│ → left_line[row], right_line[row]
|
│ → left_line[row], right_line[row]
|
||||||
│ → 全零行: left=right=-1
|
│ → 全零行: left=right=-1
|
||||||
│
|
│
|
||||||
└── 中线 + 丢线补全 (row = lt_h-1 → 10):
|
└── 中线 + 丢线补全 (row = lt_h-2 → 10):
|
||||||
有边界: mid = (left+right)/2, int 整除
|
有边界: mid = (left+right)/2, int 整除
|
||||||
丢线: mid[row] = mid[row+1] (继承下行)
|
丢线: mid[row] = mid[row+1] (继承下行)
|
||||||
若 mid > lt_w/2 → left=mid, right=lt_w-1 (偏右)
|
若 mid > lt_w/2 → left=mid, right=lt_w-1 (偏右)
|
||||||
若 mid ≤ lt_w/2 → left=0, right=mid (偏左)
|
若 mid ≤ lt_w/2 → left=0, right=mid (偏左)
|
||||||
|
底行兜底: 丢线时用 lt_w/2
|
||||||
```
|
```
|
||||||
|
|
||||||
**边界线区间有效性：** row 10~59 有可靠数据；row 0~9 的 mid_line 保持初始值 -1（不可靠，不要写入 box 过滤逻辑）。
|
**边界线区间有效性：** row 10~59 有数据；row 0~9 的 mid_line 保持初始值 -1（不可靠）。
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 舵机控制数学
|
## 舵机控制数学
|
||||||
|
|
||||||
```
|
```
|
||||||
foresee = g_cfg.foresee ← 前瞻行索引（像素，显示空间），默认 40
|
foresee = g_cfg.foresee ← 前瞻行索引（显示空间像素），默认 40
|
||||||
check_row = foresee / calc_scale ← 转为巡线空间行号（calc_scale=2, int 整除）
|
check_row = (int)foresee / calc_scale ← 转为巡线空间行号（calc_scale=2, int 整除）
|
||||||
mid_val = mid_line[check_row] ← 该行中线列坐标 (0~79)
|
mid_val = mid_line[check_row] ← 该行中线列坐标 (0~79)
|
||||||
|
|
||||||
如果 mid_val == 255 → 跳过（丢线补全也写不到 255，仅作防御）
|
如果 mid_val == -1 → 跳过（丢线行无效）
|
||||||
否则：
|
否则：
|
||||||
deviation = mid_val × 2 - newWidth / 2 ← 转为显示空间偏差(px)
|
deviation = mid_val × 2 - newWidth / 2 ← 转为显示空间偏差(px)
|
||||||
deviation -= g_cfg.center_bias ← 中心偏置修正
|
deviation -= g_cfg.center_bias ← 中心偏置修正
|
||||||
g_steer_deviation = deviation / (newWidth/2) ← 归一化到 [-1, 1]
|
g_steer_deviation = deviation / (newWidth/2) ← 归一化到 [-1, 1]
|
||||||
|
|
||||||
if |deviation| < deadband:
|
if |deviation| < deadband:
|
||||||
@@ -179,19 +159,32 @@ mid_val = mid_line[check_row] ← 该行中线列坐标 (0~79)
|
|||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 电机控制数学
|
## 电机控制数学 (`ControlUpdate`)
|
||||||
|
|
||||||
```
|
```
|
||||||
ControlUpdate(speed, zebra_block):
|
ControlUpdate(speed, zebra_or_tl_block):
|
||||||
|
|
||||||
if zebra_block || !g_cfg.start:
|
if !g_cfg.start:
|
||||||
motor[0].updateduty(0) → 两电机停转
|
motor[0].updateduty(0) → 两电机停转
|
||||||
motor[1].updateduty(0)
|
motor[1].updateduty(0)
|
||||||
if !g_cfg.start: mortorEN.setValue(0) → GPIO73 拉低
|
mortorEN.setValue(0) → GPIO73 拉低
|
||||||
return
|
return
|
||||||
|
|
||||||
curve = 1.0 - |g_steer_deviation| × 0.4
|
if zebra_or_tl_block: ← 斑马线或红灯刹停
|
||||||
curve = max(curve, 0.6) ← 最低不低于 60% 速度
|
读取 g_enc_speed (编码器线程, 脉冲/秒)
|
||||||
|
读取 g_enc_dir (编码器方向)
|
||||||
|
if cur_speed > 0.5:
|
||||||
|
brake_ns = cur_speed × g_cfg.brake_scale ← 比例刹车
|
||||||
|
brake_ns = clamp(brake_ns, 0, g_cfg.brake_max)
|
||||||
|
brake_pct = brake_ns / 500 ← ns → % (period=50000ns)
|
||||||
|
dir = cur_dir ? 1 : 0
|
||||||
|
motor[i]->updateduty(dir ? -brake_pct : brake_pct) ← 反转刹车
|
||||||
|
else:
|
||||||
|
motor[i]->updateduty(0) ← 已静止，不刹车
|
||||||
|
return
|
||||||
|
|
||||||
|
curve = 1.0 - |g_steer_deviation| × 0.4 ← 弯道减速
|
||||||
|
curve = max(curve, 0.6) ← 最低 ≥ 60%
|
||||||
spd = speed × curve
|
spd = speed × curve
|
||||||
|
|
||||||
motor[0].updateduty(spd) → 左电机 (pwmchip8/pwm2, gpio12 方向)
|
motor[0].updateduty(spd) → 左电机 (pwmchip8/pwm2, gpio12 方向)
|
||||||
@@ -199,22 +192,31 @@ ControlUpdate(speed, zebra_block):
|
|||||||
mortorEN.setValue(1) → GPIO73 拉高使能
|
mortorEN.setValue(1) → GPIO73 拉高使能
|
||||||
```
|
```
|
||||||
|
|
||||||
|
**编码器刹车（encoder brake）：** 当斑马线或红灯触发停车时，根据实时编码器速度计算反向刹车占空比，
|
||||||
|
刹车力度与当前车速成正比（`brake_scale`），有上限（`brake_max` ns）。
|
||||||
|
速度低于 0.5 pps 时不再施加刹车。
|
||||||
|
|
||||||
|
**motor_update 额外逻辑：** `motor_update()` 在调用 `ControlUpdate` 之前，若锥桶已触发（`cone_is_slow()`），
|
||||||
|
会将 speed 乘以 `g_cfg.cone_speed`（默认 0.5），实现锥桶路段减速。
|
||||||
|
|
||||||
**motor[0] vs motor[1]：** 左/右区别仅在于 gpioNum（12 vs 13）和 pwm通道（2 vs 1），函数调用完全相同。
|
**motor[0] vs motor[1]：** 左/右区别仅在于 gpioNum（12 vs 13）和 pwm通道（2 vs 1），函数调用完全相同。
|
||||||
|
|
||||||
**updateduty(duty) 内部：**
|
**updateduty(duty) 内部：**
|
||||||
- `pwm_period * |duty| / 100` → 转占空比 ns 写入 sysfs
|
- `pwm_period * |duty| / 100` → 转占空比 ns 写入 sysfs
|
||||||
- duty > 0 → GPIO 方向 = 1（正转），duty ≤ 0 → GPIO 方向 = 0（反转）
|
- duty > 0 → GPIO 方向 = 1（正转），duty ≤ 0 → GPIO 方向 = 0（反转）
|
||||||
|
|
||||||
**速度控制是开环的：** `MotorController` 仅包含 `updateduty()` 方法，无编码器反馈。
|
**速度控制是开环的：** `MotorController` 仅包含 `updateduty()` 方法，正常行驶无编码器反馈。
|
||||||
`PIDController` 类存在但从未被实例化或调用。
|
`PIDController` 类存在但从未被实例化或调用。
|
||||||
MotorController.h 中也无 `updateSpeed()` 方法（之前的文档中误记了此函数）。
|
|
||||||
|
**编码器线程** (`ControlInit` → `encoder_thread`)：独立线程高频轮询 GPIO67（LSB脉冲）+ GPIO72（方向），
|
||||||
|
每 100ms 计算一次速度 → `g_enc_speed`（脉冲/秒），含 500µs sleep 防止 CPU 饿死。
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 斑马线状态机
|
## 斑马线状态机
|
||||||
|
|
||||||
```
|
```
|
||||||
检测到 cls=3 → 取第一个斑马线框 → 去抖计数+追踪最远cy
|
检测到 cls=3 → 取第一个斑马线框（之后 break）→ 去抖计数+追踪最远cy
|
||||||
|
|
||||||
ZNORMAL 期间:
|
ZNORMAL 期间:
|
||||||
zebra_seen: g_zc_frames++, g_zc_min_cy = min(g_zc_min_cy, zebra_cy)
|
zebra_seen: g_zc_frames++, g_zc_min_cy = min(g_zc_min_cy, zebra_cy)
|
||||||
@@ -231,14 +233,14 @@ ZNORMAL 期间:
|
|||||||
│NORMAL│ ─────────────────────────────────────► ┌──────┐
|
│NORMAL│ ─────────────────────────────────────► ┌──────┐
|
||||||
│ │ ◄───────────────────────────────────── │ STOP │
|
│ │ ◄───────────────────────────────────── │ STOP │
|
||||||
└──────┘ 冷却5秒结束 │ │
|
└──────┘ 冷却5秒结束 │ │
|
||||||
▲ └──┬───┘
|
▲ └──┬───┘
|
||||||
│ 4秒后 │
|
│ 4秒后 │
|
||||||
│ ┌──────────┐ ◄─────────────────────┘
|
│ ┌──────────┐ ◄─────────────────────┘
|
||||||
└───────────│ COOLDOWN │
|
└──────────│ COOLDOWN │
|
||||||
└──────────┘
|
└──────────┘
|
||||||
|
|
||||||
NORMAL: 允许通行，检测斑马线
|
NORMAL: 允许通行，检测斑马线
|
||||||
STOP: 刹停 4 秒，g_zstate==Z_STOP 传给 ControlUpdate 第二个参数
|
STOP: 刹停 4 秒，g_zstate==Z_STOP 传给 motor_update → 编码器刹车
|
||||||
COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触发）
|
COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触发）
|
||||||
仅跳过检测，不影响其他功能（舵机/巡线正常）
|
仅跳过检测，不影响其他功能（舵机/巡线正常）
|
||||||
```
|
```
|
||||||
@@ -247,14 +249,99 @@ COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触
|
|||||||
- `g_zc_min_cy`：NORMAL 期间追踪斑马线出现时的最小 cy（越远值越小）。未检测到斑马线时衰减减2/帧，归零后重置为 120。
|
- `g_zc_min_cy`：NORMAL 期间追踪斑马线出现时的最小 cy（越远值越小）。未检测到斑马线时衰减减2/帧，归零后重置为 120。
|
||||||
- `ZEBRA_FAR_CY = 50`：斑马线的 cy 必须曾在 ≤50 处出现过（即"从远处来"）
|
- `ZEBRA_FAR_CY = 50`：斑马线的 cy 必须曾在 ≤50 处出现过（即"从远处来"）
|
||||||
- `zebrasee`（默认 60）：当前斑马线 cy > 此值视为"足够近"，触发停车
|
- `zebrasee`（默认 60）：当前斑马线 cy > 此值视为"足够近"，触发停车
|
||||||
- 去抖衰减速度：未检测到时每次 `-2`（比锥桶设计中的 `-1` 更快下降）
|
- 去抖衰减速度：未检测到时每次 `-2`（比锥桶设计的 `-1` 更快下降）
|
||||||
- Box 遍历在找到第一个 cls=3 后 `break`，忽略同帧其他斑马线框
|
- Box 遍历在找到第一个 cls=3 后 `break`，忽略同帧其他斑马线框
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
## 红绿灯状态机
|
||||||
|
|
||||||
|
```
|
||||||
|
检测到 cls=1 (红灯) / cls=2 (绿灯) → 去抖计数 → 状态转移
|
||||||
|
|
||||||
|
┌──────┐ 红灯≥3帧 ┌──────┐ 红灯消失 ┌───────────┐
|
||||||
|
│NORMAL│ ───────────► │ STOP │ ──────────► │WAIT_GREEN │
|
||||||
|
│ │ ◄─────────── │ │ │ │
|
||||||
|
└──────┘ 绿灯≥3帧 └──────┘ └─────┬─────┘
|
||||||
|
▲ │
|
||||||
|
└───────────────────────────────────────────┘
|
||||||
|
|
||||||
|
TL_NORMAL: 允许通行，检测红绿灯
|
||||||
|
TL_STOP: 红灯→停车（编码器刹车），等红灯消失
|
||||||
|
TL_WAIT_GREEN: 红灯已消失，等待绿灯出现（≥3帧）→ 恢复通行
|
||||||
|
```
|
||||||
|
|
||||||
|
**返回值：** `traffic_light_process()` 返回 `true` 当 `g_tl_state != TL_NORMAL`，
|
||||||
|
传递给 `motor_update` → `ControlUpdate` 触发编码器刹车。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 锥桶检测 & 中线变形
|
||||||
|
|
||||||
|
```
|
||||||
|
cone_detect_and_deform():
|
||||||
|
(仅在 Z_NORMAL && TL_NORMAL 时运行)
|
||||||
|
|
||||||
|
┌─ 寻找最近锥桶 ─┐
|
||||||
|
│ 遍历 g_boxes, cls=0, conf >= cone_thresh
|
||||||
|
│ 模型空间坐标 → 巡线空间坐标
|
||||||
|
│ 过滤: rl < 10 或 无边界线的行 → 跳过
|
||||||
|
│ cone_margin==0 → 中心点; ==1 → 框边缘 (左右均在边界线内)
|
||||||
|
│ 取 cy 最大(最近)的锥桶
|
||||||
|
│
|
||||||
|
├─ 去抖确认 ──────────────────────────────
|
||||||
|
│ 位置容忍: row_tol = max(2, lt_h/12), col_tol = max(3, lt_w/8)
|
||||||
|
│ 连续出现且位置接近 → g_cone_frames++
|
||||||
|
│ 丢失 → g_cone_frames = max(0, g_cone_frames - 1)
|
||||||
|
│ g_cone_confirmed = (g_cone_frames >= cone_min_frames)
|
||||||
|
│
|
||||||
|
├─ 保持/衰减 ────────────────────────────
|
||||||
|
│ 确认时: g_cone_hold_ctr=0, 记录锥桶位置
|
||||||
|
│ 未确认但有历史: g_cone_hold_ctr++, 衰减推离量
|
||||||
|
│ 超过 cone_hold_frames → 停止影响
|
||||||
|
│
|
||||||
|
└─ 中线变形 ─────────────────────────────
|
||||||
|
dir = (锥桶偏左) ? +1.0 : -1.0 ← 向远离锥桶方向推
|
||||||
|
decay = (确认) 1.0 : (1 - hold_ctr/hold_frames)
|
||||||
|
for row = 10 → src_row:
|
||||||
|
t = clamp((row-10) / cone_avoid_range, 0, 1) ← 斜坡上升
|
||||||
|
push = t × cone_avoid_gain × half_w × dir × decay
|
||||||
|
mid_line[row] = clamp(mid_line[row] + push,
|
||||||
|
left_line[row]+2, right_line[row]-2)
|
||||||
|
```
|
||||||
|
|
||||||
|
**速度联动：** `cone_is_slow()` 在锥桶确认或保持期间返回 true，
|
||||||
|
`motor_update` 将当前速度乘以 `cone_speed`（默认 0.5）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## LCD 渲染
|
||||||
|
|
||||||
|
```
|
||||||
|
lcd_render():
|
||||||
|
g_lcd_on 每 10 帧从 g_cfg.showImg 刷新
|
||||||
|
|
||||||
|
track (128/0 灰度) → resize(newWidth×newHeight) → GRAY→BGR
|
||||||
|
→ 居中拷贝到 lcd_fbImage (screenHeight × screenWidth) 的 ROI 区域
|
||||||
|
|
||||||
|
边界线绘制: 红=左边界, 绿=右边界, 蓝=中线
|
||||||
|
(跳过 mid_line[row]==-1 的行)
|
||||||
|
|
||||||
|
检测框绘制: 遍历 g_boxes, bx=newWidth/raw_frame.cols
|
||||||
|
跳过 cls=1(红灯) 和 cls=2(绿灯) 的框
|
||||||
|
cls=0 锥桶 → 橙色框, cls=3 斑马线 → 紫色框
|
||||||
|
标注框类别+置信度
|
||||||
|
|
||||||
|
状态指示 (左下角): N=正常, S=斑马线停车, C=斑马线冷却,
|
||||||
|
R=红灯停车, G=等绿灯
|
||||||
|
→ convertMatToRGB565 → /dev/fb0 mmap
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
## 配置系统
|
## 配置系统
|
||||||
|
|
||||||
所有配置通过工作目录下的纯文本文件读写。文件不存在时 readDoubleFromFile 返回 0：
|
所有配置通过工作目录下的纯文本文件读写。文件不存在时 `readDoubleFromFile` 返回 0：
|
||||||
|
|
||||||
| 文件 | 类型 | 代码默认 | ctl.sh 写入 | 含义 |
|
| 文件 | 类型 | 代码默认 | ctl.sh 写入 | 含义 |
|
||||||
|---|---|---|---|---|
|
|---|---|---|---|---|
|
||||||
@@ -271,32 +358,54 @@ COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触
|
|||||||
| `./zebrasee` | double | 60 | (不写入) | 斑马线近界阈值 (模型空间cy) |
|
| `./zebrasee` | double | 60 | (不写入) | 斑马线近界阈值 (模型空间cy) |
|
||||||
| `./destfps` | double | 30* | (不写入) | 目标帧率 (*代码兜底值) |
|
| `./destfps` | double | 30* | (不写入) | 目标帧率 (*代码兜底值) |
|
||||||
| `./saveImg` | int(0/1) | - | (不写入) | 保存帧图像 (需 debug=1) |
|
| `./saveImg` | int(0/1) | - | (不写入) | 保存帧图像 (需 debug=1) |
|
||||||
|
| `./cone_avoid_gain` | double | 0.3 | 0.3 | 锥桶中线变形推离量 (归一化) |
|
||||||
|
| `./cone_avoid_range` | int | 30 | 30 | 变形斜坡陡峭度 |
|
||||||
|
| `./cone_speed` | double | 0.5 | 0.5 | 锥桶触发时速度倍率 |
|
||||||
|
| `./cone_min_frames` | int | 3 | 3 | 锥桶连续确认帧数 |
|
||||||
|
| `./cone_margin` | int | 0 | 0 | 0=中心点, 1=框边缘检查 |
|
||||||
|
| `./cone_thresh` | double | 0.80 | 0.80 | 锥桶置信度阈值 |
|
||||||
|
| `./cone_hold_frames` | int | 30 | 30 | 锥桶消失后保持变形帧数 |
|
||||||
|
| `./brake_scale` | double | 10 | 10 | 刹车速度→占空比系数 |
|
||||||
|
| `./brake_max` | int | 10000 | 10000 | 最大刹车占空比 (ns) |
|
||||||
|
|
||||||
**debug 模式：** `./debug` = 1 时，主循环每7帧调用一次 `cfg_load_all()` 重读全部配置，支持热更新调参。同时允许 `saveImg` 保存图像。
|
**debug 模式：** `./debug` = 1 时，主循环每7帧调用一次 `cfg_load_all()` 重读全部配置，支持热更新调参。同时允许 `saveImg` 保存图像。
|
||||||
|
|
||||||
**注意**：`global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。
|
**注意：** `global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 代码中未使用的模块
|
## 代码中未使用的模块
|
||||||
|
|
||||||
以下 `.cpp` 文件已编译进 `common_lib` 但主循环中从未调用：
|
以下 `.cpp` 文件存在于 `src/` 中但已被 `CMakeLists.txt` 的 `list(FILTER ... EXCLUDE)` 排除编译：
|
||||||
|
|
||||||
| 文件 | 功能 | 状态 |
|
| 文件 | 功能 | 排除原因 |
|
||||||
|---|---|---|
|
|---|---|---|
|
||||||
| `PIDController.cpp` | 位置式/增量式 PID | 类存在但无实例化，无调用 |
|
| `vl53l0x.cpp` | 激光测距模块 | 硬件未连接 |
|
||||||
| `serial.cpp` | VOFA 串口可视化 (vofa_justfloat/vofa_image) | 已实现但无调用 |
|
| `zebra_detect.cpp` | 经典斑马线检测 | 已被 Mild 模型替代 |
|
||||||
| `Timer.cpp` | 定时器线程 | 已实现但无调用（Video 依赖它但 Video 也未用） |
|
| `PIDController.cpp` | 位置式/增量式 PID | 电机开环直驱，无调用点 |
|
||||||
| `video.cpp` | 视频文件流读取 | 已实现但无调用 |
|
| `serial.cpp` | VOFA 串口可视化 | 未使用 |
|
||||||
|
|
||||||
|
**注意：** `PIDController.h` 仍在 `lib/` 中，`MotorController` 仅提供 `updateduty()`（开环占空比），
|
||||||
|
不提供 `updateSpeed()` 方法。
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
## 当前已知缺陷 / 未利用能力
|
## 模型：Mild v12
|
||||||
|
|
||||||
1. **cls=0 锥桶** — 模型已检测但被忽略（CONE_DESIGN.md 设计了方案）
|
- **推理引擎**：`src/model_v10.{hpp,cpp}`（名称为兼容保留，内部为 Mild v3）
|
||||||
2. **cls=1 红灯 / cls=2 绿灯** — 检测框跳过不画（LCD 渲染中 `if g_boxes[i].cls == 1 || g_boxes[i].cls == 2: continue`），无决策
|
- **输入**：160×120 BGR
|
||||||
3. **电机开环** — 无编码器反馈，PID 类已实现但无调用点，`MotorController` 无 `updateSpeed()` 方法
|
- **输出**：4 类 + 背景 → 9 通道：0=锥桶, 1=红灯, 2=绿灯, 3=斑马线
|
||||||
4. **舵机死区** — 用 `abs(deviation) < deadband` 比较像素值，deadband 单位是显示空间像素
|
- **架构**：3→9→9→13→13→19→19→19→44→44→64→64 → head(64→64,dw) → out(64→9)
|
||||||
5. **中线 row 范围** — 丢线补全仅覆盖 row 10~59，row 0~9 的 mid_line 保持 -1 不可靠
|
- **阈值**：`{0.90, 0.75, 0.80, 0.90}`（锥桶/红灯/绿灯/斑马线）
|
||||||
6. **mid_val == 255 防御** — 代码中写死但 255 永远不会出现在 mid_line 中（当前实现下）
|
- **权重文件**：`mild_v12.bin`（105KB 自定义二进制格式）
|
||||||
7. **LCD 坐标缩放硬编码** — 检测框绘制用 `newWidth/160.0f`，假设 raw_frame 宽=160。回退模式下 raw_frame=640 时出错
|
- **推理频率**：每 2 帧一次（跳帧节省 CPU）
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 已知局限
|
||||||
|
|
||||||
|
1. **电机开环** — 正常行驶无编码器反馈，仅制动时使用编码器速度做比例刹车。PID 类已实现但无调用点。
|
||||||
|
2. **舵机死区** — 用 `abs(deviation) < deadband` 比较像素值，deadband 单位是显示空间像素。
|
||||||
|
3. **中线 row 范围** — 丢线补全仅覆盖 row 10~59，row 0~9 的 mid_line 保持 -1 不可靠。
|
||||||
|
4. **LCD 坐标缩放硬编码** — 检测框绘制用 `newWidth/160.0f`，假设 raw_frame 宽=160。回退模式（raw_frame=640）下会出错。
|
||||||
|
5. **锥桶变形可能跳过 row 0~9** — 变形循环从 row=10 开始，row 0~9 不会改变。
|
||||||
|
|||||||
@@ -0,0 +1,343 @@
|
|||||||
|
# 激光雷达挡板避障设计
|
||||||
|
|
||||||
|
## 问题
|
||||||
|
|
||||||
|
VL53L0X 是单点 ToF 激光测距传感器，仅返回沿光束方向的一个距离值（mm），没有扇形扫描能力。
|
||||||
|
当激光测到近处有物体时，无法直接从距离值判断它是：
|
||||||
|
|
||||||
|
- A) **挡板/障碍物** — 横在赛道上的物体，需要绕行
|
||||||
|
- B) **赛道边墙** — 弯道处车头对准了侧墙，这是正常行驶状态
|
||||||
|
|
||||||
|
**解决思路：用视觉巡线的边界线（left_line / right_line）来区分 A 和 B。**
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 关键约束：挡板会导致丢线
|
||||||
|
|
||||||
|
挡板立在赛道上时，不仅触发激光近距读数，还会**遮挡摄像头视野中的赛道边界**，
|
||||||
|
导致从挡板所在位置开始边界线丢失（left_line / right_line = -1）。
|
||||||
|
|
||||||
|
```
|
||||||
|
Camera 俯视视角 (图像坐标):
|
||||||
|
row 0 (远) : ░░░░░░░ ← 被挡板挡住，丢线
|
||||||
|
row 15 : ░░░░░░░ ← 挡板上方，丢线
|
||||||
|
row 25 : ░░░░░░░ ← 挡板顶部附近，丢线
|
||||||
|
row 30 : ███████ ← ★ 挡板所在行，边界线丢失
|
||||||
|
row 35 : ■■■■■■■ ← 挡板下方，赛道可见，边界线有效
|
||||||
|
row 59 (近) : ■■■■■■■ ← 车前方，赛道清晰
|
||||||
|
```
|
||||||
|
|
||||||
|
**这意味着：不能像锥桶检测那样"往更远处看边线是否还开着"，因为挡板后面的边线必然丢失。**
|
||||||
|
|
||||||
|
正确的判定是检测 **"有效边界 → 丢线"的过渡位置是否与激光近距读数对应**：
|
||||||
|
- 紧贴着挡板下方（更近处）：赛道应该可见，边界有效
|
||||||
|
- 挡板位置及上方（更远处）：边界丢失
|
||||||
|
- 激光读数：短距离 → 同一位置有物理障碍物
|
||||||
|
|
||||||
|
三个条件同时成立 → 挡板确认。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 几何模型
|
||||||
|
|
||||||
|
```
|
||||||
|
Camera + Laser
|
||||||
|
｜ (高度 H, 俯角 θ)
|
||||||
|
｜╲
|
||||||
|
｜ ╲ laser beam
|
||||||
|
｜ ╲
|
||||||
|
ground ──────────────┴────███████──── 挡板 at distance d_mm
|
||||||
|
(挡板后方赛道被遮挡)
|
||||||
|
```
|
||||||
|
|
||||||
|
- 激光光束沿车体正前方（图像中轴线）
|
||||||
|
- 距离 d_mm 越小 → 物体越近 → 映射到图像中越靠下的行（row 大）
|
||||||
|
- 距离 d_mm 越大 → 物体越远 → 映射到图像中越靠上的行（row 小）
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 核心算法
|
||||||
|
|
||||||
|
### 第一步：读取激光距离
|
||||||
|
|
||||||
|
```
|
||||||
|
d_mm = vl53l0x.readRange().RangeMilliMeter
|
||||||
|
|
||||||
|
if d_mm >= LIDAR_THRESHOLD_MM:
|
||||||
|
return CLEAR // 远处无障碍，不做任何处理
|
||||||
|
```
|
||||||
|
|
||||||
|
`LIDAR_THRESHOLD_MM` 是触发阈值。只有距离小于此值才进入判定。建议默认 ~300mm。
|
||||||
|
|
||||||
|
### 第二步：距离 → 图像行映射
|
||||||
|
|
||||||
|
```
|
||||||
|
// 线性模型:
|
||||||
|
// row = lt_h-1 (底行, 最近) 对应 D_NEAR
|
||||||
|
// row = 10 (最远有效行) 对应 D_FAR
|
||||||
|
// clamp 到 [10, lt_h-1]
|
||||||
|
|
||||||
|
row = lt_h - 1 - (d_mm - D_NEAR) / (D_FAR - D_NEAR) * (lt_h - 11)
|
||||||
|
row = clamp(row, 10, lt_h - 1)
|
||||||
|
```
|
||||||
|
|
||||||
|
**标定值（需根据实际安装位置测量）：**
|
||||||
|
|
||||||
|
| 参数 | 含义 | 建议初值 |
|
||||||
|
|------|------|----------|
|
||||||
|
| `D_NEAR` | row = lt_h-1 对应的物理距离 | 50 mm |
|
||||||
|
| `D_FAR` | row = 10 对应的物理距离 | 1200 mm |
|
||||||
|
|
||||||
|
标定方法：在赛道前方 300mm、600mm、900mm 处各放一个挡板，记录图像中挡板出现的 row，线性拟合。
|
||||||
|
|
||||||
|
### 第三步：用边线判定障碍物（核心）
|
||||||
|
|
||||||
|
```
|
||||||
|
// 设 row = distance_to_row(d_mm)，即激光测距对应的图像行
|
||||||
|
|
||||||
|
// ── 3a. 检查"挡板下方"（更近处）：赛道应该仍可见 ──
|
||||||
|
near_valid = false
|
||||||
|
near_ref_row = -1
|
||||||
|
for r = min(row + NEAR_START, lt_h-1) down to max(row + NEAR_END, lt_h-1):
|
||||||
|
if left_line[r] != -1 && right_line[r] != -1:
|
||||||
|
near_valid = true
|
||||||
|
near_ref_row = r // 记录有效行，后续绕行时判断宽侧
|
||||||
|
break
|
||||||
|
|
||||||
|
// ── 3b. 检查"挡板位置及上方"（更远处）：边界应该已丢失 ──
|
||||||
|
far_lost = false
|
||||||
|
for r = row down to max(row - FAR_SPAN, 10):
|
||||||
|
if left_line[r] == -1 || right_line[r] == -1:
|
||||||
|
far_lost = true
|
||||||
|
break
|
||||||
|
|
||||||
|
// ── 3c. 联合判定 ──
|
||||||
|
if near_valid && far_lost:
|
||||||
|
→ OBSTACLE_CONFIRMED
|
||||||
|
记录: g_lidar_obstacle_row = row
|
||||||
|
g_lidar_ref_row = near_ref_row // 用于判断绕行方向
|
||||||
|
else:
|
||||||
|
→ CLEAR
|
||||||
|
```
|
||||||
|
|
||||||
|
**判定原理（三种典型场景）：**
|
||||||
|
|
||||||
|
```
|
||||||
|
场景 A: 挡板挡路 → 触发绕行
|
||||||
|
d_mm = 300mm → row = 35
|
||||||
|
row 36~39 (近处): ✓ 赛道可见
|
||||||
|
row 30~35 (挡板处): ✗ 丢线 (挡板遮挡)
|
||||||
|
→ near_valid=true, far_lost=true → ★ 触发
|
||||||
|
|
||||||
|
场景 B: 正常直道，无障碍
|
||||||
|
d_mm = 1200mm → row = 10, 且 d_mm > THRESHOLD
|
||||||
|
→ 不进入判定，直接 CLEAR
|
||||||
|
|
||||||
|
场景 C: 弯道，激光打到边墙
|
||||||
|
d_mm = 200mm → row = 40
|
||||||
|
row 41~44 (近处): ✓ 赛道可见
|
||||||
|
row 37~40 (弯道处): ✓ 赛道也可能可见（边墙不一定导致丢线）
|
||||||
|
→ near_valid=true, far_lost=false → CLEAR
|
||||||
|
```
|
||||||
|
|
||||||
|
### 第四步：去抖确认
|
||||||
|
|
||||||
|
```
|
||||||
|
if OBSTACLE_CONFIRMED:
|
||||||
|
g_lidar_frames++
|
||||||
|
else:
|
||||||
|
g_lidar_frames = max(0, g_lidar_frames - 1)
|
||||||
|
|
||||||
|
g_lidar_confirmed = (g_lidar_frames >= LIDAR_MIN_FRAMES)
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 绕行策略：中线变形
|
||||||
|
|
||||||
|
挡板通常只挡赛道的一部分（偏左或偏右），通过判断挡板下方有效行中哪一侧空间更大，
|
||||||
|
将中线推向宽侧实现绕行。
|
||||||
|
|
||||||
|
### 判断绕行方向
|
||||||
|
|
||||||
|
```
|
||||||
|
// 在 near_ref_row（挡板下方最近的有效行）中判断左右空间
|
||||||
|
left_space = mid_line[near_ref_row] - left_line[near_ref_row]
|
||||||
|
right_space = right_line[near_ref_row] - mid_line[near_ref_row]
|
||||||
|
|
||||||
|
// 往宽侧推
|
||||||
|
dir = (left_space > right_space) ? +1.0 : -1.0
|
||||||
|
```
|
||||||
|
|
||||||
|
### 中线变形
|
||||||
|
|
||||||
|
对从 row=10 到挡板位置 row 的所有行施加变形，变形量从远到近线性斜坡上升：
|
||||||
|
|
||||||
|
```
|
||||||
|
// gain = lidar_avoid_gain (推离力度, 归一化)
|
||||||
|
// range = lidar_avoid_range (斜坡陡峭度, 行数)
|
||||||
|
// half_w = lt_w / 2
|
||||||
|
|
||||||
|
for r = 10 to g_lidar_obstacle_row:
|
||||||
|
t = clamp((r - 10) / range, 0.0, 1.0) // 斜率上升
|
||||||
|
push = t * gain * half_w * dir
|
||||||
|
mid_line[r] = clamp(mid_line[r] + push,
|
||||||
|
left_line[r] + 2.0,
|
||||||
|
right_line[r] - 2.0)
|
||||||
|
```
|
||||||
|
|
||||||
|
**变形示意图：**
|
||||||
|
|
||||||
|
```
|
||||||
|
row 10 ───────●──────────────────────────○ 赛道中心线
|
||||||
|
row 20 ────────●─────────────────────────○ (往右绕行)
|
||||||
|
row 30 ───────────●──────────────────────○
|
||||||
|
row 35 ──────────────●───────────────────○ ← 挡板位置
|
||||||
|
row 40 ────────────────████████████──────── ← 挡板 (丢线)
|
||||||
|
row 59 ─■────────●───■■■■■■■■■■■■■■───●───■ ← 车底 (近处可见)
|
||||||
|
left mid 方向盘往右打 right
|
||||||
|
```
|
||||||
|
|
||||||
|
### 速度联动
|
||||||
|
|
||||||
|
绕行时降速以保证安全：
|
||||||
|
|
||||||
|
```
|
||||||
|
if g_lidar_confirmed || g_lidar_hold_ctr > 0:
|
||||||
|
spd *= lidar_speed // 默认 0.4，即降至 40% 速度
|
||||||
|
```
|
||||||
|
|
||||||
|
### 消失后保持衰减
|
||||||
|
|
||||||
|
挡板不再被检测到后，变形不会立即消失，而是线性衰减（类似锥桶 hold 逻辑）：
|
||||||
|
|
||||||
|
```
|
||||||
|
// 确认状态
|
||||||
|
if g_lidar_confirmed:
|
||||||
|
g_lidar_hold_ctr = 0
|
||||||
|
g_lidar_hold_src_row = g_lidar_obstacle_row
|
||||||
|
g_lidar_hold_dir = dir
|
||||||
|
|
||||||
|
// 衰减状态（挡板消失后）
|
||||||
|
else if g_lidar_hold_src_row > 0 && g_lidar_hold_ctr < lidar_hold_frames:
|
||||||
|
g_lidar_hold_ctr++
|
||||||
|
decay = 1.0 - (double)g_lidar_hold_ctr / lidar_hold_frames // 线性衰减到 0
|
||||||
|
|
||||||
|
for r = 10 to g_lidar_hold_src_row:
|
||||||
|
t = clamp((r - 10) / range, 0.0, 1.0)
|
||||||
|
push = t * gain * half_w * g_lidar_hold_dir * decay
|
||||||
|
mid_line[r] = clamp(mid_line[r] + push, left+2, right-2)
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 状态机
|
||||||
|
|
||||||
|
```
|
||||||
|
┌────────────────────────┐
|
||||||
|
│ │
|
||||||
|
▼ │
|
||||||
|
┌──────┐ 确认 ┌──────┐ 消失 ┌──────────┐ 保持结束
|
||||||
|
│NORMAL│ ─────► │AVOID │ ─────► │HOLD_DECAY│ ────────► NORMAL
|
||||||
|
│ │ │ │ │ (衰减) │
|
||||||
|
└──────┘ └──────┘ └──────────┘
|
||||||
|
▲ │
|
||||||
|
└────────────────────────────────┘ 测距恢复正常
|
||||||
|
|
||||||
|
NORMAL: 正常行驶，激光测距 > THRESHOLD
|
||||||
|
AVOID: 挡板确认 → 中线变形绕行 + 减速 × lidar_speed
|
||||||
|
HOLD_DECAY: 挡板消失 → 变形量线性衰减 (lidar_hold_frames 帧内归零)
|
||||||
|
衰减期间若再次检测到挡板 → 立即切回 AVOID
|
||||||
|
```
|
||||||
|
|
||||||
|
| 状态 | 电机 | 舵机 | 说明 |
|
||||||
|
|------|------|------|------|
|
||||||
|
| NORMAL | 正常速度 | 正常巡线 | 无变形 |
|
||||||
|
| AVOID | speed × lidar_speed | 中线偏转绕行 | 基于 left/right 空间判断方向 |
|
||||||
|
| HOLD_DECAY | speed × lidar_speed | 变形量线性衰减 | 确保车完全通过后再恢复 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 与现有流水线的集成
|
||||||
|
|
||||||
|
`lidar_avoid_process()` 放在 cone_detect_and_deform 之前执行（两者都修改 mid_line，
|
||||||
|
lidar 优先级更高）：
|
||||||
|
|
||||||
|
```
|
||||||
|
// CameraHandler 中 Step 5:
|
||||||
|
bool zebra_block = zebra_process();
|
||||||
|
bool tl_block = traffic_light_process();
|
||||||
|
lidar_avoid_process(); // ★ 新增：直接修改 mid_line
|
||||||
|
cone_detect_and_deform();
|
||||||
|
|
||||||
|
steering_update();
|
||||||
|
motor_update(zebra_block, tl_block);
|
||||||
|
```
|
||||||
|
|
||||||
|
### 中线修改的优先级
|
||||||
|
|
||||||
|
lidar 和 cone 都会修改 `mid_line[]`。由于 lidar 绕行是结构性避障（避开整个挡板），
|
||||||
|
应**先执行 lidar 变形，再执行 cone 变形**。
|
||||||
|
cone 的 clamp 到 `[left+2, right-2]` 会确保不超出 lidar 变形后的安全区间。
|
||||||
|
|
||||||
|
### motor_update 速度联动
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
// motor_update 中新增：
|
||||||
|
if (g_lidar_confirmed || g_lidar_hold_ctr > 0) {
|
||||||
|
spd *= g_cfg.lidar_speed;
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
不通过 `block` 参数刹车（绕行不需要停车），而是降速 + 变形。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 复用现有 VL53L0X 驱动
|
||||||
|
|
||||||
|
已有驱动（当前被 `CMakeLists.txt` 排除编译）：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
// lib/vl53l0x.h
|
||||||
|
VL53L0X sensor;
|
||||||
|
sensor.init(); // open /dev/stmvl53l0x_ranging
|
||||||
|
VL53L0X_RangingMeasurementData_t data;
|
||||||
|
sensor.readRange(data); // data.RangeMilliMeter
|
||||||
|
sensor.stop();
|
||||||
|
```
|
||||||
|
|
||||||
|
**集成时需做的事：**
|
||||||
|
1. 从 `CMakeLists.txt` 的 EXCLUDE 列表中移除 `vl53l0x.cpp`
|
||||||
|
2. 在 `CameraInit()` 中调用 `sensor.init()`（硬件未连接时优雅降级）
|
||||||
|
3. 在 `cameraDeInit()` 中调用 `sensor.stop()`
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 配置参数
|
||||||
|
|
||||||
|
| 文件 | 类型 | 建议默认 | 含义 |
|
||||||
|
|------|------|----------|------|
|
||||||
|
| `./lidar_thresh` | int | 300 | 障碍判定距离阈值 (mm)，低于此值触发判定 |
|
||||||
|
| `./lidar_near` | int | 50 | row=lt_h-1 对应的物理距离 (mm) |
|
||||||
|
| `./lidar_far` | int | 1200 | row=10 对应的物理距离 (mm) |
|
||||||
|
| `./lidar_near_start` | int | 1 | near_valid 检测起始偏移 (row + N) |
|
||||||
|
| `./lidar_near_end` | int | 4 | near_valid 检测终止偏移 |
|
||||||
|
| `./lidar_far_span` | int | 6 | far_lost 检测跨度 (从 row 往上检查 N 行) |
|
||||||
|
| `./lidar_min_frames` | int | 3 | 连续确认帧数 |
|
||||||
|
| `./lidar_avoid_gain` | double | 0.4 | 绕行中线推离力度 (归一化) |
|
||||||
|
| `./lidar_avoid_range` | int | 30 | 绕行斜坡陡峭度 (行数) |
|
||||||
|
| `./lidar_speed` | double | 0.4 | 绕行时速度倍率 |
|
||||||
|
| `./lidar_hold_frames` | int | 30 | 挡板消失后变形保持帧数 |
|
||||||
|
| `./lidar_enable` | int(0/1) | 1 | 激光避障总开关（0=禁用） |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 边界情况 & 注意事项
|
||||||
|
|
||||||
|
1. **激光硬件未连接** — `sensor.init()` 失败时，`lidar_avoid_process()` 直接返回 false，不阻塞正常行驶。
|
||||||
|
2. **弯道边墙误判** — 急弯处赛道边墙可能导致激光近距 + 边线丢失同时出现。依赖 `lidar_thresh` 和 `lidar_min_frames` 调参抑制。误判时车会短暂绕向一侧，弯道通过后立即恢复。
|
||||||
|
3. **左右空间相等** — `left_space == right_space` 时默认 `dir = -1.0`（往右绕），可通过配置 `lidar_default_dir` 调整。
|
||||||
|
4. **上坡/下坡** — 车辆俯仰变化会影响距离→行的映射精度。建议在平坦路段标定。
|
||||||
|
5. **多传感器优先级** — lidar 先于 cone 修改 mid_line。zebra/tl 的 block 刹车优先级最高（红灯/斑马线停车不绕行）。
|
||||||
|
6. **首次集成建议** — 先调通数据采集：打印 `d_mm`、对应 `row`、以及 `near_ref_row` 处的 `left/right/mid` 值，跑几圈确认标定参数无误后再开启绕行。
|
||||||
|
7. **挡板材质** — 深色挡板不会被 HSV-Otsu 判为赛道（白色 255），但仍会遮挡背景赛道导致丢线。判定逻辑依赖的是**丢线**而非**像素颜色**。
|
||||||
@@ -31,6 +31,19 @@ const std::string cone_hold_frames_file = "./cone_hold_frames";
|
|||||||
const std::string brake_scale_file = "./brake_scale";
|
const std::string brake_scale_file = "./brake_scale";
|
||||||
const std::string brake_max_file = "./brake_max";
|
const std::string brake_max_file = "./brake_max";
|
||||||
|
|
||||||
|
const std::string lidar_thresh_file = "./lidar_thresh";
|
||||||
|
const std::string lidar_near_file = "./lidar_near";
|
||||||
|
const std::string lidar_far_file = "./lidar_far";
|
||||||
|
const std::string lidar_near_start_file = "./lidar_near_start";
|
||||||
|
const std::string lidar_near_end_file = "./lidar_near_end";
|
||||||
|
const std::string lidar_far_span_file = "./lidar_far_span";
|
||||||
|
const std::string lidar_min_frames_file = "./lidar_min_frames";
|
||||||
|
const std::string lidar_avoid_gain_file = "./lidar_avoid_gain";
|
||||||
|
const std::string lidar_avoid_range_file = "./lidar_avoid_range";
|
||||||
|
const std::string lidar_speed_file = "./lidar_speed";
|
||||||
|
const std::string lidar_hold_frames_file = "./lidar_hold_frames";
|
||||||
|
const std::string lidar_enable_file = "./lidar_enable";
|
||||||
|
|
||||||
double readDoubleFromFile(const std::string &filename);
|
double readDoubleFromFile(const std::string &filename);
|
||||||
bool readFlag(const std::string &filename);
|
bool readFlag(const std::string &filename);
|
||||||
void cfg_load_all();
|
void cfg_load_all();
|
||||||
@@ -57,6 +70,19 @@ struct CfgCache {
|
|||||||
|
|
||||||
double brake_scale = 10; // 速度(pps) → 刹车占空比(ns) 缩放系数
|
double brake_scale = 10; // 速度(pps) → 刹车占空比(ns) 缩放系数
|
||||||
int brake_max = 10000; // 最大刹车占空比 (ns)
|
int brake_max = 10000; // 最大刹车占空比 (ns)
|
||||||
|
|
||||||
|
int lidar_thresh = 300; // 激光障碍判定距离阈值 (mm)
|
||||||
|
int lidar_near = 50; // row=lt_h-1 对应的物理距离 (mm)
|
||||||
|
int lidar_far = 1200; // row=10 对应的物理距离 (mm)
|
||||||
|
int lidar_near_start = 1; // near_valid 检测起始偏移
|
||||||
|
int lidar_near_end = 4; // near_valid 检测终止偏移
|
||||||
|
int lidar_far_span = 6; // far_lost 检测跨度
|
||||||
|
int lidar_min_frames = 3; // 连续确认帧数
|
||||||
|
double lidar_avoid_gain = 0.4; // 绕行中线推离力度 (归一化)
|
||||||
|
int lidar_avoid_range = 30; // 绕行斜坡陡峭度 (行数)
|
||||||
|
double lidar_speed = 0.4; // 绕行时速度倍率
|
||||||
|
int lidar_hold_frames = 30; // 消失后变形保持帧数
|
||||||
|
int lidar_enable = 1; // 激光避障总开关
|
||||||
};
|
};
|
||||||
extern CfgCache g_cfg;
|
extern CfgCache g_cfg;
|
||||||
|
|
||||||
|
|||||||
+12
-8
@@ -2,21 +2,25 @@
|
|||||||
#define VL53L0X_H
|
#define VL53L0X_H
|
||||||
|
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
#include "vl53l0x_def.h"
|
|
||||||
|
extern "C" {
|
||||||
|
#include "vl53l0x_platform.h"
|
||||||
|
}
|
||||||
|
|
||||||
class VL53L0X
|
class VL53L0X
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
VL53L0X();
|
VL53L0X();
|
||||||
~VL53L0X();
|
~VL53L0X();
|
||||||
|
|
||||||
bool init();
|
bool init();
|
||||||
bool readRange(VL53L0X_RangingMeasurementData_t &data);
|
bool readRange(VL53L0X_RangingMeasurementData_t &data);
|
||||||
bool stop();
|
bool stop();
|
||||||
bool isOpen() const { return fd > 0; }
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
int fd;
|
int fd;
|
||||||
|
bool initialized;
|
||||||
|
vl53l0x_dev_t dev;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,85 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
* Copyright © 2016, STMicroelectronics International N.V.
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of STMicroelectronics nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||||
|
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||||
|
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*******************************************************************************/
|
||||||
|
|
||||||
|
#ifndef _VL53L0X_API_CALIBRATION_H_
|
||||||
|
#define _VL53L0X_API_CALIBRATION_H_
|
||||||
|
|
||||||
|
#include "vl53l0x_def.h"
|
||||||
|
#include "vl53l0x_platform.h"
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_perform_xtalk_calibration(VL53L0X_DEV Dev,
|
||||||
|
FixPoint1616_t XTalkCalDistance,
|
||||||
|
FixPoint1616_t *pXTalkCompensationRateMegaCps);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_perform_offset_calibration(VL53L0X_DEV Dev,
|
||||||
|
FixPoint1616_t CalDistanceMilliMeter,
|
||||||
|
int32_t *pOffsetMicroMeter);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
|
||||||
|
int32_t OffsetCalibrationDataMicroMeter);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
|
||||||
|
int32_t *pOffsetCalibrationDataMicroMeter);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev,
|
||||||
|
uint32_t *refSpadCount, uint8_t *isApertureSpads);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_set_reference_spads(VL53L0X_DEV Dev,
|
||||||
|
uint32_t count, uint8_t isApertureSpads);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_reference_spads(VL53L0X_DEV Dev,
|
||||||
|
uint32_t *pSpadCount, uint8_t *pIsApertureSpads);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev,
|
||||||
|
uint8_t *pPhaseCal, const uint8_t get_data_enable,
|
||||||
|
const uint8_t restore_config);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev,
|
||||||
|
uint8_t *pVhvSettings, uint8_t *pPhaseCal, uint8_t get_data_enable);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_set_ref_calibration(VL53L0X_DEV Dev,
|
||||||
|
uint8_t VhvSettings, uint8_t PhaseCal);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_ref_calibration(VL53L0X_DEV Dev,
|
||||||
|
uint8_t *pVhvSettings, uint8_t *pPhaseCal);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif /* _VL53L0X_API_CALIBRATION_H_ */
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,113 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
* Copyright © 2016, STMicroelectronics International N.V.
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of STMicroelectronics nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||||
|
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||||
|
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*******************************************************************************/
|
||||||
|
|
||||||
|
#ifndef _VL53L0X_API_CORE_H_
|
||||||
|
#define _VL53L0X_API_CORE_H_
|
||||||
|
|
||||||
|
#include "vl53l0x_def.h"
|
||||||
|
#include "vl53l0x_platform.h"
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_reverse_bytes(uint8_t *data, uint32_t size);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev);
|
||||||
|
|
||||||
|
uint8_t VL53L0X_encode_vcsel_period(uint8_t vcsel_period_pclks);
|
||||||
|
|
||||||
|
uint8_t VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg);
|
||||||
|
|
||||||
|
uint32_t VL53L0X_isqrt(uint32_t num);
|
||||||
|
|
||||||
|
uint32_t VL53L0X_quadrature_sum(uint32_t a, uint32_t b);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_info_from_device(VL53L0X_DEV Dev, uint8_t option);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV Dev,
|
||||||
|
VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV Dev,
|
||||||
|
VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK);
|
||||||
|
|
||||||
|
uint32_t VL53L0X_decode_timeout(uint16_t encoded_timeout);
|
||||||
|
|
||||||
|
VL53L0X_Error get_sequence_step_timeout(VL53L0X_DEV Dev,
|
||||||
|
VL53L0X_SequenceStepId SequenceStepId,
|
||||||
|
uint32_t *pTimeOutMicroSecs);
|
||||||
|
|
||||||
|
VL53L0X_Error set_sequence_step_timeout(VL53L0X_DEV Dev,
|
||||||
|
VL53L0X_SequenceStepId SequenceStepId,
|
||||||
|
uint32_t TimeOutMicroSecs);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_set_measurement_timing_budget_micro_seconds(
|
||||||
|
VL53L0X_DEV Dev,
|
||||||
|
uint32_t MeasurementTimingBudgetMicroSeconds);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_measurement_timing_budget_micro_seconds(
|
||||||
|
VL53L0X_DEV Dev,
|
||||||
|
uint32_t *pMeasurementTimingBudgetMicroSeconds);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_load_tuning_settings(VL53L0X_DEV Dev,
|
||||||
|
uint8_t *pTuningSettingBuffer);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev,
|
||||||
|
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
|
||||||
|
FixPoint1616_t *pSigmaEstimate);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_calc_dmax(
|
||||||
|
VL53L0X_DEV Dev, FixPoint1616_t ambRateMeas, uint32_t *pdmax_mm);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_total_xtalk_rate(VL53L0X_DEV Dev,
|
||||||
|
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
|
||||||
|
FixPoint1616_t *ptotal_xtalk_rate_mcps);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_total_signal_rate(VL53L0X_DEV Dev,
|
||||||
|
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
|
||||||
|
FixPoint1616_t *ptotal_signal_rate_mcps);
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_pal_range_status(VL53L0X_DEV Dev,
|
||||||
|
uint8_t DeviceRangeStatus,
|
||||||
|
FixPoint1616_t SignalRate,
|
||||||
|
uint16_t EffectiveSpadRtnCount,
|
||||||
|
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
|
||||||
|
uint8_t *pPalRangeStatus);
|
||||||
|
|
||||||
|
uint32_t VL53L0X_calc_timeout_mclks(VL53L0X_DEV Dev,
|
||||||
|
uint32_t timeout_period_us, uint8_t vcsel_period_pclks);
|
||||||
|
|
||||||
|
uint16_t VL53L0X_encode_timeout(uint32_t timeout_macro_clks);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif /* _VL53L0X_API_CORE_H_ */
|
||||||
@@ -0,0 +1,42 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
* Copyright © 2016, STMicroelectronics International N.V.
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of STMicroelectronics nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||||
|
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||||
|
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
******************************************************************************/
|
||||||
|
|
||||||
|
#include "vl53l0x_api.h"
|
||||||
|
#include "vl53l0x_api_core.h"
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef __KERNEL__
|
||||||
|
#include <stdlib.h>
|
||||||
|
#endif
|
||||||
|
#define LOG_FUNCTION_START(fmt, ...) \
|
||||||
|
_LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__)
|
||||||
|
#define LOG_FUNCTION_END(status, ...) \
|
||||||
|
_LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__)
|
||||||
|
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
||||||
|
_LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__)
|
||||||
|
|
||||||
@@ -0,0 +1,47 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
* Copyright © 2016, STMicroelectronics International N.V.
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of STMicroelectronics nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||||
|
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||||
|
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*******************************************************************************/
|
||||||
|
|
||||||
|
#ifndef _VL53L0X_API_RANGING_H_
|
||||||
|
#define _VL53L0X_API_RANGING_H_
|
||||||
|
|
||||||
|
#include "vl53l0x_def.h"
|
||||||
|
#include "vl53l0x_platform.h"
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif /* _VL53L0X_API_RANGING_H_ */
|
||||||
@@ -0,0 +1,21 @@
|
|||||||
|
#ifndef VL53L0X_API_STRINGS_H_
|
||||||
|
#define VL53L0X_API_STRINGS_H_
|
||||||
|
|
||||||
|
#include "vl53l0x_def.h"
|
||||||
|
|
||||||
|
#ifndef VL53L0X_API
|
||||||
|
#define VL53L0X_API
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
VL53L0X_API VL53L0X_Error VL53L0X_GetStatusErrorString(VL53L0X_Error Status, char *pErrorString);
|
||||||
|
VL53L0X_API VL53L0X_Error VL53L0X_GetStatusString(VL53L0X_Error Status, char *pStatusString);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,663 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
* Copyright © 2016, STMicroelectronics International N.V.
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of STMicroelectronics nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||||
|
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||||
|
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*******************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file VL53L0X_def.h
|
||||||
|
*
|
||||||
|
* @brief Type definitions for VL53L0X API.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef _VL53L0X_DEF_H_
|
||||||
|
#define _VL53L0X_DEF_H_
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_globaldefine_group VL53L0X Defines
|
||||||
|
* @brief VL53L0X Defines
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/** PAL SPECIFICATION major version */
|
||||||
|
#define VL53L0X10_SPECIFICATION_VER_MAJOR 1
|
||||||
|
/** PAL SPECIFICATION minor version */
|
||||||
|
#define VL53L0X10_SPECIFICATION_VER_MINOR 2
|
||||||
|
/** PAL SPECIFICATION sub version */
|
||||||
|
#define VL53L0X10_SPECIFICATION_VER_SUB 7
|
||||||
|
/** PAL SPECIFICATION sub version */
|
||||||
|
#define VL53L0X10_SPECIFICATION_VER_REVISION 1440
|
||||||
|
|
||||||
|
/** VL53L0X PAL IMPLEMENTATION major version */
|
||||||
|
#define VL53L0X10_IMPLEMENTATION_VER_MAJOR 1
|
||||||
|
/** VL53L0X PAL IMPLEMENTATION minor version */
|
||||||
|
#define VL53L0X10_IMPLEMENTATION_VER_MINOR 0
|
||||||
|
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||||
|
#define VL53L0X10_IMPLEMENTATION_VER_SUB 9
|
||||||
|
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||||
|
#define VL53L0X10_IMPLEMENTATION_VER_REVISION 3673
|
||||||
|
|
||||||
|
/** PAL SPECIFICATION major version */
|
||||||
|
#define VL53L0X_SPECIFICATION_VER_MAJOR 1
|
||||||
|
/** PAL SPECIFICATION minor version */
|
||||||
|
#define VL53L0X_SPECIFICATION_VER_MINOR 2
|
||||||
|
/** PAL SPECIFICATION sub version */
|
||||||
|
#define VL53L0X_SPECIFICATION_VER_SUB 7
|
||||||
|
/** PAL SPECIFICATION sub version */
|
||||||
|
#define VL53L0X_SPECIFICATION_VER_REVISION 1440
|
||||||
|
|
||||||
|
/** VL53L0X PAL IMPLEMENTATION major version */
|
||||||
|
#define VL53L0X_IMPLEMENTATION_VER_MAJOR 1
|
||||||
|
/** VL53L0X PAL IMPLEMENTATION minor version */
|
||||||
|
#define VL53L0X_IMPLEMENTATION_VER_MINOR 0
|
||||||
|
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||||
|
#define VL53L0X_IMPLEMENTATION_VER_SUB 4
|
||||||
|
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||||
|
#define VL53L0X_IMPLEMENTATION_VER_REVISION 4960
|
||||||
|
#define VL53L0X_DEFAULT_MAX_LOOP 2000
|
||||||
|
#define VL53L0X_MAX_STRING_LENGTH 32
|
||||||
|
|
||||||
|
|
||||||
|
#include "vl53l0x_device.h"
|
||||||
|
#include "vl53l0x_types.h"
|
||||||
|
|
||||||
|
|
||||||
|
/****************************************
|
||||||
|
* PRIVATE define do not edit
|
||||||
|
****************************************/
|
||||||
|
|
||||||
|
/** @brief Defines the parameters of the Get Version Functions
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
uint32_t revision; /*!< revision number */
|
||||||
|
uint8_t major; /*!< major number */
|
||||||
|
uint8_t minor; /*!< minor number */
|
||||||
|
uint8_t build; /*!< build number */
|
||||||
|
} VL53L0X_Version_t;
|
||||||
|
|
||||||
|
|
||||||
|
/** @brief Defines the parameters of the Get Device Info Functions
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
char Name[VL53L0X_MAX_STRING_LENGTH];
|
||||||
|
/*!< Name of the Device e.g. Left_Distance */
|
||||||
|
char Type[VL53L0X_MAX_STRING_LENGTH];
|
||||||
|
/*!< Type of the Device e.g VL53L0X */
|
||||||
|
char ProductId[VL53L0X_MAX_STRING_LENGTH];
|
||||||
|
/*!< Product Identifier String */
|
||||||
|
uint8_t ProductType;
|
||||||
|
/*!< Product Type, VL53L0X = 1, VL53L1 = 2 */
|
||||||
|
uint8_t ProductRevisionMajor;
|
||||||
|
/*!< Product revision major */
|
||||||
|
uint8_t ProductRevisionMinor;
|
||||||
|
/*!< Product revision minor */
|
||||||
|
} VL53L0X_DeviceInfo_t;
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_define_Error_group Error and Warning code returned by API
|
||||||
|
* The following DEFINE are used to identify the PAL ERROR
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
typedef int8_t VL53L0X_Error;
|
||||||
|
|
||||||
|
#define VL53L0X_ERROR_NONE ((VL53L0X_Error) 0)
|
||||||
|
#define VL53L0X_ERROR_CALIBRATION_WARNING ((VL53L0X_Error) - 1)
|
||||||
|
/*!< Warning invalid calibration data may be in used
|
||||||
|
* \a VL53L0X_InitData()
|
||||||
|
* \a VL53L0X_GetOffsetCalibrationData
|
||||||
|
* \a VL53L0X_SetOffsetCalibrationData
|
||||||
|
*/
|
||||||
|
#define VL53L0X_ERROR_MIN_CLIPPED ((VL53L0X_Error) - 2)
|
||||||
|
/*!< Warning parameter passed was clipped to min before to be applied */
|
||||||
|
|
||||||
|
#define VL53L0X_ERROR_UNDEFINED ((VL53L0X_Error) - 3)
|
||||||
|
/*!< Unqualified error */
|
||||||
|
#define VL53L0X_ERROR_INVALID_PARAMS ((VL53L0X_Error) - 4)
|
||||||
|
/*!< Parameter passed is invalid or out of range */
|
||||||
|
#define VL53L0X_ERROR_NOT_SUPPORTED ((VL53L0X_Error) - 5)
|
||||||
|
/*!< Function is not supported in current mode or configuration */
|
||||||
|
#define VL53L0X_ERROR_RANGE_ERROR ((VL53L0X_Error) - 6)
|
||||||
|
/*!< Device report a ranging error interrupt status */
|
||||||
|
#define VL53L0X_ERROR_TIME_OUT ((VL53L0X_Error) - 7)
|
||||||
|
/*!< Aborted due to time out */
|
||||||
|
#define VL53L0X_ERROR_MODE_NOT_SUPPORTED ((VL53L0X_Error) - 8)
|
||||||
|
/*!< Asked mode is not supported by the device */
|
||||||
|
#define VL53L0X_ERROR_BUFFER_TOO_SMALL ((VL53L0X_Error) - 9)
|
||||||
|
/*!< ... */
|
||||||
|
#define VL53L0X_ERROR_GPIO_NOT_EXISTING ((VL53L0X_Error) - 10)
|
||||||
|
/*!< User tried to setup a non-existing GPIO pin */
|
||||||
|
#define VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED ((VL53L0X_Error) - 11)
|
||||||
|
/*!< unsupported GPIO functionality */
|
||||||
|
#define VL53L0X_ERROR_INTERRUPT_NOT_CLEARED ((VL53L0X_Error) - 12)
|
||||||
|
/*!< Error during interrupt clear */
|
||||||
|
#define VL53L0X_ERROR_CONTROL_INTERFACE ((VL53L0X_Error) - 20)
|
||||||
|
/*!< error reported from IO functions */
|
||||||
|
#define VL53L0X_ERROR_INVALID_COMMAND ((VL53L0X_Error) - 30)
|
||||||
|
/*!< The command is not allowed in the current device state
|
||||||
|
* (power down)
|
||||||
|
*/
|
||||||
|
#define VL53L0X_ERROR_DIVISION_BY_ZERO ((VL53L0X_Error) - 40)
|
||||||
|
/*!< In the function a division by zero occurs */
|
||||||
|
#define VL53L0X_ERROR_REF_SPAD_INIT ((VL53L0X_Error) - 50)
|
||||||
|
/*!< Error during reference SPAD initialization */
|
||||||
|
#define VL53L0X_ERROR_NOT_IMPLEMENTED ((VL53L0X_Error) - 99)
|
||||||
|
/*!< Tells requested functionality has not been implemented yet or
|
||||||
|
* not compatible with the device
|
||||||
|
*/
|
||||||
|
/** @} VL53L0X_define_Error_group */
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_define_DeviceModes_group Defines Device modes
|
||||||
|
* Defines all possible modes for the device
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
typedef uint8_t VL53L0X_DeviceModes;
|
||||||
|
|
||||||
|
#define VL53L0X_DEVICEMODE_SINGLE_RANGING ((VL53L0X_DeviceModes) 0)
|
||||||
|
#define VL53L0X_DEVICEMODE_CONTINUOUS_RANGING ((VL53L0X_DeviceModes) 1)
|
||||||
|
#define VL53L0X_DEVICEMODE_SINGLE_HISTOGRAM ((VL53L0X_DeviceModes) 2)
|
||||||
|
#define VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING ((VL53L0X_DeviceModes) 3)
|
||||||
|
#define VL53L0X_DEVICEMODE_SINGLE_ALS ((VL53L0X_DeviceModes) 10)
|
||||||
|
#define VL53L0X_DEVICEMODE_GPIO_DRIVE ((VL53L0X_DeviceModes) 20)
|
||||||
|
#define VL53L0X_DEVICEMODE_GPIO_OSC ((VL53L0X_DeviceModes) 21)
|
||||||
|
/* ... Modes to be added depending on device */
|
||||||
|
/** @} VL53L0X_define_DeviceModes_group */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_define_HistogramModes_group Defines Histogram modes
|
||||||
|
* Defines all possible Histogram modes for the device
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
typedef uint8_t VL53L0X_HistogramModes;
|
||||||
|
|
||||||
|
#define VL53L0X_HISTOGRAMMODE_DISABLED ((VL53L0X_HistogramModes) 0)
|
||||||
|
/*!< Histogram Disabled */
|
||||||
|
#define VL53L0X_HISTOGRAMMODE_REFERENCE_ONLY ((VL53L0X_HistogramModes) 1)
|
||||||
|
/*!< Histogram Reference array only */
|
||||||
|
#define VL53L0X_HISTOGRAMMODE_RETURN_ONLY ((VL53L0X_HistogramModes) 2)
|
||||||
|
/*!< Histogram Return array only */
|
||||||
|
#define VL53L0X_HISTOGRAMMODE_BOTH ((VL53L0X_HistogramModes) 3)
|
||||||
|
/*!< Histogram both Reference and Return Arrays */
|
||||||
|
/* ... Modes to be added depending on device */
|
||||||
|
/** @} VL53L0X_define_HistogramModes_group */
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_define_PowerModes_group List of available Power Modes
|
||||||
|
* List of available Power Modes
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
typedef uint8_t VL53L0X_PowerModes;
|
||||||
|
|
||||||
|
#define VL53L0X_POWERMODE_STANDBY_LEVEL1 ((VL53L0X_PowerModes) 0)
|
||||||
|
/*!< Standby level 1 */
|
||||||
|
#define VL53L0X_POWERMODE_STANDBY_LEVEL2 ((VL53L0X_PowerModes) 1)
|
||||||
|
/*!< Standby level 2 */
|
||||||
|
#define VL53L0X_POWERMODE_IDLE_LEVEL1 ((VL53L0X_PowerModes) 2)
|
||||||
|
/*!< Idle level 1 */
|
||||||
|
#define VL53L0X_POWERMODE_IDLE_LEVEL2 ((VL53L0X_PowerModes) 3)
|
||||||
|
/*!< Idle level 2 */
|
||||||
|
|
||||||
|
/** @} VL53L0X_define_PowerModes_group */
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_DMAX_LUT_SIZE 7
|
||||||
|
/*!< Defines the number of items in the DMAX lookup table */
|
||||||
|
|
||||||
|
/** @brief Structure defining data pair that makes up the DMAX Lookup table.
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
FixPoint1616_t ambRate_mcps[VL53L0X_DMAX_LUT_SIZE];
|
||||||
|
/*!< Ambient rate (mcps) */
|
||||||
|
FixPoint1616_t dmax_mm[VL53L0X_DMAX_LUT_SIZE];
|
||||||
|
/*!< DMAX Value (mm) */
|
||||||
|
} VL53L0X_DMaxLUT_t;
|
||||||
|
|
||||||
|
/** @brief Defines all parameters for the device
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
VL53L0X_DeviceModes DeviceMode;
|
||||||
|
/*!< Defines type of measurement to be done for the next measure */
|
||||||
|
VL53L0X_HistogramModes HistogramMode;
|
||||||
|
/*!< Defines type of histogram measurement to be done for the next
|
||||||
|
* measure
|
||||||
|
*/
|
||||||
|
uint32_t MeasurementTimingBudgetMicroSeconds;
|
||||||
|
/*!< Defines the allowed total time for a single measurement */
|
||||||
|
uint32_t InterMeasurementPeriodMilliSeconds;
|
||||||
|
/*!< Defines time between two consecutive measurements (between two
|
||||||
|
* measurement starts). If set to 0 means back-to-back mode
|
||||||
|
*/
|
||||||
|
uint8_t XTalkCompensationEnable;
|
||||||
|
/*!< Tells if Crosstalk compensation shall be enable or not */
|
||||||
|
uint16_t XTalkCompensationRangeMilliMeter;
|
||||||
|
/*!< CrossTalk compensation range in millimeter */
|
||||||
|
FixPoint1616_t XTalkCompensationRateMegaCps;
|
||||||
|
/*!< CrossTalk compensation rate in Mega counts per seconds.
|
||||||
|
* Expressed in 16.16 fixed point format.
|
||||||
|
*/
|
||||||
|
int32_t RangeOffsetMicroMeters;
|
||||||
|
/*!< Range offset adjustment (mm). */
|
||||||
|
|
||||||
|
uint8_t LimitChecksEnable[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||||
|
/*!< This Array store all the Limit Check enable for this device. */
|
||||||
|
uint8_t LimitChecksStatus[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||||
|
/*!< This Array store all the Status of the check linked to last
|
||||||
|
* measurement.
|
||||||
|
*/
|
||||||
|
FixPoint1616_t LimitChecksValue[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||||
|
/*!< This Array store all the Limit Check value for this device */
|
||||||
|
|
||||||
|
VL53L0X_DMaxLUT_t dmax_lut;
|
||||||
|
/*!< Lookup table defining ambient rates and associated
|
||||||
|
* dmax values.
|
||||||
|
*/
|
||||||
|
|
||||||
|
uint8_t WrapAroundCheckEnable;
|
||||||
|
/*!< Tells if Wrap Around Check shall be enable or not */
|
||||||
|
} VL53L0X_DeviceParameters_t;
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_define_State_group Defines the current status
|
||||||
|
* of the device
|
||||||
|
* Defines the current status of the device
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
typedef uint8_t VL53L0X_State;
|
||||||
|
|
||||||
|
#define VL53L0X_STATE_POWERDOWN ((VL53L0X_State) 0)
|
||||||
|
/*!< Device is in HW reset */
|
||||||
|
#define VL53L0X_STATE_WAIT_STATICINIT ((VL53L0X_State) 1)
|
||||||
|
/*!< Device is initialized and wait for static initialization */
|
||||||
|
#define VL53L0X_STATE_STANDBY ((VL53L0X_State) 2)
|
||||||
|
/*!< Device is in Low power Standby mode */
|
||||||
|
#define VL53L0X_STATE_IDLE ((VL53L0X_State) 3)
|
||||||
|
/*!< Device has been initialized and ready to do measurements */
|
||||||
|
#define VL53L0X_STATE_RUNNING ((VL53L0X_State) 4)
|
||||||
|
/*!< Device is performing measurement */
|
||||||
|
#define VL53L0X_STATE_UNKNOWN ((VL53L0X_State) 98)
|
||||||
|
/*!< Device is in unknown state and need to be rebooted */
|
||||||
|
#define VL53L0X_STATE_ERROR ((VL53L0X_State) 99)
|
||||||
|
/*!< Device is in error state and need to be rebooted */
|
||||||
|
|
||||||
|
/** @} VL53L0X_define_State_group */
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @struct VL53L0X_RangeData_t
|
||||||
|
* @brief Range measurement data.
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
uint32_t TimeStamp; /*!< 32-bit time stamp. */
|
||||||
|
uint32_t MeasurementTimeUsec;
|
||||||
|
/*!< Give the Measurement time needed by the device to do the
|
||||||
|
* measurement.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
uint16_t RangeMilliMeter; /*!< range distance in millimeter. */
|
||||||
|
|
||||||
|
uint16_t RangeDMaxMilliMeter;
|
||||||
|
/*!< Tells what is the maximum detection distance of the device
|
||||||
|
* in current setup and environment conditions (Filled when
|
||||||
|
* applicable)
|
||||||
|
*/
|
||||||
|
|
||||||
|
FixPoint1616_t SignalRateRtnMegaCps;
|
||||||
|
/*!< Return signal rate (MCPS)\n these is a 16.16 fix point
|
||||||
|
* value, which is effectively a measure of target
|
||||||
|
* reflectance.
|
||||||
|
*/
|
||||||
|
FixPoint1616_t AmbientRateRtnMegaCps;
|
||||||
|
/*!< Return ambient rate (MCPS)\n these is a 16.16 fix point
|
||||||
|
* value, which is effectively a measure of the ambien
|
||||||
|
* t light.
|
||||||
|
*/
|
||||||
|
|
||||||
|
uint16_t EffectiveSpadRtnCount;
|
||||||
|
/*!< Return the effective SPAD count for the return signal.
|
||||||
|
* To obtain Real value it should be divided by 256
|
||||||
|
*/
|
||||||
|
|
||||||
|
uint8_t ZoneId;
|
||||||
|
/*!< Denotes which zone and range scheduler stage the range
|
||||||
|
* data relates to.
|
||||||
|
*/
|
||||||
|
uint8_t RangeFractionalPart;
|
||||||
|
/*!< Fractional part of range distance. Final value is a
|
||||||
|
* FixPoint168 value.
|
||||||
|
*/
|
||||||
|
uint8_t RangeStatus;
|
||||||
|
/*!< Range Status for the current measurement. This is device
|
||||||
|
* dependent. Value = 0 means value is valid.
|
||||||
|
* See \ref RangeStatusPage
|
||||||
|
*/
|
||||||
|
} VL53L0X_RangingMeasurementData_t;
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_HISTOGRAM_BUFFER_SIZE 24
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @struct VL53L0X_HistogramData_t
|
||||||
|
* @brief Histogram measurement data.
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
/* Histogram Measurement data */
|
||||||
|
uint32_t HistogramData[VL53L0X_HISTOGRAM_BUFFER_SIZE];
|
||||||
|
/*!< Histogram data */
|
||||||
|
/*!< Indicate the types of histogram data :
|
||||||
|
*Return only, Reference only, both Return and Reference
|
||||||
|
*/
|
||||||
|
uint8_t FirstBin; /*!< First Bin value */
|
||||||
|
uint8_t BufferSize; /*!< Buffer Size - Set by the user.*/
|
||||||
|
uint8_t NumberOfBins;
|
||||||
|
/*!< Number of bins filled by the histogram measurement */
|
||||||
|
|
||||||
|
VL53L0X_DeviceError ErrorStatus;
|
||||||
|
/*!< Error status of the current measurement. \n
|
||||||
|
* see @a ::VL53L0X_DeviceError @a VL53L0X_GetStatusErrorString()
|
||||||
|
*/
|
||||||
|
} VL53L0X_HistogramMeasurementData_t;
|
||||||
|
|
||||||
|
#define VL53L0X_REF_SPAD_BUFFER_SIZE 6
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @struct VL53L0X_SpadData_t
|
||||||
|
* @brief Spad Configuration Data.
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
uint8_t RefSpadEnables[VL53L0X_REF_SPAD_BUFFER_SIZE];
|
||||||
|
/*!< Reference Spad Enables */
|
||||||
|
uint8_t RefGoodSpadMap[VL53L0X_REF_SPAD_BUFFER_SIZE];
|
||||||
|
/*!< Reference Spad Good Spad Map */
|
||||||
|
} VL53L0X_SpadData_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
FixPoint1616_t OscFrequencyMHz; /* Frequency used */
|
||||||
|
|
||||||
|
uint16_t LastEncodedTimeout;
|
||||||
|
/* last encoded Time out used for timing budget*/
|
||||||
|
|
||||||
|
VL53L0X_GpioFunctionality Pin0GpioFunctionality;
|
||||||
|
/* store the functionality of the GPIO: pin0 */
|
||||||
|
|
||||||
|
uint32_t FinalRangeTimeoutMicroSecs;
|
||||||
|
/*!< Execution time of the final range*/
|
||||||
|
uint8_t FinalRangeVcselPulsePeriod;
|
||||||
|
/*!< Vcsel pulse period (pll clocks) for the final range measurement*/
|
||||||
|
uint32_t PreRangeTimeoutMicroSecs;
|
||||||
|
/*!< Execution time of the final range*/
|
||||||
|
uint8_t PreRangeVcselPulsePeriod;
|
||||||
|
/*!< Vcsel pulse period (pll clocks) for the pre-range measurement*/
|
||||||
|
|
||||||
|
uint16_t SigmaEstRefArray;
|
||||||
|
/*!< Reference array sigma value in 1/100th of [mm] e.g. 100 = 1mm */
|
||||||
|
uint16_t SigmaEstEffPulseWidth;
|
||||||
|
/*!< Effective Pulse width for sigma estimate in 1/100th
|
||||||
|
* of ns e.g. 900 = 9.0ns
|
||||||
|
*/
|
||||||
|
uint16_t SigmaEstEffAmbWidth;
|
||||||
|
/*!< Effective Ambient width for sigma estimate in 1/100th of ns
|
||||||
|
* e.g. 500 = 5.0ns
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/* Indicate if read from device has been done (==1) or not (==0) */
|
||||||
|
uint8_t ReadDataFromDeviceDone;
|
||||||
|
uint8_t ModuleId; /* Module ID */
|
||||||
|
uint8_t Revision; /* test Revision */
|
||||||
|
char ProductId[VL53L0X_MAX_STRING_LENGTH];
|
||||||
|
/* Product Identifier String */
|
||||||
|
uint8_t ReferenceSpadCount; /* used for ref spad management */
|
||||||
|
uint8_t ReferenceSpadType; /* used for ref spad management */
|
||||||
|
uint8_t RefSpadsInitialised; /* reports if ref spads are initialised. */
|
||||||
|
uint32_t PartUIDUpper; /*!< Unique Part ID Upper */
|
||||||
|
uint32_t PartUIDLower; /*!< Unique Part ID Lower */
|
||||||
|
/*!< Peek Signal rate at 400 mm*/
|
||||||
|
FixPoint1616_t SignalRateMeasFixed400mm;
|
||||||
|
|
||||||
|
} VL53L0X_DeviceSpecificParameters_t;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @struct VL53L0X_DevData_t
|
||||||
|
*
|
||||||
|
* @brief VL53L0X PAL device ST private data structure \n
|
||||||
|
* End user should never access any of these field directly
|
||||||
|
*
|
||||||
|
* These must never access directly but only via macro
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
int32_t Part2PartOffsetNVMMicroMeter;
|
||||||
|
/*!< backed up NVM value */
|
||||||
|
int32_t Part2PartOffsetAdjustmentNVMMicroMeter;
|
||||||
|
/*!< backed up NVM value representing additional offset adjustment */
|
||||||
|
VL53L0X_DeviceParameters_t CurrentParameters;
|
||||||
|
/*!< Current Device Parameter */
|
||||||
|
VL53L0X_RangingMeasurementData_t LastRangeMeasure;
|
||||||
|
/*!< Ranging Data */
|
||||||
|
VL53L0X_HistogramMeasurementData_t LastHistogramMeasure;
|
||||||
|
/*!< Histogram Data */
|
||||||
|
VL53L0X_DeviceSpecificParameters_t DeviceSpecificParameters;
|
||||||
|
/*!< Parameters specific to the device */
|
||||||
|
VL53L0X_SpadData_t SpadData;
|
||||||
|
/*!< Spad Data */
|
||||||
|
uint8_t SequenceConfig;
|
||||||
|
/*!< Internal value for the sequence config */
|
||||||
|
uint8_t RangeFractionalEnable;
|
||||||
|
/*!< Enable/Disable fractional part of ranging data */
|
||||||
|
VL53L0X_State PalState;
|
||||||
|
/*!< Current state of the PAL for this device */
|
||||||
|
VL53L0X_PowerModes PowerMode;
|
||||||
|
/*!< Current Power Mode */
|
||||||
|
uint16_t SigmaEstRefArray;
|
||||||
|
/*!< Reference array sigma value in 1/100th of [mm] e.g. 100 = 1mm */
|
||||||
|
uint16_t SigmaEstEffPulseWidth;
|
||||||
|
/*!< Effective Pulse width for sigma estimate in 1/100th
|
||||||
|
* of ns e.g. 900 = 9.0ns
|
||||||
|
*/
|
||||||
|
uint16_t SigmaEstEffAmbWidth;
|
||||||
|
/*!< Effective Ambient width for sigma estimate in 1/100th of ns
|
||||||
|
* e.g. 500 = 5.0ns
|
||||||
|
*/
|
||||||
|
uint8_t StopVariable;
|
||||||
|
/*!< StopVariable used during the stop sequence */
|
||||||
|
uint16_t targetRefRate;
|
||||||
|
/*!< Target Ambient Rate for Ref spad management */
|
||||||
|
FixPoint1616_t SigmaEstimate;
|
||||||
|
/*!< Sigma Estimate - based on ambient & VCSEL rates and
|
||||||
|
* signal_total_events
|
||||||
|
*/
|
||||||
|
FixPoint1616_t SignalEstimate;
|
||||||
|
/*!< Signal Estimate - based on ambient & VCSEL rates and cross talk */
|
||||||
|
FixPoint1616_t LastSignalRefMcps;
|
||||||
|
/*!< Latest Signal ref in Mcps */
|
||||||
|
uint8_t *pTuningSettingsPointer;
|
||||||
|
/*!< Pointer for Tuning Settings table */
|
||||||
|
uint8_t UseInternalTuningSettings;
|
||||||
|
/*!< Indicate if we use Tuning Settings table */
|
||||||
|
uint16_t LinearityCorrectiveGain;
|
||||||
|
/*!< Linearity Corrective Gain value in x1000 */
|
||||||
|
} VL53L0X_DevData_t;
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_define_InterruptPolarity_group Defines the Polarity
|
||||||
|
* of the Interrupt
|
||||||
|
* Defines the Polarity of the Interrupt
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
typedef uint8_t VL53L0X_InterruptPolarity;
|
||||||
|
|
||||||
|
#define VL53L0X_INTERRUPTPOLARITY_LOW ((VL53L0X_InterruptPolarity) 0)
|
||||||
|
/*!< Set active low polarity best setup for falling edge. */
|
||||||
|
#define VL53L0X_INTERRUPTPOLARITY_HIGH ((VL53L0X_InterruptPolarity) 1)
|
||||||
|
/*!< Set active high polarity best setup for rising edge. */
|
||||||
|
|
||||||
|
/** @} VL53L0X_define_InterruptPolarity_group */
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_define_VcselPeriod_group Vcsel Period Defines
|
||||||
|
* Defines the range measurement for which to access the vcsel period.
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
typedef uint8_t VL53L0X_VcselPeriod;
|
||||||
|
|
||||||
|
#define VL53L0X_VCSEL_PERIOD_PRE_RANGE ((VL53L0X_VcselPeriod) 0)
|
||||||
|
/*!<Identifies the pre-range vcsel period. */
|
||||||
|
#define VL53L0X_VCSEL_PERIOD_FINAL_RANGE ((VL53L0X_VcselPeriod) 1)
|
||||||
|
/*!<Identifies the final range vcsel period. */
|
||||||
|
|
||||||
|
/** @} VL53L0X_define_VcselPeriod_group */
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_define_SchedulerSequence_group Defines the steps
|
||||||
|
* carried out by the scheduler during a range measurement.
|
||||||
|
* @{
|
||||||
|
* Defines the states of all the steps in the scheduler
|
||||||
|
* i.e. enabled/disabled.
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
uint8_t TccOn; /*!<Reports if Target Centre Check On */
|
||||||
|
uint8_t MsrcOn; /*!<Reports if MSRC On */
|
||||||
|
uint8_t DssOn; /*!<Reports if DSS On */
|
||||||
|
uint8_t PreRangeOn; /*!<Reports if Pre-Range On */
|
||||||
|
uint8_t FinalRangeOn; /*!<Reports if Final-Range On */
|
||||||
|
} VL53L0X_SchedulerSequenceSteps_t;
|
||||||
|
|
||||||
|
/** @} VL53L0X_define_SchedulerSequence_group */
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_define_SequenceStepId_group Defines the Polarity
|
||||||
|
* of the Interrupt
|
||||||
|
* Defines the the sequence steps performed during ranging..
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
typedef uint8_t VL53L0X_SequenceStepId;
|
||||||
|
|
||||||
|
#define VL53L0X_SEQUENCESTEP_TCC ((VL53L0X_VcselPeriod) 0)
|
||||||
|
/*!<Target CentreCheck identifier. */
|
||||||
|
#define VL53L0X_SEQUENCESTEP_DSS ((VL53L0X_VcselPeriod) 1)
|
||||||
|
/*!<Dynamic Spad Selection function Identifier. */
|
||||||
|
#define VL53L0X_SEQUENCESTEP_MSRC ((VL53L0X_VcselPeriod) 2)
|
||||||
|
/*!<Minimum Signal Rate Check function Identifier. */
|
||||||
|
#define VL53L0X_SEQUENCESTEP_PRE_RANGE ((VL53L0X_VcselPeriod) 3)
|
||||||
|
/*!<Pre-Range check Identifier. */
|
||||||
|
#define VL53L0X_SEQUENCESTEP_FINAL_RANGE ((VL53L0X_VcselPeriod) 4)
|
||||||
|
/*!<Final Range Check Identifier. */
|
||||||
|
|
||||||
|
#define VL53L0X_SEQUENCESTEP_NUMBER_OF_CHECKS 5
|
||||||
|
/*!<Number of Sequence Step Managed by the API. */
|
||||||
|
|
||||||
|
/** @} VL53L0X_define_SequenceStepId_group */
|
||||||
|
|
||||||
|
|
||||||
|
/* MACRO Definitions */
|
||||||
|
/** @defgroup VL53L0X_define_GeneralMacro_group General Macro Defines
|
||||||
|
* General Macro Defines
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* Defines */
|
||||||
|
#define VL53L0X_SETPARAMETERFIELD(Dev, field, value) \
|
||||||
|
PALDevDataSet(Dev, CurrentParameters.field, value)
|
||||||
|
|
||||||
|
#define VL53L0X_GETPARAMETERFIELD(Dev, field, variable) \
|
||||||
|
(variable = ((PALDevDataGet(Dev, CurrentParameters)).field))
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_SETARRAYPARAMETERFIELD(Dev, field, index, value) \
|
||||||
|
PALDevDataSet(Dev, CurrentParameters.field[index], value)
|
||||||
|
|
||||||
|
#define VL53L0X_GETARRAYPARAMETERFIELD(Dev, field, index, variable) \
|
||||||
|
(variable = (PALDevDataGet(Dev, CurrentParameters)).field[index])
|
||||||
|
|
||||||
|
#define VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, field, value) \
|
||||||
|
PALDevDataSet(Dev, DeviceSpecificParameters.field, value)
|
||||||
|
|
||||||
|
#define VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, field) \
|
||||||
|
PALDevDataGet(Dev, DeviceSpecificParameters).field
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_FIXPOINT1616TOFIXPOINT97(Value) \
|
||||||
|
(uint16_t)((Value>>9)&0xFFFF)
|
||||||
|
#define VL53L0X_FIXPOINT97TOFIXPOINT1616(Value) \
|
||||||
|
(FixPoint1616_t)(Value<<9)
|
||||||
|
|
||||||
|
#define VL53L0X_FIXPOINT1616TOFIXPOINT88(Value) \
|
||||||
|
(uint16_t)((Value>>8)&0xFFFF)
|
||||||
|
#define VL53L0X_FIXPOINT88TOFIXPOINT1616(Value) \
|
||||||
|
(FixPoint1616_t)(Value<<8)
|
||||||
|
|
||||||
|
#define VL53L0X_FIXPOINT1616TOFIXPOINT412(Value) \
|
||||||
|
(uint16_t)((Value>>4)&0xFFFF)
|
||||||
|
#define VL53L0X_FIXPOINT412TOFIXPOINT1616(Value) \
|
||||||
|
(FixPoint1616_t)(Value<<4)
|
||||||
|
|
||||||
|
#define VL53L0X_FIXPOINT1616TOFIXPOINT313(Value) \
|
||||||
|
(uint16_t)((Value>>3)&0xFFFF)
|
||||||
|
#define VL53L0X_FIXPOINT313TOFIXPOINT1616(Value) \
|
||||||
|
(FixPoint1616_t)(Value<<3)
|
||||||
|
|
||||||
|
#define VL53L0X_FIXPOINT1616TOFIXPOINT08(Value) \
|
||||||
|
(uint8_t)((Value>>8)&0x00FF)
|
||||||
|
#define VL53L0X_FIXPOINT08TOFIXPOINT1616(Value) \
|
||||||
|
(FixPoint1616_t)(Value<<8)
|
||||||
|
|
||||||
|
#define VL53L0X_FIXPOINT1616TOFIXPOINT53(Value) \
|
||||||
|
(uint8_t)((Value>>13)&0x00FF)
|
||||||
|
#define VL53L0X_FIXPOINT53TOFIXPOINT1616(Value) \
|
||||||
|
(FixPoint1616_t)(Value<<13)
|
||||||
|
|
||||||
|
#define VL53L0X_FIXPOINT1616TOFIXPOINT102(Value) \
|
||||||
|
(uint16_t)((Value>>14)&0x0FFF)
|
||||||
|
#define VL53L0X_FIXPOINT102TOFIXPOINT1616(Value) \
|
||||||
|
(FixPoint1616_t)(Value<<12)
|
||||||
|
|
||||||
|
#define VL53L0X_MAKEUINT16(lsb, msb) (uint16_t)((((uint16_t)msb)<<8) + \
|
||||||
|
(uint16_t)lsb)
|
||||||
|
|
||||||
|
/** @} VL53L0X_define_GeneralMacro_group */
|
||||||
|
|
||||||
|
/** @} VL53L0X_globaldefine_group */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
#endif /* _VL53L0X_DEF_H_ */
|
||||||
@@ -0,0 +1,262 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
* Copyright © 2016, STMicroelectronics International N.V.
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of STMicroelectronics nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||||
|
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||||
|
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*******************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Device specific defines. To be adapted by implementer for the targeted
|
||||||
|
* device.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef _VL53L0X_DEVICE_H_
|
||||||
|
#define _VL53L0X_DEVICE_H_
|
||||||
|
|
||||||
|
#include "vl53l0x_types.h"
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_DevSpecDefines_group VL53L0X cut1.1 Device
|
||||||
|
* Specific Defines
|
||||||
|
* @brief VL53L0X cut1.1 Device Specific Defines
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_DeviceError_group Device Error
|
||||||
|
* @brief Device Error code
|
||||||
|
*
|
||||||
|
* This enum is Device specific it should be updated in the implementation
|
||||||
|
* Use @a VL53L0X_GetStatusErrorString() to get the string.
|
||||||
|
* It is related to Status Register of the Device.
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
typedef uint8_t VL53L0X_DeviceError;
|
||||||
|
|
||||||
|
#define VL53L0X_DEVICEERROR_NONE ((VL53L0X_DeviceError) 0)
|
||||||
|
/*!< 0 NoError */
|
||||||
|
#define VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ((VL53L0X_DeviceError) 1)
|
||||||
|
#define VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ((VL53L0X_DeviceError) 2)
|
||||||
|
#define VL53L0X_DEVICEERROR_NOVHVVALUEFOUND ((VL53L0X_DeviceError) 3)
|
||||||
|
#define VL53L0X_DEVICEERROR_MSRCNOTARGET ((VL53L0X_DeviceError) 4)
|
||||||
|
#define VL53L0X_DEVICEERROR_SNRCHECK ((VL53L0X_DeviceError) 5)
|
||||||
|
#define VL53L0X_DEVICEERROR_RANGEPHASECHECK ((VL53L0X_DeviceError) 6)
|
||||||
|
#define VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK ((VL53L0X_DeviceError) 7)
|
||||||
|
#define VL53L0X_DEVICEERROR_TCC ((VL53L0X_DeviceError) 8)
|
||||||
|
#define VL53L0X_DEVICEERROR_PHASECONSISTENCY ((VL53L0X_DeviceError) 9)
|
||||||
|
#define VL53L0X_DEVICEERROR_MINCLIP ((VL53L0X_DeviceError) 10)
|
||||||
|
#define VL53L0X_DEVICEERROR_RANGECOMPLETE ((VL53L0X_DeviceError) 11)
|
||||||
|
#define VL53L0X_DEVICEERROR_ALGOUNDERFLOW ((VL53L0X_DeviceError) 12)
|
||||||
|
#define VL53L0X_DEVICEERROR_ALGOOVERFLOW ((VL53L0X_DeviceError) 13)
|
||||||
|
#define VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD ((VL53L0X_DeviceError) 14)
|
||||||
|
|
||||||
|
/** @} end of VL53L0X_DeviceError_group */
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_CheckEnable_group Check Enable list
|
||||||
|
* @brief Check Enable code
|
||||||
|
*
|
||||||
|
* Define used to specify the LimitCheckId.
|
||||||
|
* Use @a VL53L0X_GetLimitCheckInfo() to get the string.
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE 0
|
||||||
|
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE 1
|
||||||
|
#define VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP 2
|
||||||
|
#define VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD 3
|
||||||
|
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC 4
|
||||||
|
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE 5
|
||||||
|
|
||||||
|
#define VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS 6
|
||||||
|
|
||||||
|
/** @} end of VL53L0X_CheckEnable_group */
|
||||||
|
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_GpioFunctionality_group Gpio Functionality
|
||||||
|
* @brief Defines the different functionalities for the device GPIO(s)
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
typedef uint8_t VL53L0X_GpioFunctionality;
|
||||||
|
|
||||||
|
#define VL53L0X_GPIOFUNCTIONALITY_OFF \
|
||||||
|
((VL53L0X_GpioFunctionality) 0) /*!< NO Interrupt */
|
||||||
|
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW \
|
||||||
|
((VL53L0X_GpioFunctionality) 1) /*!< Level Low (value < thresh_low) */
|
||||||
|
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH \
|
||||||
|
((VL53L0X_GpioFunctionality) 2) /*!< Level High (value>thresh_high) */
|
||||||
|
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT \
|
||||||
|
((VL53L0X_GpioFunctionality) 3)
|
||||||
|
/*!< Out Of Window (value < thresh_low OR value > thresh_high) */
|
||||||
|
#define VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY \
|
||||||
|
((VL53L0X_GpioFunctionality) 4) /*!< New Sample Ready */
|
||||||
|
|
||||||
|
/** @} end of VL53L0X_GpioFunctionality_group */
|
||||||
|
|
||||||
|
|
||||||
|
/* Device register map */
|
||||||
|
|
||||||
|
/** @defgroup VL53L0X_DefineRegisters_group Define Registers
|
||||||
|
* @brief List of all the defined registers
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
#define VL53L0X_REG_SYSRANGE_START 0x000
|
||||||
|
/** mask existing bit in #VL53L0X_REG_SYSRANGE_START*/
|
||||||
|
#define VL53L0X_REG_SYSRANGE_MODE_MASK 0x0F
|
||||||
|
/** bit 0 in #VL53L0X_REG_SYSRANGE_START write 1 toggle state in
|
||||||
|
* continuous mode and arm next shot in single shot mode
|
||||||
|
*/
|
||||||
|
#define VL53L0X_REG_SYSRANGE_MODE_START_STOP 0x01
|
||||||
|
/** bit 1 write 0 in #VL53L0X_REG_SYSRANGE_START set single shot mode */
|
||||||
|
#define VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT 0x00
|
||||||
|
/** bit 1 write 1 in #VL53L0X_REG_SYSRANGE_START set back-to-back
|
||||||
|
* operation mode
|
||||||
|
*/
|
||||||
|
#define VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK 0x02
|
||||||
|
/** bit 2 write 1 in #VL53L0X_REG_SYSRANGE_START set timed operation
|
||||||
|
* mode
|
||||||
|
*/
|
||||||
|
#define VL53L0X_REG_SYSRANGE_MODE_TIMED 0x04
|
||||||
|
/** bit 3 write 1 in #VL53L0X_REG_SYSRANGE_START set histogram operation
|
||||||
|
* mode
|
||||||
|
*/
|
||||||
|
#define VL53L0X_REG_SYSRANGE_MODE_HISTOGRAM 0x08
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_REG_SYSTEM_THRESH_HIGH 0x000C
|
||||||
|
#define VL53L0X_REG_SYSTEM_THRESH_LOW 0x000E
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG 0x0001
|
||||||
|
#define VL53L0X_REG_SYSTEM_RANGE_CONFIG 0x0009
|
||||||
|
#define VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD 0x0004
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO 0x000A
|
||||||
|
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_DISABLED 0x00
|
||||||
|
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_LOW 0x01
|
||||||
|
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_HIGH 0x02
|
||||||
|
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_OUT_OF_WINDOW 0x03
|
||||||
|
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY 0x04
|
||||||
|
|
||||||
|
#define VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH 0x0084
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR 0x000B
|
||||||
|
|
||||||
|
/* Result registers */
|
||||||
|
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS 0x0013
|
||||||
|
#define VL53L0X_REG_RESULT_RANGE_STATUS 0x0014
|
||||||
|
|
||||||
|
#define VL53L0X_REG_RESULT_CORE_PAGE 1
|
||||||
|
#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN 0x00BC
|
||||||
|
#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_RTN 0x00C0
|
||||||
|
#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF 0x00D0
|
||||||
|
#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_REF 0x00D4
|
||||||
|
#define VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF 0x00B6
|
||||||
|
|
||||||
|
/* Algo register */
|
||||||
|
|
||||||
|
#define VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM 0x0028
|
||||||
|
|
||||||
|
#define VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS 0x008a
|
||||||
|
|
||||||
|
/* Check Limit registers */
|
||||||
|
#define VL53L0X_REG_MSRC_CONFIG_CONTROL 0x0060
|
||||||
|
|
||||||
|
#define VL53L0X_REG_PRE_RANGE_CONFIG_MIN_SNR 0X0027
|
||||||
|
#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW 0x0056
|
||||||
|
#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH 0x0057
|
||||||
|
#define VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT 0x0064
|
||||||
|
|
||||||
|
#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_SNR 0X0067
|
||||||
|
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW 0x0047
|
||||||
|
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH 0x0048
|
||||||
|
#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT 0x0044
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_HI 0X0061
|
||||||
|
#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_LO 0X0062
|
||||||
|
|
||||||
|
/* PRE RANGE registers */
|
||||||
|
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x0050
|
||||||
|
#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0051
|
||||||
|
#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0052
|
||||||
|
|
||||||
|
#define VL53L0X_REG_SYSTEM_HISTOGRAM_BIN 0x0081
|
||||||
|
#define VL53L0X_REG_HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT 0x0033
|
||||||
|
#define VL53L0X_REG_HISTOGRAM_CONFIG_READOUT_CTRL 0x0055
|
||||||
|
|
||||||
|
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x0070
|
||||||
|
#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0071
|
||||||
|
#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0072
|
||||||
|
#define VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS 0x0020
|
||||||
|
|
||||||
|
#define VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP 0x0046
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N 0x00bf
|
||||||
|
#define VL53L0X_REG_IDENTIFICATION_MODEL_ID 0x00c0
|
||||||
|
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID 0x00c2
|
||||||
|
|
||||||
|
#define VL53L0X_REG_OSC_CALIBRATE_VAL 0x00f8
|
||||||
|
|
||||||
|
|
||||||
|
#define VL53L0X_SIGMA_ESTIMATE_MAX_VALUE 65535
|
||||||
|
/* equivalent to a range sigma of 655.35mm */
|
||||||
|
|
||||||
|
#define VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH 0x032
|
||||||
|
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 0x0B0
|
||||||
|
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_1 0x0B1
|
||||||
|
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_2 0x0B2
|
||||||
|
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_3 0x0B3
|
||||||
|
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_4 0x0B4
|
||||||
|
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_5 0x0B5
|
||||||
|
|
||||||
|
#define VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT 0xB6
|
||||||
|
#define VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD 0x4E /* 0x14E */
|
||||||
|
#define VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET 0x4F /* 0x14F */
|
||||||
|
#define VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE 0x80
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Speed of light in um per 1E-10 Seconds
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define VL53L0X_SPEED_OF_LIGHT_IN_AIR 2997
|
||||||
|
|
||||||
|
#define VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV 0x0089
|
||||||
|
|
||||||
|
#define VL53L0X_REG_ALGO_PHASECAL_LIM 0x0030 /* 0x130 */
|
||||||
|
#define VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT 0x0030
|
||||||
|
|
||||||
|
/** @} VL53L0X_DefineRegisters_group */
|
||||||
|
|
||||||
|
/** @} VL53L0X_DevSpecDefines_group */
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* _VL53L0X_DEVICE_H_ */
|
||||||
|
|
||||||
|
|
||||||
@@ -0,0 +1,402 @@
|
|||||||
|
/*
|
||||||
|
* vl53l0x_i2c_platform.h - Linux kernel modules for STM VL53L0 FlightSense TOF
|
||||||
|
* sensor
|
||||||
|
*
|
||||||
|
* Copyright (C) 2016 STMicroelectronics Imaging Division.
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file VL53L0X_i2c_platform.h
|
||||||
|
* @brief Function prototype definitions for EWOK Platform layer.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef _VL53L0X_I2C_PLATFORM_H_
|
||||||
|
#define _VL53L0X_I2C_PLATFORM_H_
|
||||||
|
|
||||||
|
#include "vl53l0x_def.h"
|
||||||
|
|
||||||
|
|
||||||
|
/** Maximum buffer size to be used in i2c */
|
||||||
|
#define VL53L0X_MAX_I2C_XFER_SIZE 64
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Typedef defining .\n
|
||||||
|
* The developer should modify this to suit the platform being deployed.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Typedef defining 8 bit unsigned char type.\n
|
||||||
|
* The developer should modify this to suit the platform being deployed.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef bool_t
|
||||||
|
typedef unsigned char bool_t;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
#define I2C 0x01
|
||||||
|
#define SPI 0x00
|
||||||
|
|
||||||
|
#define COMMS_BUFFER_SIZE 64
|
||||||
|
/*MUST be the same size as the SV task buffer */
|
||||||
|
|
||||||
|
#define BYTES_PER_WORD 2
|
||||||
|
#define BYTES_PER_DWORD 4
|
||||||
|
|
||||||
|
#define VL53L0X_MAX_STRING_LENGTH_PLT 256
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Initialise platform comms.
|
||||||
|
*
|
||||||
|
* @param comms_type - selects between I2C and SPI
|
||||||
|
* @param comms_speed_khz - unsigned short containing the I2C speed in kHz
|
||||||
|
*
|
||||||
|
* @return status - status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_comms_initialise(uint8_t comms_type,
|
||||||
|
uint16_t comms_speed_khz);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Close platform comms.
|
||||||
|
*
|
||||||
|
* @return status - status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_comms_close(void);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Cycle Power to Device
|
||||||
|
*
|
||||||
|
* @return status - status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_cycle_power(void);
|
||||||
|
|
||||||
|
int32_t VL53L0X_set_page(VL53L0X_DEV dev, uint8_t page_data);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Writes the supplied byte buffer to the device
|
||||||
|
*
|
||||||
|
* Wrapper for SystemVerilog Write Multi task
|
||||||
|
*
|
||||||
|
* @code
|
||||||
|
*
|
||||||
|
* Example:
|
||||||
|
*
|
||||||
|
* uint8_t *spad_enables;
|
||||||
|
*
|
||||||
|
* int status = VL53L0X_write_multi(RET_SPAD_EN_0, spad_enables, 36);
|
||||||
|
*
|
||||||
|
* @endcode
|
||||||
|
*
|
||||||
|
* @param address - uint8_t device address value
|
||||||
|
* @param index - uint8_t register index value
|
||||||
|
* @param pdata - pointer to uint8_t buffer containing the data to be written
|
||||||
|
* @param count - number of bytes in the supplied byte buffer
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_write_multi(VL53L0X_DEV dev, uint8_t index, uint8_t *pdata,
|
||||||
|
int32_t count);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Reads the requested number of bytes from the device
|
||||||
|
*
|
||||||
|
* Wrapper for SystemVerilog Read Multi task
|
||||||
|
*
|
||||||
|
* @code
|
||||||
|
*
|
||||||
|
* Example:
|
||||||
|
*
|
||||||
|
* uint8_t buffer[COMMS_BUFFER_SIZE];
|
||||||
|
*
|
||||||
|
* int status = status = VL53L0X_read_multi(DEVICE_ID, buffer, 2)
|
||||||
|
*
|
||||||
|
* @endcode
|
||||||
|
*
|
||||||
|
* @param address - uint8_t device address value
|
||||||
|
* @param index - uint8_t register index value
|
||||||
|
* @param pdata - pointer to the uint8_t buffer to store read data
|
||||||
|
* @param count - number of uint8_t's to read
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_read_multi(VL53L0X_DEV dev, uint8_t index, uint8_t *pdata,
|
||||||
|
int32_t count);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Writes a single byte to the device
|
||||||
|
*
|
||||||
|
* Wrapper for SystemVerilog Write Byte task
|
||||||
|
*
|
||||||
|
* @code
|
||||||
|
*
|
||||||
|
* Example:
|
||||||
|
*
|
||||||
|
* uint8_t page_number = MAIN_SELECT_PAGE;
|
||||||
|
*
|
||||||
|
* int status = VL53L0X_write_byte(PAGE_SELECT, page_number);
|
||||||
|
*
|
||||||
|
* @endcode
|
||||||
|
*
|
||||||
|
* @param address - uint8_t device address value
|
||||||
|
* @param index - uint8_t register index value
|
||||||
|
* @param data - uint8_t data value to write
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_write_byte(VL53L0X_DEV dev, uint8_t index, uint8_t data);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Writes a single word (16-bit unsigned) to the device
|
||||||
|
*
|
||||||
|
* Manages the big-endian nature of the device (first byte written is the
|
||||||
|
* MS byte).
|
||||||
|
* Uses SystemVerilog Write Multi task.
|
||||||
|
*
|
||||||
|
* @code
|
||||||
|
*
|
||||||
|
* Example:
|
||||||
|
*
|
||||||
|
* uint16_t nvm_ctrl_pulse_width = 0x0004;
|
||||||
|
*
|
||||||
|
* int status = VL53L0X_write_word(NVM_CTRL__PULSE_WIDTH_MSB,
|
||||||
|
* nvm_ctrl_pulse_width);
|
||||||
|
*
|
||||||
|
* @endcode
|
||||||
|
*
|
||||||
|
* @param address - uint8_t device address value
|
||||||
|
* @param index - uint8_t register index value
|
||||||
|
* @param data - uin16_t data value write
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_write_word(VL53L0X_DEV dev, uint8_t index, uint16_t data);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Writes a single dword (32-bit unsigned) to the device
|
||||||
|
*
|
||||||
|
* Manages the big-endian nature of the device (first byte written is the
|
||||||
|
* MS byte).
|
||||||
|
* Uses SystemVerilog Write Multi task.
|
||||||
|
*
|
||||||
|
* @code
|
||||||
|
*
|
||||||
|
* Example:
|
||||||
|
*
|
||||||
|
* uint32_t nvm_data = 0x0004;
|
||||||
|
*
|
||||||
|
* int status = VL53L0X_write_dword(NVM_CTRL__DATAIN_MMM, nvm_data);
|
||||||
|
*
|
||||||
|
* @endcode
|
||||||
|
*
|
||||||
|
* @param address - uint8_t device address value
|
||||||
|
* @param index - uint8_t register index value
|
||||||
|
* @param data - uint32_t data value to write
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_write_dword(VL53L0X_DEV dev, uint8_t index, uint32_t data);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Reads a single byte from the device
|
||||||
|
*
|
||||||
|
* Uses SystemVerilog Read Byte task.
|
||||||
|
*
|
||||||
|
* @code
|
||||||
|
*
|
||||||
|
* Example:
|
||||||
|
*
|
||||||
|
* uint8_t device_status = 0;
|
||||||
|
*
|
||||||
|
* int status = VL53L0X_read_byte(STATUS, &device_status);
|
||||||
|
*
|
||||||
|
* @endcode
|
||||||
|
*
|
||||||
|
* @param address - uint8_t device address value
|
||||||
|
* @param index - uint8_t register index value
|
||||||
|
* @param pdata - pointer to uint8_t data value
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_read_byte(VL53L0X_DEV dev, uint8_t index, uint8_t *pdata);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Reads a single word (16-bit unsigned) from the device
|
||||||
|
*
|
||||||
|
* Manages the big-endian nature of the device (first byte read is the MS byte).
|
||||||
|
* Uses SystemVerilog Read Multi task.
|
||||||
|
*
|
||||||
|
* @code
|
||||||
|
*
|
||||||
|
* Example:
|
||||||
|
*
|
||||||
|
* uint16_t timeout = 0;
|
||||||
|
*
|
||||||
|
* int status = VL53L0X_read_word(TIMEOUT_OVERALL_PERIODS_MSB, &timeout);
|
||||||
|
*
|
||||||
|
* @endcode
|
||||||
|
*
|
||||||
|
* @param address - uint8_t device address value
|
||||||
|
* @param index - uint8_t register index value
|
||||||
|
* @param pdata - pointer to uint16_t data value
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_read_word(VL53L0X_DEV dev, uint8_t index, uint16_t *pdata);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Reads a single dword (32-bit unsigned) from the device
|
||||||
|
*
|
||||||
|
* Manages the big-endian nature of the device (first byte read is the MS byte).
|
||||||
|
* Uses SystemVerilog Read Multi task.
|
||||||
|
*
|
||||||
|
* @code
|
||||||
|
*
|
||||||
|
* Example:
|
||||||
|
*
|
||||||
|
* uint32_t range_1 = 0;
|
||||||
|
*
|
||||||
|
* int status = VL53L0X_read_dword(RANGE_1_MMM, &range_1);
|
||||||
|
*
|
||||||
|
* @endcode
|
||||||
|
*
|
||||||
|
* @param address - uint8_t device address value
|
||||||
|
* @param index - uint8_t register index value
|
||||||
|
* @param pdata - pointer to uint32_t data value
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_read_dword(VL53L0X_DEV dev, uint8_t index, uint32_t *pdata);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Implements a programmable wait in us
|
||||||
|
*
|
||||||
|
* Wrapper for SystemVerilog Wait in micro seconds task
|
||||||
|
*
|
||||||
|
* @param wait_us - integer wait in micro seconds
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_platform_wait_us(int32_t wait_us);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Implements a programmable wait in ms
|
||||||
|
*
|
||||||
|
* Wrapper for SystemVerilog Wait in milli seconds task
|
||||||
|
*
|
||||||
|
* @param wait_ms - integer wait in milli seconds
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_wait_ms(int32_t wait_ms);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Set GPIO value
|
||||||
|
*
|
||||||
|
* @param level - input level - either 0 or 1
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_set_gpio(uint8_t level);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get GPIO value
|
||||||
|
*
|
||||||
|
* @param plevel - uint8_t pointer to store GPIO level (0 or 1)
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_get_gpio(uint8_t *plevel);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Release force on GPIO
|
||||||
|
*
|
||||||
|
* @return status - SystemVerilog status 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_release_gpio(void);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get the frequency of the timer used for ranging results time stamps
|
||||||
|
*
|
||||||
|
* @param[out] ptimer_freq_hz : pointer for timer frequency
|
||||||
|
*
|
||||||
|
* @return status : 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_get_timer_frequency(int32_t *ptimer_freq_hz);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get the timer value in units of timer_freq_hz
|
||||||
|
* (see VL53L0X_get_timestamp_frequency())
|
||||||
|
*
|
||||||
|
* @param[out] ptimer_count : pointer for timer count value
|
||||||
|
*
|
||||||
|
* @return status : 0 = ok, 1 = error
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
int32_t VL53L0X_get_timer_value(int32_t *ptimer_count);
|
||||||
|
int VL53L0X_I2CWrite(VL53L0X_DEV dev, uint8_t *buff, uint8_t len);
|
||||||
|
int VL53L0X_I2CRead(VL53L0X_DEV dev, uint8_t *buff, uint8_t len);
|
||||||
|
|
||||||
|
#endif /* _VL53L0X_I2C_PLATFORM_H_ */
|
||||||
|
|
||||||
@@ -0,0 +1,194 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
* Copyright © 2016, STMicroelectronics International N.V.
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of STMicroelectronics nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||||
|
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||||
|
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*******************************************************************************/
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_
|
||||||
|
#define _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
uint8_t InterruptThresholdSettings[] = {
|
||||||
|
|
||||||
|
/* Start of Interrupt Threshold Settings */
|
||||||
|
0x1, 0xff, 0x00,
|
||||||
|
0x1, 0x80, 0x01,
|
||||||
|
0x1, 0xff, 0x01,
|
||||||
|
0x1, 0x00, 0x00,
|
||||||
|
0x1, 0xff, 0x01,
|
||||||
|
0x1, 0x4f, 0x02,
|
||||||
|
0x1, 0xFF, 0x0E,
|
||||||
|
0x1, 0x00, 0x03,
|
||||||
|
0x1, 0x01, 0x84,
|
||||||
|
0x1, 0x02, 0x0A,
|
||||||
|
0x1, 0x03, 0x03,
|
||||||
|
0x1, 0x04, 0x08,
|
||||||
|
0x1, 0x05, 0xC8,
|
||||||
|
0x1, 0x06, 0x03,
|
||||||
|
0x1, 0x07, 0x8D,
|
||||||
|
0x1, 0x08, 0x08,
|
||||||
|
0x1, 0x09, 0xC6,
|
||||||
|
0x1, 0x0A, 0x01,
|
||||||
|
0x1, 0x0B, 0x02,
|
||||||
|
0x1, 0x0C, 0x00,
|
||||||
|
0x1, 0x0D, 0xD5,
|
||||||
|
0x1, 0x0E, 0x18,
|
||||||
|
0x1, 0x0F, 0x12,
|
||||||
|
0x1, 0x10, 0x01,
|
||||||
|
0x1, 0x11, 0x82,
|
||||||
|
0x1, 0x12, 0x00,
|
||||||
|
0x1, 0x13, 0xD5,
|
||||||
|
0x1, 0x14, 0x18,
|
||||||
|
0x1, 0x15, 0x13,
|
||||||
|
0x1, 0x16, 0x03,
|
||||||
|
0x1, 0x17, 0x86,
|
||||||
|
0x1, 0x18, 0x0A,
|
||||||
|
0x1, 0x19, 0x09,
|
||||||
|
0x1, 0x1A, 0x08,
|
||||||
|
0x1, 0x1B, 0xC2,
|
||||||
|
0x1, 0x1C, 0x03,
|
||||||
|
0x1, 0x1D, 0x8F,
|
||||||
|
0x1, 0x1E, 0x0A,
|
||||||
|
0x1, 0x1F, 0x06,
|
||||||
|
0x1, 0x20, 0x01,
|
||||||
|
0x1, 0x21, 0x02,
|
||||||
|
0x1, 0x22, 0x00,
|
||||||
|
0x1, 0x23, 0xD5,
|
||||||
|
0x1, 0x24, 0x18,
|
||||||
|
0x1, 0x25, 0x22,
|
||||||
|
0x1, 0x26, 0x01,
|
||||||
|
0x1, 0x27, 0x82,
|
||||||
|
0x1, 0x28, 0x00,
|
||||||
|
0x1, 0x29, 0xD5,
|
||||||
|
0x1, 0x2A, 0x18,
|
||||||
|
0x1, 0x2B, 0x0B,
|
||||||
|
0x1, 0x2C, 0x28,
|
||||||
|
0x1, 0x2D, 0x78,
|
||||||
|
0x1, 0x2E, 0x28,
|
||||||
|
0x1, 0x2F, 0x91,
|
||||||
|
0x1, 0x30, 0x00,
|
||||||
|
0x1, 0x31, 0x0B,
|
||||||
|
0x1, 0x32, 0x00,
|
||||||
|
0x1, 0x33, 0x0B,
|
||||||
|
0x1, 0x34, 0x00,
|
||||||
|
0x1, 0x35, 0xA1,
|
||||||
|
0x1, 0x36, 0x00,
|
||||||
|
0x1, 0x37, 0xA0,
|
||||||
|
0x1, 0x38, 0x00,
|
||||||
|
0x1, 0x39, 0x04,
|
||||||
|
0x1, 0x3A, 0x28,
|
||||||
|
0x1, 0x3B, 0x30,
|
||||||
|
0x1, 0x3C, 0x0C,
|
||||||
|
0x1, 0x3D, 0x04,
|
||||||
|
0x1, 0x3E, 0x0F,
|
||||||
|
0x1, 0x3F, 0x79,
|
||||||
|
0x1, 0x40, 0x28,
|
||||||
|
0x1, 0x41, 0x1E,
|
||||||
|
0x1, 0x42, 0x2F,
|
||||||
|
0x1, 0x43, 0x87,
|
||||||
|
0x1, 0x44, 0x00,
|
||||||
|
0x1, 0x45, 0x0B,
|
||||||
|
0x1, 0x46, 0x00,
|
||||||
|
0x1, 0x47, 0x0B,
|
||||||
|
0x1, 0x48, 0x00,
|
||||||
|
0x1, 0x49, 0xA7,
|
||||||
|
0x1, 0x4A, 0x00,
|
||||||
|
0x1, 0x4B, 0xA6,
|
||||||
|
0x1, 0x4C, 0x00,
|
||||||
|
0x1, 0x4D, 0x04,
|
||||||
|
0x1, 0x4E, 0x01,
|
||||||
|
0x1, 0x4F, 0x00,
|
||||||
|
0x1, 0x50, 0x00,
|
||||||
|
0x1, 0x51, 0x80,
|
||||||
|
0x1, 0x52, 0x09,
|
||||||
|
0x1, 0x53, 0x08,
|
||||||
|
0x1, 0x54, 0x01,
|
||||||
|
0x1, 0x55, 0x00,
|
||||||
|
0x1, 0x56, 0x0F,
|
||||||
|
0x1, 0x57, 0x79,
|
||||||
|
0x1, 0x58, 0x09,
|
||||||
|
0x1, 0x59, 0x05,
|
||||||
|
0x1, 0x5A, 0x00,
|
||||||
|
0x1, 0x5B, 0x60,
|
||||||
|
0x1, 0x5C, 0x05,
|
||||||
|
0x1, 0x5D, 0xD1,
|
||||||
|
0x1, 0x5E, 0x0C,
|
||||||
|
0x1, 0x5F, 0x3C,
|
||||||
|
0x1, 0x60, 0x00,
|
||||||
|
0x1, 0x61, 0xD0,
|
||||||
|
0x1, 0x62, 0x0B,
|
||||||
|
0x1, 0x63, 0x03,
|
||||||
|
0x1, 0x64, 0x28,
|
||||||
|
0x1, 0x65, 0x10,
|
||||||
|
0x1, 0x66, 0x2A,
|
||||||
|
0x1, 0x67, 0x39,
|
||||||
|
0x1, 0x68, 0x0B,
|
||||||
|
0x1, 0x69, 0x02,
|
||||||
|
0x1, 0x6A, 0x28,
|
||||||
|
0x1, 0x6B, 0x10,
|
||||||
|
0x1, 0x6C, 0x2A,
|
||||||
|
0x1, 0x6D, 0x61,
|
||||||
|
0x1, 0x6E, 0x0C,
|
||||||
|
0x1, 0x6F, 0x00,
|
||||||
|
0x1, 0x70, 0x0F,
|
||||||
|
0x1, 0x71, 0x79,
|
||||||
|
0x1, 0x72, 0x00,
|
||||||
|
0x1, 0x73, 0x0B,
|
||||||
|
0x1, 0x74, 0x00,
|
||||||
|
0x1, 0x75, 0x0B,
|
||||||
|
0x1, 0x76, 0x00,
|
||||||
|
0x1, 0x77, 0xA1,
|
||||||
|
0x1, 0x78, 0x00,
|
||||||
|
0x1, 0x79, 0xA0,
|
||||||
|
0x1, 0x7A, 0x00,
|
||||||
|
0x1, 0x7B, 0x04,
|
||||||
|
0x1, 0xFF, 0x04,
|
||||||
|
0x1, 0x79, 0x1D,
|
||||||
|
0x1, 0x7B, 0x27,
|
||||||
|
0x1, 0x96, 0x0E,
|
||||||
|
0x1, 0x97, 0xFE,
|
||||||
|
0x1, 0x98, 0x03,
|
||||||
|
0x1, 0x99, 0xEF,
|
||||||
|
0x1, 0x9A, 0x02,
|
||||||
|
0x1, 0x9B, 0x44,
|
||||||
|
0x1, 0x73, 0x07,
|
||||||
|
0x1, 0x70, 0x01,
|
||||||
|
0x1, 0xff, 0x01,
|
||||||
|
0x1, 0x00, 0x01,
|
||||||
|
0x1, 0xff, 0x00,
|
||||||
|
0x00, 0x00, 0x00
|
||||||
|
};
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif /* _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_ */
|
||||||
@@ -0,0 +1,41 @@
|
|||||||
|
#ifndef VL53L0X_PLATFORM_H_
|
||||||
|
#define VL53L0X_PLATFORM_H_
|
||||||
|
|
||||||
|
#include "vl53l0x_def.h"
|
||||||
|
#include "vl53l0x_platform_log.h"
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define VL53L0X_MAX_I2C_XFER_SIZE 64
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
VL53L0X_DevData_t Data;
|
||||||
|
uint8_t I2cDevAddr;
|
||||||
|
int i2c_fd;
|
||||||
|
} vl53l0x_dev_t;
|
||||||
|
|
||||||
|
typedef vl53l0x_dev_t *VL53L0X_DEV;
|
||||||
|
|
||||||
|
#define PALDevDataGet(Dev, field) (Dev->Data.field)
|
||||||
|
#define PALDevDataSet(Dev, field, data) ((Dev->Data.field) = (data))
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_LockSequenceAccess(VL53L0X_DEV Dev);
|
||||||
|
VL53L0X_Error VL53L0X_UnlockSequenceAccess(VL53L0X_DEV Dev);
|
||||||
|
VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count);
|
||||||
|
VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count);
|
||||||
|
VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data);
|
||||||
|
VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data);
|
||||||
|
VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data);
|
||||||
|
VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data);
|
||||||
|
VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data);
|
||||||
|
VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data);
|
||||||
|
VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData);
|
||||||
|
VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,14 @@
|
|||||||
|
#ifndef VL53L0X_PLATFORM_LOG_H_
|
||||||
|
#define VL53L0X_PLATFORM_LOG_H_
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#define VL53L0X_COPYSTRING(str, ...) strncpy(str, ##__VA_ARGS__, sizeof(str))
|
||||||
|
|
||||||
|
#define _LOG_FUNCTION_START(module, fmt, ...) (void)0
|
||||||
|
#define _LOG_FUNCTION_END(module, status, ...) (void)0
|
||||||
|
#define _LOG_FUNCTION_END_FMT(module, status, fmt, ...) (void)0
|
||||||
|
|
||||||
|
#define VL53L0X_ErrLog(...) (void)0
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,146 @@
|
|||||||
|
/*******************************************************************************
|
||||||
|
* Copyright © 2016, STMicroelectronics International N.V.
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of STMicroelectronics nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||||
|
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||||
|
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*******************************************************************************/
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef _VL53L0X_TUNING_H_
|
||||||
|
#define _VL53L0X_TUNING_H_
|
||||||
|
|
||||||
|
#include "vl53l0x_def.h"
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
uint8_t DefaultTuningSettings[] = {
|
||||||
|
|
||||||
|
/* update 02/11/2015_v36 */
|
||||||
|
0x01, 0xFF, 0x01,
|
||||||
|
0x01, 0x00, 0x00,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x00,
|
||||||
|
0x01, 0x09, 0x00,
|
||||||
|
0x01, 0x10, 0x00,
|
||||||
|
0x01, 0x11, 0x00,
|
||||||
|
|
||||||
|
0x01, 0x24, 0x01,
|
||||||
|
0x01, 0x25, 0xff,
|
||||||
|
0x01, 0x75, 0x00,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x01,
|
||||||
|
0x01, 0x4e, 0x2c,
|
||||||
|
0x01, 0x48, 0x00,
|
||||||
|
0x01, 0x30, 0x20,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x00,
|
||||||
|
0x01, 0x30, 0x09, /* mja changed from 0x64. */
|
||||||
|
0x01, 0x54, 0x00,
|
||||||
|
0x01, 0x31, 0x04,
|
||||||
|
0x01, 0x32, 0x03,
|
||||||
|
0x01, 0x40, 0x83,
|
||||||
|
0x01, 0x46, 0x25,
|
||||||
|
0x01, 0x60, 0x00,
|
||||||
|
0x01, 0x27, 0x00,
|
||||||
|
0x01, 0x50, 0x06,
|
||||||
|
0x01, 0x51, 0x00,
|
||||||
|
0x01, 0x52, 0x96,
|
||||||
|
0x01, 0x56, 0x08,
|
||||||
|
0x01, 0x57, 0x30,
|
||||||
|
0x01, 0x61, 0x00,
|
||||||
|
0x01, 0x62, 0x00,
|
||||||
|
0x01, 0x64, 0x00,
|
||||||
|
0x01, 0x65, 0x00,
|
||||||
|
0x01, 0x66, 0xa0,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x01,
|
||||||
|
0x01, 0x22, 0x32,
|
||||||
|
0x01, 0x47, 0x14,
|
||||||
|
0x01, 0x49, 0xff,
|
||||||
|
0x01, 0x4a, 0x00,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x00,
|
||||||
|
0x01, 0x7a, 0x0a,
|
||||||
|
0x01, 0x7b, 0x00,
|
||||||
|
0x01, 0x78, 0x21,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x01,
|
||||||
|
0x01, 0x23, 0x34,
|
||||||
|
0x01, 0x42, 0x00,
|
||||||
|
0x01, 0x44, 0xff,
|
||||||
|
0x01, 0x45, 0x26,
|
||||||
|
0x01, 0x46, 0x05,
|
||||||
|
0x01, 0x40, 0x40,
|
||||||
|
0x01, 0x0E, 0x06,
|
||||||
|
0x01, 0x20, 0x1a,
|
||||||
|
0x01, 0x43, 0x40,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x00,
|
||||||
|
0x01, 0x34, 0x03,
|
||||||
|
0x01, 0x35, 0x44,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x01,
|
||||||
|
0x01, 0x31, 0x04,
|
||||||
|
0x01, 0x4b, 0x09,
|
||||||
|
0x01, 0x4c, 0x05,
|
||||||
|
0x01, 0x4d, 0x04,
|
||||||
|
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x00,
|
||||||
|
0x01, 0x44, 0x00,
|
||||||
|
0x01, 0x45, 0x20,
|
||||||
|
0x01, 0x47, 0x08,
|
||||||
|
0x01, 0x48, 0x28,
|
||||||
|
0x01, 0x67, 0x00,
|
||||||
|
0x01, 0x70, 0x04,
|
||||||
|
0x01, 0x71, 0x01,
|
||||||
|
0x01, 0x72, 0xfe,
|
||||||
|
0x01, 0x76, 0x00,
|
||||||
|
0x01, 0x77, 0x00,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x01,
|
||||||
|
0x01, 0x0d, 0x01,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x00,
|
||||||
|
0x01, 0x80, 0x01,
|
||||||
|
0x01, 0x01, 0xF8,
|
||||||
|
|
||||||
|
0x01, 0xFF, 0x01,
|
||||||
|
0x01, 0x8e, 0x01,
|
||||||
|
0x01, 0x00, 0x01,
|
||||||
|
0x01, 0xFF, 0x00,
|
||||||
|
0x01, 0x80, 0x00,
|
||||||
|
|
||||||
|
0x00, 0x00, 0x00
|
||||||
|
};
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif /* _VL53L0X_TUNING_H_ */
|
||||||
@@ -0,0 +1,13 @@
|
|||||||
|
#ifndef VL53L0X_TYPES_H_
|
||||||
|
#define VL53L0X_TYPES_H_
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stddef.h>
|
||||||
|
|
||||||
|
#ifndef NULL
|
||||||
|
#define NULL 0
|
||||||
|
#endif
|
||||||
|
|
||||||
|
typedef unsigned int FixPoint1616_t;
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,23 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
#include "vl53l0x_platform.h"
|
||||||
|
}
|
||||||
|
|
||||||
|
class VL53L0X_Direct
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
VL53L0X_Direct();
|
||||||
|
~VL53L0X_Direct();
|
||||||
|
|
||||||
|
bool init();
|
||||||
|
bool readRange(VL53L0X_RangingMeasurementData_t &data);
|
||||||
|
void stop();
|
||||||
|
bool isInitialized() const { return initialized; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool initialized;
|
||||||
|
vl53l0x_dev_t dev;
|
||||||
|
|
||||||
|
static void unbindKernel();
|
||||||
|
};
|
||||||
@@ -4,6 +4,8 @@
|
|||||||
#include <atomic>
|
#include <atomic>
|
||||||
#include <thread>
|
#include <thread>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
|
#include <sched.h>
|
||||||
|
#include <sys/resource.h>
|
||||||
|
|
||||||
#include "global.h"
|
#include "global.h"
|
||||||
#include "camera.h"
|
#include "camera.h"
|
||||||
@@ -25,6 +27,15 @@ int main(void)
|
|||||||
if (avoid_range_val > 0) g_cfg.cone_avoid_range = avoid_range_val;
|
if (avoid_range_val > 0) g_cfg.cone_avoid_range = avoid_range_val;
|
||||||
if (hold_frames_val > 0) g_cfg.cone_hold_frames = hold_frames_val;
|
if (hold_frames_val > 0) g_cfg.cone_hold_frames = hold_frames_val;
|
||||||
|
|
||||||
|
double lidar_gain_val = readDoubleFromFile(lidar_avoid_gain_file);
|
||||||
|
int lidar_range_val = (int)readDoubleFromFile(lidar_avoid_range_file);
|
||||||
|
int lidar_hold_val = (int)readDoubleFromFile(lidar_hold_frames_file);
|
||||||
|
int lidar_thresh_val = (int)readDoubleFromFile(lidar_thresh_file);
|
||||||
|
if (lidar_gain_val > 0) g_cfg.lidar_avoid_gain = lidar_gain_val;
|
||||||
|
if (lidar_range_val > 0) g_cfg.lidar_avoid_range = lidar_range_val;
|
||||||
|
if (lidar_hold_val > 0) g_cfg.lidar_hold_frames = lidar_hold_val;
|
||||||
|
if (lidar_thresh_val > 0) g_cfg.lidar_thresh = lidar_thresh_val;
|
||||||
|
|
||||||
if (CameraInit(0) < 0) {
|
if (CameraInit(0) < 0) {
|
||||||
std::cerr << "CameraInit failed" << std::endl;
|
std::cerr << "CameraInit failed" << std::endl;
|
||||||
return -1;
|
return -1;
|
||||||
@@ -32,9 +43,12 @@ int main(void)
|
|||||||
ControlInit();
|
ControlInit();
|
||||||
std::cout << "All services started" << std::endl;
|
std::cout << "All services started" << std::endl;
|
||||||
|
|
||||||
|
setpriority(PRIO_PROCESS, 0, -10);
|
||||||
|
|
||||||
while (running.load()) {
|
while (running.load()) {
|
||||||
CameraHandler();
|
CameraHandler();
|
||||||
target_speed = g_cfg.speed;
|
target_speed = g_cfg.speed;
|
||||||
|
sched_yield();
|
||||||
}
|
}
|
||||||
|
|
||||||
std::cout << "Stopping..." << std::endl;
|
std::cout << "Stopping..." << std::endl;
|
||||||
|
|||||||
Binary file not shown.
+141
@@ -1,5 +1,6 @@
|
|||||||
#include "camera.h"
|
#include "camera.h"
|
||||||
#include "model_v10.hpp"
|
#include "model_v10.hpp"
|
||||||
|
#include "vl53l0x.h"
|
||||||
|
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
@@ -72,6 +73,19 @@ static int g_cone_hold_ctr = 0;
|
|||||||
static int g_cone_hold_src_row = -1;
|
static int g_cone_hold_src_row = -1;
|
||||||
static int g_cone_hold_src_col = -1;
|
static int g_cone_hold_src_col = -1;
|
||||||
|
|
||||||
|
// ═══════════════════════════════════════════════════════════
|
||||||
|
// 激光雷达挡板避障 (主循环每 5 帧同步读取一次)
|
||||||
|
// ═══════════════════════════════════════════════════════════
|
||||||
|
static VL53L0X g_lidar_sensor;
|
||||||
|
static bool g_lidar_ok = false;
|
||||||
|
static int g_lidar_frames = 0;
|
||||||
|
static bool g_lidar_confirmed = false;
|
||||||
|
static int g_lidar_obstacle_row = -1;
|
||||||
|
static int g_lidar_ref_row = -1;
|
||||||
|
static int g_lidar_hold_ctr = 0;
|
||||||
|
static int g_lidar_hold_src_row = -1;
|
||||||
|
static double g_lidar_hold_dir = 0;
|
||||||
|
|
||||||
// ═══════════════════════════════════════════════════════════
|
// ═══════════════════════════════════════════════════════════
|
||||||
// LCD & FPS
|
// LCD & FPS
|
||||||
// ═══════════════════════════════════════════════════════════
|
// ═══════════════════════════════════════════════════════════
|
||||||
@@ -178,6 +192,18 @@ int CameraInit(int camera_id)
|
|||||||
printf("[MODEL] Mild v12 模型已加载\n");
|
printf("[MODEL] Mild v12 模型已加载\n");
|
||||||
|
|
||||||
i2c_audio_open();
|
i2c_audio_open();
|
||||||
|
|
||||||
|
// 只在启用时才初始化激光,避免驱动内核定时器拖慢 CPU
|
||||||
|
if (g_cfg.lidar_enable) {
|
||||||
|
g_lidar_ok = g_lidar_sensor.init();
|
||||||
|
if (g_lidar_ok)
|
||||||
|
printf("[LIDAR] VL53L0X 已初始化\n");
|
||||||
|
else
|
||||||
|
printf("[LIDAR] VL53L0X 未连接, 避障禁用\n");
|
||||||
|
} else {
|
||||||
|
printf("[LIDAR] 已禁用\n");
|
||||||
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -191,6 +217,7 @@ void cameraDeInit(void)
|
|||||||
}
|
}
|
||||||
close(fb);
|
close(fb);
|
||||||
if (i2c_audio_fd >= 0) close(i2c_audio_fd);
|
if (i2c_audio_fd >= 0) close(i2c_audio_fd);
|
||||||
|
if (g_lidar_ok) g_lidar_sensor.stop();
|
||||||
model_v10_deinit();
|
model_v10_deinit();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -350,6 +377,116 @@ static bool traffic_light_process()
|
|||||||
return (g_tl_state != TL_NORMAL);
|
return (g_tl_state != TL_NORMAL);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// ═══════════════════════════════════════════════════════════
|
||||||
|
// 激光雷达挡板避障
|
||||||
|
// ═══════════════════════════════════════════════════════════
|
||||||
|
static bool lidar_is_active() {
|
||||||
|
return g_lidar_confirmed ||
|
||||||
|
(g_lidar_hold_ctr > 0 && g_lidar_hold_ctr <= g_cfg.lidar_hold_frames);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void lidar_avoid_process()
|
||||||
|
{
|
||||||
|
// 每 5 帧读取一次, 单次测距 ~20ms (高速模式)
|
||||||
|
static int lidar_skip = 0;
|
||||||
|
if (!g_lidar_ok || !g_cfg.lidar_enable) return;
|
||||||
|
if (++lidar_skip < 5) return;
|
||||||
|
lidar_skip = 0;
|
||||||
|
|
||||||
|
// ── 1. 单次激光测距 (高速模式 ~20ms) ──
|
||||||
|
VL53L0X_RangingMeasurementData_t data;
|
||||||
|
if (!g_lidar_sensor.readRange(data)) return;
|
||||||
|
int d_mm = data.RangeMilliMeter;
|
||||||
|
|
||||||
|
if (d_mm >= g_cfg.lidar_thresh) {
|
||||||
|
if (g_lidar_frames > 0)
|
||||||
|
g_lidar_frames = std::max(0, g_lidar_frames - 1);
|
||||||
|
goto hold_decay;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ── 2. 距离 → 图像行映射 ──
|
||||||
|
{
|
||||||
|
int lt_h = line_tracking_height;
|
||||||
|
int row = lt_h - 1 - (d_mm - g_cfg.lidar_near) * (lt_h - 11)
|
||||||
|
/ (g_cfg.lidar_far - g_cfg.lidar_near);
|
||||||
|
if (row < 10) row = 10;
|
||||||
|
if (row >= lt_h) row = lt_h - 1;
|
||||||
|
|
||||||
|
// ── 3a. 近处边线有效检查 ──
|
||||||
|
bool near_valid = false;
|
||||||
|
int near_row = -1;
|
||||||
|
int near_start = std::min(row + g_cfg.lidar_near_start, lt_h - 1);
|
||||||
|
int near_end = std::min(row + g_cfg.lidar_near_end, lt_h - 1);
|
||||||
|
for (int r = near_start; r <= near_end; ++r) {
|
||||||
|
if (left_line[r] != -1 && right_line[r] != -1) {
|
||||||
|
near_valid = true;
|
||||||
|
near_row = r;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// ── 3b. 远处丢线检查 ──
|
||||||
|
bool far_lost = false;
|
||||||
|
int far_start = std::max(row - g_cfg.lidar_far_span, 10);
|
||||||
|
for (int r = row; r >= far_start; --r) {
|
||||||
|
if (left_line[r] == -1 || right_line[r] == -1) {
|
||||||
|
far_lost = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// ── 3c. 联合判定 ──
|
||||||
|
if (near_valid && far_lost) {
|
||||||
|
g_lidar_frames++;
|
||||||
|
g_lidar_obstacle_row = row;
|
||||||
|
g_lidar_ref_row = near_row;
|
||||||
|
} else {
|
||||||
|
if (g_lidar_frames > 0)
|
||||||
|
g_lidar_frames = std::max(0, g_lidar_frames - 1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
g_lidar_confirmed = (g_lidar_frames >= g_cfg.lidar_min_frames);
|
||||||
|
|
||||||
|
// ── 保持/衰减 ──
|
||||||
|
if (g_lidar_confirmed) {
|
||||||
|
g_lidar_hold_ctr = 0;
|
||||||
|
g_lidar_hold_src_row = g_lidar_obstacle_row;
|
||||||
|
// 在参考行判断左右空间，往宽侧推
|
||||||
|
int r = g_lidar_ref_row;
|
||||||
|
double lspace = mid_line[r] - left_line[r];
|
||||||
|
double rspace = right_line[r] - mid_line[r];
|
||||||
|
g_lidar_hold_dir = (lspace > rspace) ? 1.0 : -1.0;
|
||||||
|
if (g_cfg.debug)
|
||||||
|
printf("[LIDAR] 触发 d=%d row=%d dir=%.0f\n", d_mm, g_lidar_obstacle_row, g_lidar_hold_dir);
|
||||||
|
g_lidar_frames = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
hold_decay:
|
||||||
|
if (g_lidar_hold_src_row > 0 && g_lidar_hold_ctr <= g_cfg.lidar_hold_frames) {
|
||||||
|
g_lidar_hold_ctr++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!lidar_is_active()) return;
|
||||||
|
|
||||||
|
// ── 中线变形 ──
|
||||||
|
int src_row = g_lidar_hold_src_row;
|
||||||
|
double rng = (double)g_cfg.lidar_avoid_range;
|
||||||
|
double half_w = line_tracking_width / 2.0;
|
||||||
|
double dir = g_lidar_hold_dir;
|
||||||
|
double decay = g_lidar_confirmed ? 1.0 :
|
||||||
|
(1.0 - (double)g_lidar_hold_ctr / g_cfg.lidar_hold_frames);
|
||||||
|
|
||||||
|
for (int row = 10; row <= src_row; ++row) {
|
||||||
|
double t = std::clamp(((row - 10) / rng), 0.0, 1.0);
|
||||||
|
double push = t * g_cfg.lidar_avoid_gain * half_w * dir * decay;
|
||||||
|
double nm = std::clamp(mid_line[row] + push,
|
||||||
|
(double)left_line[row] + 2.0,
|
||||||
|
(double)right_line[row] - 2.0);
|
||||||
|
mid_line[row] = (int)(nm + 0.5);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// ═══════════════════════════════════════════════════════════
|
// ═══════════════════════════════════════════════════════════
|
||||||
// 锥桶检测 + 中线变形
|
// 锥桶检测 + 中线变形
|
||||||
// ═══════════════════════════════════════════════════════════
|
// ═══════════════════════════════════════════════════════════
|
||||||
@@ -476,6 +613,8 @@ static void motor_update(bool zebra_block, bool tl_block)
|
|||||||
double spd = target_speed;
|
double spd = target_speed;
|
||||||
if (cone_is_slow() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
|
if (cone_is_slow() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
|
||||||
spd *= g_cfg.cone_speed;
|
spd *= g_cfg.cone_speed;
|
||||||
|
if (lidar_is_active() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
|
||||||
|
spd *= g_cfg.lidar_speed;
|
||||||
ControlUpdate(spd, zebra_block || tl_block);
|
ControlUpdate(spd, zebra_block || tl_block);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -534,6 +673,7 @@ static void lcd_render()
|
|||||||
|
|
||||||
// 状态指示
|
// 状态指示
|
||||||
char ztxt[8]; int zi = 0;
|
char ztxt[8]; int zi = 0;
|
||||||
|
if (lidar_is_active()) ztxt[zi++] = 'L';
|
||||||
if (g_tl_state == TL_STOP) ztxt[zi++] = 'R';
|
if (g_tl_state == TL_STOP) ztxt[zi++] = 'R';
|
||||||
else if (g_tl_state == TL_WAIT_GREEN) ztxt[zi++] = 'G';
|
else if (g_tl_state == TL_WAIT_GREEN) ztxt[zi++] = 'G';
|
||||||
if (g_zstate == Z_STOP) ztxt[zi++] = 'S';
|
if (g_zstate == Z_STOP) ztxt[zi++] = 'S';
|
||||||
@@ -596,6 +736,7 @@ int CameraHandler(void)
|
|||||||
// 5. 场景识别
|
// 5. 场景识别
|
||||||
bool zebra_block = zebra_process();
|
bool zebra_block = zebra_process();
|
||||||
bool tl_block = traffic_light_process();
|
bool tl_block = traffic_light_process();
|
||||||
|
lidar_avoid_process();
|
||||||
cone_detect_and_deform();
|
cone_detect_and_deform();
|
||||||
|
|
||||||
// 6. 舵机
|
// 6. 舵机
|
||||||
|
|||||||
@@ -39,4 +39,17 @@ void cfg_load_all()
|
|||||||
|
|
||||||
g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
|
g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
|
||||||
g_cfg.brake_max = readDoubleFromFile(brake_max_file);
|
g_cfg.brake_max = readDoubleFromFile(brake_max_file);
|
||||||
|
|
||||||
|
g_cfg.lidar_thresh = (int)readDoubleFromFile(lidar_thresh_file);
|
||||||
|
g_cfg.lidar_near = (int)readDoubleFromFile(lidar_near_file);
|
||||||
|
g_cfg.lidar_far = (int)readDoubleFromFile(lidar_far_file);
|
||||||
|
g_cfg.lidar_near_start = (int)readDoubleFromFile(lidar_near_start_file);
|
||||||
|
g_cfg.lidar_near_end = (int)readDoubleFromFile(lidar_near_end_file);
|
||||||
|
g_cfg.lidar_far_span = (int)readDoubleFromFile(lidar_far_span_file);
|
||||||
|
g_cfg.lidar_min_frames = (int)readDoubleFromFile(lidar_min_frames_file);
|
||||||
|
g_cfg.lidar_avoid_gain = readDoubleFromFile(lidar_avoid_gain_file);
|
||||||
|
g_cfg.lidar_avoid_range = (int)readDoubleFromFile(lidar_avoid_range_file);
|
||||||
|
g_cfg.lidar_speed = readDoubleFromFile(lidar_speed_file);
|
||||||
|
g_cfg.lidar_hold_frames = (int)readDoubleFromFile(lidar_hold_frames_file);
|
||||||
|
g_cfg.lidar_enable = (int)readDoubleFromFile(lidar_enable_file);
|
||||||
}
|
}
|
||||||
|
|||||||
+116
-44
@@ -2,71 +2,143 @@
|
|||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <sys/ioctl.h>
|
#include <sys/ioctl.h>
|
||||||
|
#include <linux/i2c-dev.h>
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <cerrno>
|
|
||||||
#include <cstdio>
|
#include <cstdio>
|
||||||
|
#include <cerrno>
|
||||||
|
|
||||||
#define VL53L0X_IOCTL_INIT _IO('p', 0x01)
|
extern "C" {
|
||||||
#define VL53L0X_IOCTL_STOP _IO('p', 0x05)
|
#include "vl53l0x_api.h"
|
||||||
#define VL53L0X_IOCTL_GETDATA _IOR('p', 0x0b, VL53L0X_RangingMeasurementData_t)
|
}
|
||||||
|
|
||||||
VL53L0X::VL53L0X() : fd(-1) {}
|
static void unbind_kernel_driver()
|
||||||
|
{
|
||||||
|
FILE *f = fopen("/sys/bus/i2c/drivers/stmvl53l0/unbind", "w");
|
||||||
|
if (f) {
|
||||||
|
fprintf(f, "1-0029");
|
||||||
|
fclose(f);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X::VL53L0X()
|
||||||
|
: fd(-1), initialized(false)
|
||||||
|
{
|
||||||
|
memset(&dev, 0, sizeof(dev));
|
||||||
|
}
|
||||||
|
|
||||||
VL53L0X::~VL53L0X()
|
VL53L0X::~VL53L0X()
|
||||||
{
|
{
|
||||||
if (fd > 0)
|
stop();
|
||||||
{
|
|
||||||
stop();
|
|
||||||
close(fd);
|
|
||||||
fd = -1;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool VL53L0X::init()
|
bool VL53L0X::init()
|
||||||
{
|
{
|
||||||
fd = open("/dev/stmvl53l0x_ranging", O_RDWR | O_SYNC);
|
unbind_kernel_driver();
|
||||||
if (fd <= 0)
|
usleep(100000);
|
||||||
{
|
|
||||||
fprintf(stderr, "[VL53L0X] open failed: %s\n", strerror(errno));
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
ioctl(fd, VL53L0X_IOCTL_STOP, nullptr);
|
fd = open("/dev/i2c-1", O_RDWR);
|
||||||
|
if (fd < 0) {
|
||||||
|
fprintf(stderr, "[VL53L0X] open /dev/i2c-1 failed: %s\n", strerror(errno));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
if (ioctl(fd, VL53L0X_IOCTL_INIT, nullptr) < 0)
|
if (ioctl(fd, I2C_SLAVE, 0x29) < 0) {
|
||||||
{
|
fprintf(stderr, "[VL53L0X] I2C_SLAVE failed: %s\n", strerror(errno));
|
||||||
fprintf(stderr, "[VL53L0X] init failed: %s\n", strerror(errno));
|
close(fd);
|
||||||
close(fd);
|
fd = -1;
|
||||||
fd = -1;
|
return false;
|
||||||
return false;
|
}
|
||||||
}
|
|
||||||
|
|
||||||
fprintf(stderr, "[VL53L0X] init OK\n");
|
dev.I2cDevAddr = 0x29;
|
||||||
return true;
|
dev.i2c_fd = fd;
|
||||||
|
|
||||||
|
VL53L0X_Error Status = VL53L0X_DataInit(&dev);
|
||||||
|
if (Status != VL53L0X_ERROR_NONE) {
|
||||||
|
fprintf(stderr, "[VL53L0X] DataInit failed: %d\n", Status);
|
||||||
|
close(fd);
|
||||||
|
fd = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
Status = VL53L0X_StaticInit(&dev);
|
||||||
|
if (Status != VL53L0X_ERROR_NONE) {
|
||||||
|
fprintf(stderr, "[VL53L0X] StaticInit failed: %d\n", Status);
|
||||||
|
close(fd);
|
||||||
|
fd = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t refSpadCount;
|
||||||
|
uint8_t isApertureSpads;
|
||||||
|
uint8_t VhvSettings;
|
||||||
|
uint8_t PhaseCal;
|
||||||
|
|
||||||
|
VL53L0X_PerformRefCalibration(&dev, &VhvSettings, &PhaseCal);
|
||||||
|
VL53L0X_PerformRefSpadManagement(&dev, &refSpadCount, &isApertureSpads);
|
||||||
|
|
||||||
|
Status = VL53L0X_SetDeviceMode(&dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
|
||||||
|
if (Status != VL53L0X_ERROR_NONE) {
|
||||||
|
fprintf(stderr, "[VL53L0X] SetDeviceMode failed: %d\n", Status);
|
||||||
|
close(fd);
|
||||||
|
fd = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&dev, 20000);
|
||||||
|
|
||||||
|
uint32_t actualBudget = 0;
|
||||||
|
VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&dev, &actualBudget);
|
||||||
|
fprintf(stderr, "[VL53L0X] timing budget = %u us\n", actualBudget);
|
||||||
|
|
||||||
|
initialized = true;
|
||||||
|
fprintf(stderr, "[VL53L0X] init OK\n");
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool VL53L0X::readRange(VL53L0X_RangingMeasurementData_t &data)
|
bool VL53L0X::readRange(VL53L0X_RangingMeasurementData_t &data)
|
||||||
{
|
{
|
||||||
if (fd <= 0)
|
if (!initialized) return false;
|
||||||
return false;
|
|
||||||
|
|
||||||
if (ioctl(fd, VL53L0X_IOCTL_GETDATA, &data) < 0)
|
VL53L0X_Error Status;
|
||||||
{
|
|
||||||
fprintf(stderr, "[VL53L0X] read failed: %s\n", strerror(errno));
|
VL53L0X_ClearInterruptMask(&dev, 0);
|
||||||
return false;
|
Status = VL53L0X_StartMeasurement(&dev);
|
||||||
}
|
if (Status != VL53L0X_ERROR_NONE) {
|
||||||
return true;
|
fprintf(stderr, "[VL53L0X] StartMeasurement failed: %d\n", Status);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t ready = 0;
|
||||||
|
int timeout = 500;
|
||||||
|
while (!ready && --timeout > 0) {
|
||||||
|
Status = VL53L0X_GetMeasurementDataReady(&dev, &ready);
|
||||||
|
if (Status != VL53L0X_ERROR_NONE)
|
||||||
|
break;
|
||||||
|
if (!ready)
|
||||||
|
usleep(1000);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!ready) {
|
||||||
|
fprintf(stderr, "[VL53L0X] measurement timeout\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
Status = VL53L0X_GetRangingMeasurementData(&dev, &data);
|
||||||
|
if (Status != VL53L0X_ERROR_NONE) {
|
||||||
|
fprintf(stderr, "[VL53L0X] GetRangingMeasurementData failed: %d\n", Status);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_ClearInterruptMask(&dev, 0);
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool VL53L0X::stop()
|
bool VL53L0X::stop()
|
||||||
{
|
{
|
||||||
if (fd <= 0)
|
if (fd >= 0) {
|
||||||
return false;
|
close(fd);
|
||||||
|
fd = -1;
|
||||||
if (ioctl(fd, VL53L0X_IOCTL_STOP, nullptr) < 0)
|
}
|
||||||
{
|
initialized = false;
|
||||||
fprintf(stderr, "[VL53L0X] stop failed: %s\n", strerror(errno));
|
return true;
|
||||||
return false;
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -0,0 +1,159 @@
|
|||||||
|
extern "C" {
|
||||||
|
#include "vl53l0x_platform.h"
|
||||||
|
}
|
||||||
|
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_LockSequenceAccess(VL53L0X_DEV Dev)
|
||||||
|
{
|
||||||
|
(void)Dev;
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_UnlockSequenceAccess(VL53L0X_DEV Dev)
|
||||||
|
{
|
||||||
|
(void)Dev;
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index,
|
||||||
|
uint8_t *pdata, uint32_t count)
|
||||||
|
{
|
||||||
|
if (count >= VL53L0X_MAX_I2C_XFER_SIZE)
|
||||||
|
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||||
|
|
||||||
|
uint8_t buf[VL53L0X_MAX_I2C_XFER_SIZE + 1];
|
||||||
|
buf[0] = index;
|
||||||
|
memcpy(buf + 1, pdata, count);
|
||||||
|
|
||||||
|
if (write(Dev->i2c_fd, buf, count + 1) != (ssize_t)(count + 1))
|
||||||
|
return VL53L0X_ERROR_CONTROL_INTERFACE;
|
||||||
|
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index,
|
||||||
|
uint8_t *pdata, uint32_t count)
|
||||||
|
{
|
||||||
|
if (count >= VL53L0X_MAX_I2C_XFER_SIZE)
|
||||||
|
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||||
|
|
||||||
|
if (write(Dev->i2c_fd, &index, 1) != 1)
|
||||||
|
return VL53L0X_ERROR_CONTROL_INTERFACE;
|
||||||
|
|
||||||
|
if (read(Dev->i2c_fd, pdata, count) != (ssize_t)count)
|
||||||
|
return VL53L0X_ERROR_CONTROL_INTERFACE;
|
||||||
|
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
|
||||||
|
{
|
||||||
|
return VL53L0X_WriteMulti(Dev, index, &data, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data)
|
||||||
|
{
|
||||||
|
uint8_t buf[2];
|
||||||
|
buf[0] = (data >> 8) & 0xFF;
|
||||||
|
buf[1] = data & 0xFF;
|
||||||
|
return VL53L0X_WriteMulti(Dev, index, buf, 2);
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
|
||||||
|
{
|
||||||
|
uint8_t buf[4];
|
||||||
|
buf[0] = (data >> 24) & 0xFF;
|
||||||
|
buf[1] = (data >> 16) & 0xFF;
|
||||||
|
buf[2] = (data >> 8) & 0xFF;
|
||||||
|
buf[3] = data & 0xFF;
|
||||||
|
return VL53L0X_WriteMulti(Dev, index, buf, 4);
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data)
|
||||||
|
{
|
||||||
|
return VL53L0X_ReadMulti(Dev, index, data, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data)
|
||||||
|
{
|
||||||
|
uint8_t buf[2];
|
||||||
|
VL53L0X_Error Status = VL53L0X_ReadMulti(Dev, index, buf, 2);
|
||||||
|
*data = ((uint16_t)buf[0] << 8) | buf[1];
|
||||||
|
return Status;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data)
|
||||||
|
{
|
||||||
|
uint8_t buf[4];
|
||||||
|
VL53L0X_Error Status = VL53L0X_ReadMulti(Dev, index, buf, 4);
|
||||||
|
*data = ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) |
|
||||||
|
((uint32_t)buf[2] << 8) | buf[3];
|
||||||
|
return Status;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index,
|
||||||
|
uint8_t AndData, uint8_t OrData)
|
||||||
|
{
|
||||||
|
uint8_t data;
|
||||||
|
VL53L0X_Error Status = VL53L0X_RdByte(Dev, index, &data);
|
||||||
|
if (Status != VL53L0X_ERROR_NONE)
|
||||||
|
return Status;
|
||||||
|
data = (data & AndData) | OrData;
|
||||||
|
return VL53L0X_WrByte(Dev, index, data);
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev)
|
||||||
|
{
|
||||||
|
(void)Dev;
|
||||||
|
usleep(1000);
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* string/internal stubs — never called but needed for linking */
|
||||||
|
extern "C" {
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_device_info(VL53L0X_DEV Dev, VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
|
||||||
|
{
|
||||||
|
(void)Dev; (void)pVL53L0X_DeviceInfo;
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_device_error_string(VL53L0X_DeviceError ErrorCode, char *pDeviceErrorString)
|
||||||
|
{
|
||||||
|
(void)ErrorCode; (void)pDeviceErrorString;
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_range_status_string(uint8_t RangeStatus, char *pRangeStatusString)
|
||||||
|
{
|
||||||
|
(void)RangeStatus; (void)pRangeStatusString;
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_pal_error_string(VL53L0X_Error PalErrorCode, char *pPalErrorString)
|
||||||
|
{
|
||||||
|
(void)PalErrorCode; (void)pPalErrorString;
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_pal_state_string(VL53L0X_State PalStateCode, char *pPalStateString)
|
||||||
|
{
|
||||||
|
(void)PalStateCode; (void)pPalStateString;
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_sequence_steps_info(VL53L0X_SequenceStepId SequenceStepId, char *pSequenceStepsString)
|
||||||
|
{
|
||||||
|
(void)SequenceStepId; (void)pSequenceStepsString;
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
VL53L0X_Error VL53L0X_get_limit_check_info(VL53L0X_DEV Dev, uint16_t LimitCheckId, char *pLimitCheckString)
|
||||||
|
{
|
||||||
|
(void)Dev; (void)LimitCheckId; (void)pLimitCheckString;
|
||||||
|
return VL53L0X_ERROR_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user