spdis/Loongson_2k0300_SmartCar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

C路: VL53L0X 用户态I2C直驱 + 高速模式 29.6fps

Browse Source 
- 搬运 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
2026-06-17 13:47:13 +08:00
parent 06a73b0f02
commit 95a46c130d
31 changed files with 11645 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/camera.cpp
+141
View File
@@ -1,5 +1,6 @@
#include "camera.h"
#include "model_v10.hpp"
#include "vl53l0x.h"
#include <fcntl.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_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
// ═══════════════════════════════════════════════════════════
@@ -178,6 +192,18 @@ int CameraInit(int camera_id)
printf("[MODEL] Mild v12 模型已加载\n");
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;
}
@@ -191,6 +217,7 @@ void cameraDeInit(void)
}
close(fb);
if (i2c_audio_fd >= 0) close(i2c_audio_fd);
if (g_lidar_ok) g_lidar_sensor.stop();
model_v10_deinit();
}
@@ -350,6 +377,116 @@ static bool traffic_light_process()
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;
if (cone_is_slow() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
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);
}
@@ -534,6 +673,7 @@ static void lcd_render()
// 状态指示
char ztxt[8]; int zi = 0;
if (lidar_is_active()) ztxt[zi++] = 'L';
if (g_tl_state == TL_STOP) ztxt[zi++] = 'R';
else if (g_tl_state == TL_WAIT_GREEN) ztxt[zi++] = 'G';
if (g_zstate == Z_STOP) ztxt[zi++] = 'S';
@@ -596,6 +736,7 @@ int CameraHandler(void)
// 5. 场景识别
bool zebra_block = zebra_process();
bool tl_block = traffic_light_process();
lidar_avoid_process();
cone_detect_and_deform();
// 6. 舵机