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

斑马线: 3f去抖/-1衰减/3s停车/赛道外忽略/丢线忽略/reject日志; 起步弹射2x/0.7s; 斑马线后delay1.5s改foresee=35/5s; PWM构造先归零防瞬动

Browse Source
This commit is contained in:
spdis
2026-06-29 18:38:14 +08:00
parent 4a38259d25
commit 2cab70791e
2 changed files with 82 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MotorController.cpp
+1
View File
@@ -7,6 +7,7 @@ MotorController::MotorController(int pwmchip, int pwmnum, int gpioNum, unsigned
: pwmController(pwmchip, pwmnum), directionGPIO(gpioNum)
{
pwmController.setPeriod(period_ns);
pwmController.setDutyCycle(0); // 先归零再使能, 防止 sysfs 残留值导致电机瞬动
directionGPIO.setDirection("out");
pwmController.enable();
}
src/camera.cpp
+81 -11
View File
@@ -86,7 +86,7 @@ static int g_box_count = 0;
// g_zc_min_cy: 本轮追踪中斑马线出现时的最小 cy (越小=越远)
// g_ztime: 进入 Z_STOP / Z_COOLDOWN 的时间戳
// ═══════════════════════════════════════════════════════════
static constexpr int ZEBRA_MIN_FRAMES = 5; // 连续确认帧数
static constexpr int ZEBRA_MIN_FRAMES = 3; // 连续确认帧数
static constexpr int ZEBRA_FAR_CY = 80; // 远处 cy 阈值: 斑马线必须曾出现在 cy≤80
static int g_zc_frames = 0;
static int g_zc_min_cy = 120; // 初始设远 (120=从未检测到)
@@ -426,6 +426,12 @@ static void run_model_inference()
// ═══════════════════════════════════════════════════════════
static bool zebra_process()
{
// 双边丢线超过50% → 不采纳任何斑马线检测
if (g_lost_rows > line_tracking_height / 2) {
if (g_cfg.debug) printf("[ZEBRA] 忽略: 丢线%d/%d行\n", g_lost_rows, line_tracking_height);
return false;
}
bool zebra_seen = false;
bool zebra_near = false;
int zebra_cy = 0;
@@ -434,6 +440,30 @@ static bool zebra_process()
// 遍历检测框, 找第一个斑马线
for (int i = 0; i < g_box_count; ++i) {
if (g_boxes[i].cls != MD_ZEBRA) continue;
// ── 赛道内占比检查: 框80%以上在赛道外 → 忽略 ──
{
float bx = (float)line_tracking_width / raw_frame.cols;
float by = (float)line_tracking_height / raw_frame.rows;
int bx1 = (int)((g_boxes[i].cx - g_boxes[i].w/2) * bx);
int by1 = (int)((g_boxes[i].cy - g_boxes[i].h/2) * by);
int bx2 = (int)((g_boxes[i].cx + g_boxes[i].w/2) * bx);
int by2 = (int)((g_boxes[i].cy + g_boxes[i].h/2) * by);
int overlap = 0, area = (bx2 - bx1) * (by2 - by1);
if (area > 0) {
int y0 = std::max(0, std::min(by1, line_tracking_height - 1));
int y1 = std::min(by2, line_tracking_height - 1);
for (int y = y0; y <= y1 && y >= 0; ++y) {
if (left_line[y] == -1 || right_line[y] == -1) continue;
int ol = std::max(0, std::min(bx2, right_line[y]) - std::max(bx1, left_line[y]));
overlap += ol;
}
}
if (area > 0 && (float)overlap / area < 0.2f) {
if (g_cfg.debug) printf("[ZEBRA] 忽略: 赛道内仅%.0f%% cf=%.2f\n",
(float)overlap / area * 100, g_boxes[i].conf);
continue; // 赛道内<20% → 忽略
}
}
zebra_cy = (int)g_boxes[i].cy;
zebra_cf = g_boxes[i].conf;
zebra_seen = true;
@@ -447,7 +477,7 @@ static bool zebra_process()
// 未检测到 → 衰减去抖计数
if (!zebra_seen) {
if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 2); // 每帧 -2 快速衰减
if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 1); // 每帧 -1 衰减
if (g_zc_frames == 0) g_zc_min_cy = 120; // 完全衰减 → 重置远处追踪
}
@@ -458,12 +488,19 @@ static bool zebra_process()
if (zebra_near && g_zc_frames >= ZEBRA_MIN_FRAMES && g_zc_min_cy <= ZEBRA_FAR_CY) {
play_zebra_audio(); // I2C 语音播报
g_zstate = Z_STOP; g_ztime = now;
if (g_cfg.debug) printf("[ZEBRA] cy=%d cf=%.2f 停车4s 冷却5s\n", zebra_cy, zebra_cf);
if (g_cfg.debug) printf("[ZEBRA] cy=%d cf=%.2f 停车3s 冷却5s\n", zebra_cy, zebra_cf);
g_zc_frames = 0; g_zc_min_cy = 120; // 重置, 为下次检测做准备
} else if (zebra_seen && g_cfg.debug) {
if (!zebra_near)
printf("[ZEBRA] 忽略: 太远 cy=%d <= %d\n", zebra_cy, (int)g_cfg.zebrasee);
else if (g_zc_frames < ZEBRA_MIN_FRAMES)
printf("[ZEBRA] 忽略: 帧数不足 %d/%d cy=%d\n", g_zc_frames, ZEBRA_MIN_FRAMES, zebra_cy);
else if (g_zc_min_cy > ZEBRA_FAR_CY)
printf("[ZEBRA] 忽略: 未曾远距出现 min_cy=%d > %d\n", g_zc_min_cy, ZEBRA_FAR_CY);
}
break;
case Z_STOP:
if (now - g_ztime >= 4) { // 停车 4 秒
if (now - g_ztime >= 3) { // 停车 3 秒
g_zstate = Z_COOLDOWN; g_ztime = now;
if (g_cfg.debug) printf("[ZEBRA] 起步\n");
}
@@ -863,12 +900,12 @@ static void detect_sharp_turn()
static void steering_update()
{
if (!g_cfg.start) return;
// foresee (显示空间像素) → check_row (巡线空间行号)
// double scale = 1.0;
// if (g_sharp_turn) scale = g_cfg.sharp_turn_scale;
// else if (g_lost_rows > 15) scale = g_cfg.foresee_lost_scale;
// double eff_foresee = g_cfg.foresee * scale;
int check_row = (int)g_cfg.foresee / calc_scale;
// 斑马线起步后延迟1.5s, 再5s内缩短前瞻, 先走稳再改
double eff_foresee = g_cfg.foresee;
if (g_zstate == Z_COOLDOWN && time(nullptr) - g_ztime >= 2
&& time(nullptr) - g_ztime < 5)
eff_foresee = 35;
int check_row = (int)eff_foresee / calc_scale;
if (check_row < 0 || check_row >= line_tracking_height) return;
if (mid_line[check_row] == -1) return; // 丢线行 → 跳过
@@ -904,10 +941,43 @@ static void steering_update()
// 调用 ControlUpdate(final_spd, block) 执行:
// block = zebra_block || tl_block → 编码器比例刹车
// 否则 → 开环占空比驱动
//
// 起步弹射: 启动/斑马线恢复后 1.5x 速度持续 0.5s 快速起步
// ═══════════════════════════════════════════════════════════
static struct timespec g_boost_end = {0, 0};
static double g_boost_mul = 2.0;
static constexpr double BOOST_SECS = 0.7;
static void trigger_boost()
{
clock_gettime(CLOCK_MONOTONIC, &g_boost_end);
g_boost_end.tv_nsec += (long)(BOOST_SECS * 1e9);
if (g_boost_end.tv_nsec >= 1000000000L) {
g_boost_end.tv_sec += 1;
g_boost_end.tv_nsec -= 1000000000L;
}
}
static double boost_factor()
{
struct timespec now;
clock_gettime(CLOCK_MONOTONIC, &now);
if (now.tv_sec > g_boost_end.tv_sec) return 1.0;
if (now.tv_sec == g_boost_end.tv_sec && now.tv_nsec >= g_boost_end.tv_nsec) return 1.0;
return g_boost_mul;
}
static void motor_update(bool zebra_block, bool tl_block)
{
double spd = target_speed;
// 起步弹射检测
static int prev_start = 0;
static ZState prev_zstate = Z_NORMAL;
if (g_cfg.start && !prev_start) trigger_boost(); // 启动
if (prev_zstate == Z_STOP && g_zstate == Z_COOLDOWN) trigger_boost(); // 斑马线起步
prev_start = g_cfg.start;
prev_zstate = g_zstate;
double spd = target_speed * boost_factor();
// 弯道减速: 舵机偏差越大 → 速度越低
double curve = 1.0 - std::abs(g_steer_deviation) * g_cfg.curve_slope;