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

模型每类阈值+配置热重载+雷达纯距离触发+速度min不叠加+弯道参数化

Browse Source
This commit is contained in:
spdis
2026-06-21 14:15:53 +08:00
parent 95a46c130d
commit 40d5cb1604
11 changed files with 234 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/camera.cpp
+118 -90
View File
@@ -15,6 +15,7 @@
#include <cstring>
#include <sstream>
#include <iomanip>
#include <malloc.h>
// ═══════════════════════════════════════════════════════════
// 模型检测类别
@@ -38,7 +39,7 @@ static constexpr int calc_scale = 2;
// ═══════════════════════════════════════════════════════════
// 模型相关静态
// ═══════════════════════════════════════════════════════════
static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.90f};
static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.82f};
static DetectBoxV10 g_boxes[16];
static int g_box_count = 0;
@@ -72,6 +73,8 @@ static bool g_cone_confirmed = false;
static int g_cone_hold_ctr = 0;
static int g_cone_hold_src_row = -1;
static int g_cone_hold_src_col = -1;
static int g_cone_return_ctr = 0;
static double g_cone_return_dir = 0;
// ═══════════════════════════════════════════════════════════
// 激光雷达挡板避障 (主循环每 5 帧同步读取一次)
@@ -81,9 +84,8 @@ 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 int g_lidar_hold_ctr = 0;
static double g_lidar_hold_dir = 0;
// ═══════════════════════════════════════════════════════════
@@ -196,9 +198,10 @@ int CameraInit(int camera_id)
// 只在启用时才初始化激光,避免驱动内核定时器拖慢 CPU
if (g_cfg.lidar_enable) {
g_lidar_ok = g_lidar_sensor.init();
if (g_lidar_ok)
if (g_lidar_ok) {
printf("[LIDAR] VL53L0X 已初始化\n");
else
g_lidar_sensor.startMeasure();
} else
printf("[LIDAR] VL53L0X 未连接, 避障禁用\n");
} else {
printf("[LIDAR] 已禁用\n");
@@ -387,89 +390,62 @@ static bool lidar_is_active() {
static void lidar_avoid_process()
{
// 每 5 帧读取一次, 单次测距 ~20ms (高速模式)
// 每 2 帧一读 (15Hz), 预触发
static int lidar_skip = 0;
if (!g_lidar_ok || !g_cfg.lidar_enable) return;
if (++lidar_skip < 5) return;
if (++lidar_skip < 2) return;
lidar_skip = 0;
// ── 1. 单次激光测距 (高速模式 ~20ms) ──
VL53L0X_RangingMeasurementData_t data;
if (!g_lidar_sensor.readRange(data)) return;
if (!g_lidar_sensor.readResult(data)) return;
int d_mm = data.RangeMilliMeter;
g_lidar_sensor.startMeasure();
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;
// 超出预触发窗口 → 减帧(不归零), 传感器0不计数
int lt_h = line_tracking_height;
bool skip = false;
if (d_mm == 0) {
skip = true; // 读失败, 本帧不动
} else if (d_mm >= g_cfg.lidar_pre) {
if (g_lidar_frames > 0) g_lidar_frames--;
skip = true;
}
// ── 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;
if (!skip) {
// 距离 → 图像行
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;
}
}
// 纯距离触发, 不看赛道线
g_lidar_frames++;
g_lidar_obstacle_row = row;
// ── 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;
}
}
g_lidar_confirmed = (d_mm < g_cfg.lidar_thresh) && (g_lidar_frames >= g_cfg.lidar_min_frames);
// ── 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);
}
static int dbg = 0;
if (g_cfg.debug && ++dbg >= 5) { dbg = 0;
printf("[LIDAR] d=%d row=%d frames=%d confirm=%d\n",
d_mm, row, g_lidar_frames, g_lidar_confirmed);
}
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_hold_dir = 1.0;
if (g_cfg.debug) printf("[LIDAR] 触发 d=%d row=%d dir=右\n", d_mm, g_lidar_obstacle_row);
g_lidar_frames = 0;
}
}
hold_decay:
if (g_lidar_hold_src_row > 0 && g_lidar_hold_ctr <= g_cfg.lidar_hold_frames) {
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;
@@ -477,13 +453,13 @@ hold_decay:
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);
for (int i = 10; i < lt_h; ++i) {
double t = (i <= src_row) ? std::clamp((i - 10) / rng, 0.0, 1.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);
double nm = std::clamp(mid_line[i] + push,
(double)left_line[i] + 2.0,
(double)right_line[i] - 2.0);
mid_line[i] = (int)(nm + 0.5);
}
}
@@ -492,7 +468,8 @@ hold_decay:
// ═══════════════════════════════════════════════════════════
static bool cone_is_slow() {
return g_cone_confirmed ||
(g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);
(g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) ||
(g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_hold_frames / 3);
}
static void cone_detect_and_deform()
@@ -550,11 +527,19 @@ static void cone_detect_and_deform()
// ── 保持/衰减 ──
if (g_cone_confirmed) {
g_cone_hold_ctr = 0;
g_cone_hold_ctr = 0;
g_cone_hold_src_row = cone_row;
g_cone_hold_src_col = cone_col;
g_cone_return_ctr = 0;
} else if (g_cone_hold_src_row > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) {
g_cone_hold_ctr++;
if (g_cone_hold_ctr > g_cfg.cone_hold_frames) {
double ms = mid_line[g_cone_hold_src_row];
g_cone_return_dir = (g_cone_hold_src_col < ms) ? -1.0 : 1.0;
g_cone_return_ctr = 1;
}
} else if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_hold_frames / 3) {
g_cone_return_ctr++;
}
if (!cone_is_slow()) return;
@@ -571,13 +556,33 @@ static void cone_detect_and_deform()
double dir = (src_col < ms) ? 1.0 : -1.0;
double decay = g_cone_confirmed ? 1.0 : (1.0 - (double)g_cone_hold_ctr / g_cfg.cone_hold_frames);
for (int row = 10; row <= src_row; ++row) {
double t = std::clamp(((row - 10) / rng), 0.0, 1.0);
double push = t * g_cfg.cone_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);
// ── 避开阶段: 推离锥桶 ──
if (g_cone_return_ctr == 0) {
for (int row = 10; row <= src_row; ++row) {
double t = std::clamp(((row - 10) / rng), 0.0, 1.0);
double push = t * g_cfg.cone_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);
}
}
// ── 回弹: 锥桶消失后朝锥桶方向推, 把车带回赛道中心 ──
if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_hold_frames / 3) {
int lt_h = line_tracking_height;
double ret_rng = rng * 1.2;
double ret_hw = half_w;
double ret_dir = g_cone_return_dir;
double ret_dec = 1.0 - (double)g_cone_return_ctr / (g_cfg.cone_hold_frames / 3);
for (int row = 10; row < lt_h; ++row) {
double t = std::clamp((row - 10) / ret_rng, 0.0, 1.0);
double push = t * g_cfg.cone_avoid_gain * 0.5 * ret_hw * ret_dir * ret_dec;
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);
}
}
}
@@ -611,11 +616,27 @@ static void steering_update()
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);
// 弯道减速
double curve = 1.0 - std::abs(g_steer_deviation) * g_cfg.curve_slope;
if (curve < g_cfg.curve_min) curve = g_cfg.curve_min;
// 取最低倍率: 弯道 / 锥桶 / 挡板 三者不叠加
double factor = curve;
// if (cone_is_slow() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
// factor = std::min(factor, g_cfg.cone_speed);
// if (lidar_is_active() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
// factor = std::min(factor, g_cfg.lidar_speed);
double final_spd = spd * factor;
static int dbg = 0;
if (++dbg >= 30) { dbg = 0;
printf("[SPD] base=%.0f dev=%.2f curve=%.2f final=%.1f\n",
spd, g_steer_deviation, curve, final_spd);
}
ControlUpdate(final_spd, zebra_block || tl_block);
}
@@ -717,6 +738,11 @@ static void fps_log(struct timespec *ts)
// ═══════════════════════════════════════════════════════════
int CameraHandler(void)
{
static int reloadCnt = 0;
if (g_cfg.debug && ++reloadCnt >= 30) { reloadCnt = 0; cfg_load_all(); }
static int trimCnt = 0;
if (++trimCnt >= 150) { trimCnt = 0; malloc_trim(0); }
struct timespec ts[5]; // [0]=capture_end, [1]=vision_end, [2]=model_end, [3]=control_end, [4]=fps
// 1. 取帧 + 解码
@@ -729,27 +755,29 @@ int CameraHandler(void)
image_main();
clock_gettime(CLOCK_MONOTONIC, &ts[2]);
// 4. 模型推理
// 4. 避障激光 (预触发: 读数+起测 ~1ms, 测距 20ms 后台跑)
lidar_avoid_process();
// 5. 模型推理
run_model_inference();
clock_gettime(CLOCK_MONOTONIC, &ts[3]);
// 5. 场景识别
// 6. 场景识别
bool zebra_block = zebra_process();
bool tl_block = traffic_light_process();
lidar_avoid_process();
cone_detect_and_deform();
// 6. 舵机
// 7. 舵机
steering_update();
// 7. 电机
// 8. 电机
motor_update(zebra_block, tl_block);
clock_gettime(CLOCK_MONOTONIC, &ts[4]);
// 8. LCD
// 9. LCD
lcd_render();
// 9. FPS
// 10. FPS
fps_log(ts);
return 0;