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

替换 Mild v12 模型 + 同事编码器刹车红绿灯状态机合并

Browse Source
This commit is contained in:
spdis
2026-06-13 13:05:18 +08:00
parent 457d608621
commit b9e32c5a40
66 changed files with 1698 additions and 5625 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/camera.cpp
+171 -7
View File
@@ -22,7 +22,8 @@ PwmController servo(1, 0);
// ── 模型检测参数 ──
#define ZEBRA_CLASS 3
static float g_thresh[4] = {0.80f, 0.80f, 0.80f, 0.75f};
#define CONE_CLASS 0
static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.90f};
// ── 斑马线去抖 ──
#define ZEBRA_MIN_FRAMES 5
@@ -36,6 +37,23 @@ static ZState g_zstate = Z_NORMAL;
static time_t g_ztime = 0;
static bool g_zebra_ever = false;
// ── 红绿灯状态机 ──
#define TL_RED_CLASS 1
#define TL_GREEN_CLASS 2
enum TLState { TL_NORMAL, TL_STOP, TL_WAIT_GREEN };
static TLState g_tl_state = TL_NORMAL;
static int g_tl_red_frames = 0;
static int g_tl_green_frames = 0;
// ── 锥桶检测 ──
static int g_cone_frames = 0;
static int g_cone_last_row = -1;
static int g_cone_last_col = -1;
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 DetectBoxV10 g_boxes[16];
static int g_box_count = 0;
@@ -110,10 +128,10 @@ int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
line_tracking_width = newWidth / calc_scale;
line_tracking_height = newHeight / calc_scale;
if (!model_v10_init("./nanodetv10.bin")) {
if (!model_v10_init("./mild_v12.bin")) {
printf("[MODEL] 警告: 模型加载失败\n");
} else {
printf("[MODEL] v10 模型已加载\n");
printf("[MODEL] Mild v12 模型已加载\n");
}
i2c_audio_open();
@@ -283,6 +301,138 @@ int CameraHandler(void)
break;
}
// ── 5.5 红绿灯状态机 ──
bool red_seen = false;
bool green_seen = false;
for (int i = 0; i < g_box_count; ++i) {
if (g_boxes[i].cls == TL_RED_CLASS) red_seen = true;
if (g_boxes[i].cls == TL_GREEN_CLASS) green_seen = true;
}
if (red_seen) g_tl_red_frames++;
else g_tl_red_frames = std::max(0, g_tl_red_frames - 1);
if (green_seen) g_tl_green_frames++;
else g_tl_green_frames = std::max(0, g_tl_green_frames - 1);
switch (g_tl_state) {
case TL_NORMAL:
if (g_tl_red_frames >= 3) {
g_tl_state = TL_STOP;
if (g_cfg.debug) printf("[TL] 红灯停车\n");
}
break;
case TL_STOP:
if (!red_seen) {
g_tl_state = TL_WAIT_GREEN;
if (g_cfg.debug) printf("[TL] 等待绿灯\n");
}
break;
case TL_WAIT_GREEN:
if (g_tl_green_frames >= 3) {
g_tl_state = TL_NORMAL;
g_tl_red_frames = 0;
g_tl_green_frames = 0;
if (g_cfg.debug) printf("[TL] 绿灯通行\n");
}
break;
}
bool tl_block = (g_tl_state != TL_NORMAL);
// ── 5.6 锥桶检测 + 中线变形 ──
bool cone_seen = false;
int cone_row_keep = -1;
int cone_col_keep = -1;
if (g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
for (int i = 0; i < g_box_count; ++i) {
if (g_boxes[i].cls != CONE_CLASS) continue;
if (g_boxes[i].conf < g_cfg.cone_thresh) continue;
int col_lt = g_boxes[i].cx * line_tracking_width / raw_frame.cols;
int row_lt = g_boxes[i].cy * line_tracking_height / raw_frame.rows;
if (row_lt < 10 || row_lt >= line_tracking_height) continue;
if (left_line[row_lt] == -1 || right_line[row_lt] == -1) continue;
if (g_cfg.cone_margin) {
int bl = (g_boxes[i].cx - g_boxes[i].w/2) * line_tracking_width / raw_frame.cols;
int br = (g_boxes[i].cx + g_boxes[i].w/2) * line_tracking_width / raw_frame.cols;
if (br < left_line[row_lt] || bl > right_line[row_lt]) continue;
} else {
if (col_lt < left_line[row_lt] || col_lt > right_line[row_lt]) continue;
}
if (row_lt > cone_row_keep) { cone_row_keep = row_lt; cone_col_keep = col_lt; }
cone_seen = true;
}
}
{
int row_tol = std::max(2, line_tracking_height / 12);
int col_tol = std::max(3, line_tracking_width / 8);
if (cone_seen) {
if (g_cone_frames == 0) {
g_cone_last_row = cone_row_keep;
g_cone_last_col = cone_col_keep;
g_cone_frames = 1;
} else {
if (std::abs(cone_row_keep - g_cone_last_row) <= row_tol &&
std::abs(cone_col_keep - g_cone_last_col) <= col_tol) {
g_cone_frames++;
g_cone_last_row = cone_row_keep;
g_cone_last_col = cone_col_keep;
} else {
g_cone_frames = 1;
g_cone_last_row = cone_row_keep;
g_cone_last_col = cone_col_keep;
}
}
} else {
if (g_cone_frames > 0) g_cone_frames = std::max(0, g_cone_frames - 1);
}
}
g_cone_confirmed = (g_cone_frames >= g_cfg.cone_min_frames);
if (g_cone_confirmed) {
g_cone_hold_ctr = 0;
g_cone_hold_src_row = cone_row_keep;
g_cone_hold_src_col = cone_col_keep;
} else if (g_cone_hold_src_row > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) {
g_cone_hold_ctr++;
}
bool cone_active = g_cone_confirmed ||
(g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);
if (cone_active && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
int src_row = g_cone_confirmed ? cone_row_keep : g_cone_hold_src_row;
int src_col = g_cone_confirmed ? cone_col_keep : g_cone_hold_src_col;
int start_row = 10;
if (src_row > start_row) {
double total_rows = src_row - start_row;
double rng = (double)g_cfg.cone_avoid_range;
double half_w = line_tracking_width / 2.0;
double mid_at_cone = mid_line[src_row];
double dir = (src_col < mid_at_cone) ? 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 = start_row; row <= src_row; row++) {
double t_raw = (row - start_row) / total_rows;
double t = std::clamp(t_raw * (total_rows / rng), 0.0, 1.0);
double push = t * g_cfg.cone_avoid_gain * half_w * dir * decay;
double new_mid = mid_line[row] + push;
new_mid = std::clamp(new_mid,
(double)left_line[row] + 2.0,
(double)right_line[row] - 2.0);
mid_line[row] = (int)(new_mid + 0.5);
}
}
}
// ── 6. 舵机 ──
if (g_cfg.start) {
int foresee = (int)g_cfg.foresee;
@@ -304,7 +454,15 @@ int CameraHandler(void)
}
// ── 7. 电机控制 ──
ControlUpdate(target_speed, g_zstate == Z_STOP);
{
double spd = target_speed;
bool cone_slow = g_cone_confirmed ||
(g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);
if (cone_slow && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
spd *= g_cfg.cone_speed;
}
ControlUpdate(spd, g_zstate == Z_STOP || tl_block);
}
clock_gettime(CLOCK_MONOTONIC,&t4);
// ── 8. LCD ──
@@ -340,14 +498,20 @@ int CameraHandler(void)
x2=std::max(0,std::min(newWidth-1,x2)); y2=std::max(0,std::min(newHeight-1,y2));
cv::Scalar color(0,255,0);
if (g_boxes[i].cls == ZEBRA_CLASS) color=cv::Scalar(255,0,255);
else if (g_boxes[i].cls == CONE_CLASS) color=cv::Scalar(0,165,255);
cv::rectangle(lcd_fbImage(roi), cv::Point(x1,y1), cv::Point(x2,y2), color, 2);
char lab[16]; std::snprintf(lab,16,"%d %.0f",g_boxes[i].cls,g_boxes[i].conf*100);
cv::putText(lcd_fbImage(roi), lab, cv::Point(x1+2,y1+10), cv::FONT_HERSHEY_SIMPLEX,0.3,color,1);
}
const char* ztxt="N";
if (g_zstate==Z_STOP) ztxt="S";
else if (g_zstate==Z_COOLDOWN) ztxt="C";
char ztxt[8];
int zidx = 0;
if (g_tl_state == TL_STOP) ztxt[zidx++] = 'R';
else if (g_tl_state == TL_WAIT_GREEN) ztxt[zidx++] = 'G';
if (g_zstate == Z_STOP) ztxt[zidx++] = 'S';
else if (g_zstate == Z_COOLDOWN) ztxt[zidx++] = 'C';
else ztxt[zidx++] = 'N';
ztxt[zidx] = '\0';
cv::putText(lcd_fbImage(roi), ztxt, cv::Point(2,newHeight-4), cv::FONT_HERSHEY_SIMPLEX,0.4,cv::Scalar(0,255,255),1);
convertMatToRGB565(lcd_fbImage, fb_buffer, screenWidth, screenHeight);