From 54a9915bf5fb85555b1e83f53a54be59cbdbdebc Mon Sep 17 00:00:00 2001 From: spdis Date: Sat, 13 Jun 2026 14:07:35 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E4=BC=98=E9=9B=85=E5=8C=96?= =?UTF-8?q?=E9=87=8D=E6=9E=84:=20CameraHandler=E6=8B=869=E4=B8=AA=E5=87=BD?= =?UTF-8?q?=E6=95=B0+=E5=88=A0serial=E6=AD=BB=E4=BB=A3=E7=A0=81+=E5=B8=B8?= =?UTF-8?q?=E9=87=8F=E7=BB=9F=E4=B8=80+=E6=AD=BB=E4=BA=A1=E5=8F=82?= =?UTF-8?q?=E6=95=B0=E6=B8=85=E7=90=86+camera.h=E7=B2=BE=E7=AE=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 3 +- ctl.sh | 6 - lib/camera.h | 16 +- main/main.cpp | 2 +- src/camera.cpp | 672 +++++++++++++++++++++++++++---------------------- 5 files changed, 381 insertions(+), 318 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index be26cba..ac307f2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,8 @@ aux_source_directory(src DIR_SRCS) # vl53l0x.cpp — 激光测距, 硬件未接 # zebra_detect.cpp — 经典斑马线检测, 已被 Mild 模型替代 # PIDController.cpp — PID 类未使用 (电机开环直驱) -list(FILTER DIR_SRCS EXCLUDE REGEX "(vl53l0x\\.cpp|zebra_detect\\.cpp|PIDController\\.cpp)") +# serial.cpp — Vofa/串口图传未使用 +list(FILTER DIR_SRCS EXCLUDE REGEX "(vl53l0x\\.cpp|zebra_detect\\.cpp|PIDController\\.cpp|serial\\.cpp)") # 静态库 + 主程序 add_library(common_lib STATIC ${DIR_SRCS}) diff --git a/ctl.sh b/ctl.sh index 1043929..ae2420f 100644 --- a/ctl.sh +++ b/ctl.sh @@ -35,12 +35,6 @@ init_pins() { echo 11 > "$DIR/speed" 2>/dev/null echo 8 > "$DIR/deadband" 2>/dev/null echo 1.5 > "$DIR/steer_gain" 2>/dev/null - echo 3.5 > "$DIR/kp" 2>/dev/null - echo 0.3 > "$DIR/ki" 2>/dev/null - echo 2.0 > "$DIR/kd" 2>/dev/null - echo 0.6 > "$DIR/mortor_kp" 2>/dev/null - echo 0.2 > "$DIR/mortor_ki" 2>/dev/null - echo 0 > "$DIR/mortor_kd" 2>/dev/null echo 40 > "$DIR/foresee" 2>/dev/null echo 0 > "$DIR/start" 2>/dev/null diff --git a/lib/camera.h b/lib/camera.h index 1996276..378342a 100644 --- a/lib/camera.h +++ b/lib/camera.h @@ -1,27 +1,15 @@ #ifndef CAMERA_H_ #define CAMERA_H_ +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include "image_cv.h" #include "global.h" #include "frame_buffer.h" #include "control.h" -int CameraInit(uint8_t camera_id, double dest_fps, int width, int height); +int CameraInit(int camera_id, double dest_fps); int CameraHandler(void); void cameraDeInit(void); diff --git a/main/main.cpp b/main/main.cpp index a3e5425..4077769 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -33,7 +33,7 @@ int main(void) 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; - int frame_delay = CameraInit(0, dest_fps, 320, 240); + int frame_delay = CameraInit(0, dest_fps); if (frame_delay < 0) { std::cerr << "CameraInit failed" << std::endl; return -1; diff --git a/src/camera.cpp b/src/camera.cpp index 354ac42..d7e6fe9 100644 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -7,61 +7,81 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +// ═══════════════════════════════════════════════════════════ +// 模型检测类别 +// ═══════════════════════════════════════════════════════════ +enum { MD_CONE = 0, MD_RED = 1, MD_GREEN = 2, MD_ZEBRA = 3 }; + +// ═══════════════════════════════════════════════════════════ +// 摄像头 & 帧缓冲全局 +// ═══════════════════════════════════════════════════════════ cv::VideoCapture cap; -cv::Mat raw_mat; -cv::Mat decoded_frame; // 1/4 解码输出复用 buffer -static int g_decode_mode = -1; // -1=未检测 0=全BGR回退 1=原始JPEG缩放解码 +cv::Mat raw_mat, decoded_frame; +static int g_decode_mode = -1; int screenWidth, screenHeight, newWidth, newHeight; int fb; uint16_t *fb_buffer; PwmController servo(1, 0); -#define calc_scale 2 +static constexpr int calc_scale = 2; -// ── 模型检测参数 ── -#define ZEBRA_CLASS 3 -#define CONE_CLASS 0 +// ═══════════════════════════════════════════════════════════ +// 模型相关静态 +// ═══════════════════════════════════════════════════════════ static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.90f}; +static DetectBoxV10 g_boxes[16]; +static int g_box_count = 0; -// ── 斑马线去抖 ── -#define ZEBRA_MIN_FRAMES 5 -#define ZEBRA_FAR_CY 50 +// ═══════════════════════════════════════════════════════════ +// 斑马线状态机 +// ═══════════════════════════════════════════════════════════ +static constexpr int ZEBRA_MIN_FRAMES = 5; +static constexpr int ZEBRA_FAR_CY = 50; static int g_zc_frames = 0; static int g_zc_min_cy = 120; -// ── 斑马线状态机 ── enum ZState { Z_NORMAL, Z_STOP, Z_COOLDOWN }; 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 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_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; -static bool g_lcd_on = true; - +// ═══════════════════════════════════════════════════════════ +// LCD & FPS +// ═══════════════════════════════════════════════════════════ +static bool g_lcd_on = true; double g_steer_deviation = 0; -// ── I2C 音频 ── + +// ═══════════════════════════════════════════════════════════ +// I2C 音频 (斑马线语音播报) +// ═══════════════════════════════════════════════════════════ static int i2c_audio_fd = -1; static bool i2c_audio_open() @@ -80,7 +100,31 @@ static bool i2c_audio_open() return true; } -int CameraInit(uint8_t camera_id, double dest_fps, int width, int height) +static void play_zebra_audio() +{ + if (!i2c_audio_open()) { printf("[ZEBRA] 语音失败: I2C 未打开\n"); return; } + ioctl(i2c_audio_fd, I2C_SLAVE, 0x34); + + union i2c_smbus_data d; + struct i2c_smbus_ioctl_data a; + __u8 v[] = {0xFF, 0x10}; + d.block[0] = 2; d.block[1] = v[0]; d.block[2] = v[1]; + a.read_write = I2C_SMBUS_WRITE; + a.command = 0x6E; + a.size = I2C_SMBUS_I2C_BLOCK_DATA; + a.data = &d; + + if (ioctl(i2c_audio_fd, I2C_SMBUS, &a) < 0) + printf("[ZEBRA] 语音失败: %s\n", strerror(errno)); + else + printf("[ZEBRA] 语音播报已触发\n"); +} + + +// ═══════════════════════════════════════════════════════════ +// CameraInit / cameraDeInit +// ═══════════════════════════════════════════════════════════ +int CameraInit(int camera_id, double dest_fps) { servo.setPeriod(3000000); servo.setDutyCycle(1500000); @@ -103,40 +147,39 @@ int CameraInit(uint8_t camera_id, double dest_fps, int width, int height) cap.open(0, cv::CAP_V4L2); if (!cap.isOpened()) cap.open(0); cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); - cap.set(cv::CAP_PROP_FRAME_WIDTH, 640); + cap.set(cv::CAP_PROP_FRAME_WIDTH, 640); cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480); - cap.set(cv::CAP_PROP_CONVERT_RGB, 0); // 后端直接返回原始 MJPEG 字节流, 由我们做 1/4 解码 + cap.set(cv::CAP_PROP_CONVERT_RGB, 0); if (!cap.isOpened()) { printf("无法打开摄像头\n"); munmap(fb_buffer, fb_size); close(fb); return -1; } - int cameraWidth = cap.get(cv::CAP_PROP_FRAME_WIDTH); - int cameraHeight = cap.get(cv::CAP_PROP_FRAME_HEIGHT); - printf("摄像头分辨率: %d x %d\n", cameraWidth, cameraHeight); + int camW = cap.get(cv::CAP_PROP_FRAME_WIDTH); + int camH = cap.get(cv::CAP_PROP_FRAME_HEIGHT); + printf("摄像头分辨率: %d x %d\n", camW, camH); - double widthRatio = static_cast(screenWidth) / cameraWidth; - double heightRatio = static_cast(screenHeight) / cameraHeight; - double scale = std::min(widthRatio, heightRatio); - newWidth = static_cast(cameraWidth * scale); - newHeight = static_cast(cameraHeight * scale); + double ws = (double)screenWidth / camW; + double hs = (double)screenHeight / camH; + double s = std::min(ws, hs); + newWidth = (int)(camW * s); + newHeight = (int)(camH * s); printf("自适应分辨率: %d x %d\n", newWidth, newHeight); double fps = cap.get(cv::CAP_PROP_FPS); - printf("Camera fps:%lf\n", fps); + printf("Camera fps: %f\n", fps); - line_tracking_width = newWidth / calc_scale; + line_tracking_width = newWidth / calc_scale; line_tracking_height = newHeight / calc_scale; - if (!model_v10_init("./mild_v12.bin")) { + if (!model_v10_init("./mild_v12.bin")) printf("[MODEL] 警告: 模型加载失败\n"); - } else { + else printf("[MODEL] Mild v12 模型已加载\n"); - } i2c_audio_open(); - return static_cast(1000.0 / std::min(fps, dest_fps)); + return (int)(1000.0 / std::min(fps, dest_fps)); } void cameraDeInit(void) @@ -152,59 +195,39 @@ void cameraDeInit(void) model_v10_deinit(); } -int saved_frame_count = 0; -static bool saveCameraImage(cv::Mat frame, const std::string &directory) + +// ═══════════════════════════════════════════════════════════ +// 图像保存 (debug 模式用) +// ═══════════════════════════════════════════════════════════ +static int saved_frame_count = 0; + +static void save_image_if_requested() { - if (frame.empty()) return false; + if (!g_cfg.debug) return; + if (!readFlag(saveImg_file)) return; std::ostringstream filename; - filename << directory << "/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg"; - saved_frame_count++; - return cv::imwrite(filename.str(), frame); -} - -static void play_zebra_audio() -{ - if (!i2c_audio_open()) { - printf("[ZEBRA] 语音失败: I2C 未打开\n"); - return; + filename << "./image/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg"; + if (cv::imwrite(filename.str(), raw_frame)) { + printf("图像%d已保存\n", saved_frame_count); + saved_frame_count++; } - ioctl(i2c_audio_fd, I2C_SLAVE, 0x34); - - union i2c_smbus_data d; - struct i2c_smbus_ioctl_data a; - __u8 v[] = {0xFF, 0x10}; - d.block[0] = 2; d.block[1] = v[0]; d.block[2] = v[1]; - a.read_write = I2C_SMBUS_WRITE; - a.command = 0x6E; - a.size = I2C_SMBUS_I2C_BLOCK_DATA; - a.data = &d; - - if (ioctl(i2c_audio_fd, I2C_SMBUS, &a) < 0) - printf("[ZEBRA] 语音失败: %s\n", strerror(errno)); - else - printf("[ZEBRA] 语音播报已触发\n"); } -// ── LCD Mats 预分配 (避免每帧 new/delete) ── -static cv::Mat lcd_fbImage; -static cv::Mat lcd_resized; -static cv::Mat lcd_colored; -int CameraHandler(void) +// ═══════════════════════════════════════════════════════════ +// 帧捕获 + 解码 +// ═══════════════════════════════════════════════════════════ +static int capture_frame(struct timespec *t0, struct timespec *t1) { - struct timespec t0,t1,t2,t3,t4,t5; - - // ── 1. 取帧 ── - clock_gettime(CLOCK_MONOTONIC,&t0); + clock_gettime(CLOCK_MONOTONIC, t0); cap.read(raw_mat); - clock_gettime(CLOCK_MONOTONIC,&t1); + clock_gettime(CLOCK_MONOTONIC, t1); if (raw_mat.empty()) return -1; - // CONVERT_RGB=0 检测: 单通道=原始JPEG字节流 → 1/4解码; 三通道=后端已解码 → 回退 if (g_decode_mode == -1) { g_decode_mode = (raw_mat.channels() == 1) ? 1 : 0; printf("[CAM] CONVERT_RGB=0 %s, mode=%d\n", - g_decode_mode == 1 ? "生效->1/4解码160x120" : "未生效->全解码回退640x480", + g_decode_mode == 1 ? "生效->1/4解码" : "未生效->全解码回退", g_decode_mode); } @@ -215,53 +238,46 @@ int CameraHandler(void) } else { raw_frame = raw_mat; } + return 0; +} - // ── 2. 保存图像 ── - if (g_cfg.debug) { - if (readFlag(saveImg_file)) { - if (saveCameraImage(raw_frame, "./image")) - printf("图像%d已保存\n", saved_frame_count); - } - } - - // ── 3. 视觉巡线 ── - image_main(); - clock_gettime(CLOCK_MONOTONIC,&t2); - - // ── 4. 模型推理 (每2帧一次, 640×480直入) ── +// ═══════════════════════════════════════════════════════════ +// 模型推理 (每 2 帧一次) +// ═══════════════════════════════════════════════════════════ +static void run_model_inference() +{ static int infer_skip = 0; - if (++infer_skip >= 2) { - infer_skip = 0; - g_box_count = 0; - if (model_v10_ready()) { - g_box_count = model_v10_detect(raw_frame.data, raw_frame.cols, raw_frame.rows, - g_boxes, 16, g_thresh); - } + if (++infer_skip < 2) return; + infer_skip = 0; + g_box_count = 0; + if (model_v10_ready()) { + g_box_count = model_v10_detect(raw_frame.data, raw_frame.cols, raw_frame.rows, + g_boxes, 16, g_thresh); } - clock_gettime(CLOCK_MONOTONIC,&t3); +} - // ── 5. 斑马线去抖+状态机 ── - bool zebra_near = false; + +// ═══════════════════════════════════════════════════════════ +// 斑马线检测 + 状态机 +// ═══════════════════════════════════════════════════════════ +static bool zebra_process() +{ bool zebra_seen = false; + bool zebra_near = false; int zebra_cy = 0; float zebra_cf = 0; - g_zebra_ever = false; for (int i = 0; i < g_box_count; ++i) { - if (g_boxes[i].cls == ZEBRA_CLASS) { - g_zebra_ever = true; - zebra_cy = (int)g_boxes[i].cy; - zebra_cf = g_boxes[i].conf; - if (g_zstate == Z_NORMAL) { - if (zebra_cy < g_zc_min_cy) g_zc_min_cy = zebra_cy; - g_zc_frames++; - } - zebra_seen = true; - - if (zebra_cy > g_cfg.zebrasee) - zebra_near = true; - break; + if (g_boxes[i].cls != MD_ZEBRA) continue; + zebra_cy = (int)g_boxes[i].cy; + zebra_cf = g_boxes[i].conf; + zebra_seen = true; + zebra_near = (zebra_cy > g_cfg.zebrasee); + if (g_zstate == Z_NORMAL) { + if (zebra_cy < g_zc_min_cy) g_zc_min_cy = zebra_cy; + g_zc_frames++; } + break; } if (!zebra_seen) { @@ -272,19 +288,11 @@ int CameraHandler(void) time_t now = time(nullptr); switch (g_zstate) { case Z_NORMAL: - if (zebra_near) { - bool enough = (g_zc_frames >= ZEBRA_MIN_FRAMES); - bool from_far = (g_zc_min_cy <= ZEBRA_FAR_CY); - if (enough && from_far) { - play_zebra_audio(); - g_zstate = Z_STOP; g_ztime = now; - if (g_cfg.debug) - printf("[ZEBRA] cy=%d cf=%.2f 停车4s 冷却5s\n", zebra_cy, zebra_cf); - g_zc_frames = 0; g_zc_min_cy = 120; - } else if (g_cfg.debug) { - printf("[ZEBRA] cy=%d 拒绝 f=%d/%d mc=%d/%d\n", - zebra_cy, g_zc_frames, ZEBRA_MIN_FRAMES, g_zc_min_cy, ZEBRA_FAR_CY); - } + if (zebra_near && g_zc_frames >= ZEBRA_MIN_FRAMES && g_zc_min_cy <= ZEBRA_FAR_CY) { + play_zebra_audio(); + g_zstate = Z_STOP; g_ztime = now; + if (g_cfg.debug) printf("[ZEBRA] cy=%d cf=%.2f 停车4s 冷却5s\n", zebra_cy, zebra_cf); + g_zc_frames = 0; g_zc_min_cy = 120; } break; case Z_STOP: @@ -300,17 +308,22 @@ int CameraHandler(void) } break; } + return (g_zstate == Z_STOP); +} - // ── 5.5 红绿灯状态机 ── - bool red_seen = false; - bool green_seen = false; +// ═══════════════════════════════════════════════════════════ +// 红绿灯状态机 +// ═══════════════════════════════════════════════════════════ +static bool traffic_light_process() +{ + bool red_seen = false, 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 (g_boxes[i].cls == MD_RED) red_seen = true; + if (g_boxes[i].cls == MD_GREEN) green_seen = true; } if (red_seen) g_tl_red_frames++; - else g_tl_red_frames = std::max(0, g_tl_red_frames - 1); + 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); @@ -330,63 +343,67 @@ int CameraHandler(void) 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; + g_tl_red_frames = g_tl_green_frames = 0; if (g_cfg.debug) printf("[TL] 绿灯通行\n"); } break; } + return (g_tl_state != TL_NORMAL); +} - bool tl_block = (g_tl_state != TL_NORMAL); +// ═══════════════════════════════════════════════════════════ +// 锥桶检测 + 中线变形 +// ═══════════════════════════════════════════════════════════ +static bool cone_is_slow() { + return g_cone_confirmed || + (g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames); +} - // ── 5.6 锥桶检测 + 中线变形 ── +static void cone_detect_and_deform() +{ + if (g_zstate == Z_STOP || g_tl_state != TL_NORMAL) return; + + // ── 找最近锥桶 ── bool cone_seen = false; - int cone_row_keep = -1; - int cone_col_keep = -1; + int cone_row = -1, cone_col = -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; + for (int i = 0; i < g_box_count; ++i) { + if (g_boxes[i].cls != MD_CONE) 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; + int cl = g_boxes[i].cx * line_tracking_width / raw_frame.cols; + int rl = 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 (rl < 10 || rl >= line_tracking_height) continue; + if (left_line[rl] == -1 || right_line[rl] == -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; + 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[rl] || bl > right_line[rl]) continue; + } else { + if (cl < left_line[rl] || cl > right_line[rl]) continue; } + + if (rl > cone_row) { cone_row = rl; cone_col = cl; } + 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_last_row = cone_row; g_cone_last_col = cone_col; g_cone_frames = 1; + } else if (std::abs(cone_row - g_cone_last_row) <= row_tol && + std::abs(cone_col - g_cone_last_col) <= col_tol) { + g_cone_frames++; + g_cone_last_row = cone_row; g_cone_last_col = cone_col; } 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; - } + g_cone_frames = 1; + g_cone_last_row = cone_row; g_cone_last_col = cone_col; } } else { if (g_cone_frames > 0) g_cone_frames = std::max(0, g_cone_frames - 1); @@ -395,143 +412,206 @@ int CameraHandler(void) 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; + g_cone_hold_src_row = cone_row; + g_cone_hold_src_col = cone_col; } 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_is_slow()) return; + if (g_zstate == Z_STOP || g_tl_state != TL_NORMAL) return; - 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); + // ── 中线变形 ── + int src_row = g_cone_confirmed ? cone_row : g_cone_hold_src_row; + int src_col = g_cone_confirmed ? cone_col : g_cone_hold_src_col; + if (src_row <= 10) return; - 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); - } - } + double total_rows = src_row - 10; + double rng = (double)g_cfg.cone_avoid_range; + double half_w = line_tracking_width / 2.0; + double ms = mid_line[src_row]; + 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); } +} - // ── 6. 舵机 ── - if (g_cfg.start) { - int foresee = (int)g_cfg.foresee; - int check_row = foresee / calc_scale; - if (check_row >= 0 && check_row < line_tracking_height && mid_line[check_row] != -1) { - double deviation = mid_line[check_row] * calc_scale - newWidth / 2; - deviation -= g_cfg.center_bias; - g_steer_deviation = deviation / (newWidth / 2.0); - if (std::abs(deviation) < g_cfg.deadband) { - servo.setDutyCycle(1500000); - } else { - double norm = deviation / (newWidth / 2.0); - double offset = norm * g_cfg.steer_gain * 300000; - double duty_ns = 1500000.0 + offset; - duty_ns = std::clamp(duty_ns, 1200000.0, 1800000.0); - servo.setDutyCycle(static_cast(duty_ns)); - } - } + +// ═══════════════════════════════════════════════════════════ +// 舵机控制 +// ═══════════════════════════════════════════════════════════ +static void steering_update() +{ + if (!g_cfg.start) return; + int check_row = (int)g_cfg.foresee / calc_scale; + if (check_row < 0 || check_row >= line_tracking_height) return; + if (mid_line[check_row] == -1) return; + + double deviation = mid_line[check_row] * calc_scale - newWidth / 2; + deviation -= g_cfg.center_bias; + g_steer_deviation = deviation / (newWidth / 2.0); + + if (std::abs(deviation) < g_cfg.deadband) { + servo.setDutyCycle(1500000); + } else { + double offset = deviation / (newWidth / 2.0) * g_cfg.steer_gain * 300000; + double duty_ns = std::clamp(1500000.0 + offset, 1200000.0, 1800000.0); + servo.setDutyCycle((unsigned int)duty_ns); } +} - // ── 7. 电机控制 ── - { - 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); +// ═══════════════════════════════════════════════════════════ +// 电机控制 +// ═══════════════════════════════════════════════════════════ +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; + ControlUpdate(spd, zebra_block || tl_block); +} - // ── 8. LCD ── + +// ═══════════════════════════════════════════════════════════ +// LCD 渲染 +// ═══════════════════════════════════════════════════════════ +static cv::Mat lcd_fbImage, lcd_resized, lcd_colored; + +static void lcd_render() +{ static int lcd_check = 0; - if (--lcd_check < 0) { - g_lcd_on = g_cfg.showImg; - lcd_check = 10; + if (--lcd_check < 0) { g_lcd_on = g_cfg.showImg; lcd_check = 10; } + if (!g_lcd_on) return; + + lcd_fbImage.create(screenHeight, screenWidth, CV_8UC3); + lcd_fbImage.setTo(cv::Scalar(0, 0, 0)); + cv::resize(track, lcd_resized, cv::Size(newWidth, newHeight)); + cv::cvtColor(lcd_resized, lcd_colored, cv::COLOR_GRAY2BGR); + + cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight); + lcd_colored.copyTo(lcd_fbImage(roi)); + + // 边界线 + for (int y = 0; y < line_tracking_height; ++y) { + int lx = (int)(left_line[y] * calc_scale), ly = y * calc_scale; + int rx = (int)(right_line[y] * calc_scale); + int mx = (int)(mid_line[y] * calc_scale); + cv::line(lcd_fbImage(roi), cv::Point(lx, ly), cv::Point(lx, ly), cv::Scalar(0, 0, 255), calc_scale); + cv::line(lcd_fbImage(roi), cv::Point(rx, ly), cv::Point(rx, ly), cv::Scalar(0, 255, 0), calc_scale); + cv::line(lcd_fbImage(roi), cv::Point(mx, ly), cv::Point(mx, ly), cv::Scalar(255, 0, 0), calc_scale); } - if (g_lcd_on) { - lcd_fbImage.create(screenHeight, screenWidth, CV_8UC3); - lcd_fbImage.setTo(cv::Scalar(0, 0, 0)); - cv::resize(track, lcd_resized, cv::Size(newWidth, newHeight)); - cv::cvtColor(lcd_resized, lcd_colored, cv::COLOR_GRAY2BGR); + // 检测框 (跳过红绿灯，不画框) + float bx = (float)newWidth / raw_frame.cols, by = (float)newHeight / raw_frame.rows; + for (int i = 0; i < g_box_count; ++i) { + if (g_boxes[i].cls == MD_RED || g_boxes[i].cls == MD_GREEN) continue; + int x1 = (int)((g_boxes[i].cx - g_boxes[i].w/2) * bx); + int y1 = (int)((g_boxes[i].cy - g_boxes[i].h/2) * by); + int x2 = (int)((g_boxes[i].cx + g_boxes[i].w/2) * bx); + int y2 = (int)((g_boxes[i].cy + g_boxes[i].h/2) * by); + x1 = std::max(0, std::min(newWidth - 1, x1)); + y1 = std::max(0, std::min(newHeight - 1, y1)); + x2 = std::max(0, std::min(newWidth - 1, x2)); + y2 = std::max(0, std::min(newHeight - 1, y2)); - cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight); - lcd_colored.copyTo(lcd_fbImage(roi)); + cv::Scalar color(0, 255, 0); + if (g_boxes[i].cls == MD_ZEBRA) color = cv::Scalar(255, 0, 255); + else if (g_boxes[i].cls == MD_CONE) color = cv::Scalar(0, 165, 255); + cv::rectangle(lcd_fbImage(roi), cv::Point(x1, y1), cv::Point(x2, y2), color, 2); - for (int y = 0; y < line_tracking_height; y++) { - int sLX=static_cast(left_line[y]*calc_scale), sRX=static_cast(right_line[y]*calc_scale); - int sMX=static_cast(mid_line[y]*calc_scale), sY=static_cast(y*calc_scale); - cv::line(lcd_fbImage(roi), cv::Point(sLX,sY), cv::Point(sLX,sY), cv::Scalar(0,0,255), calc_scale); - cv::line(lcd_fbImage(roi), cv::Point(sRX,sY), cv::Point(sRX,sY), cv::Scalar(0,255,0), calc_scale); - cv::line(lcd_fbImage(roi), cv::Point(sMX,sY), cv::Point(sMX,sY), cv::Scalar(255,0,0), calc_scale); - } - - float bx=(float)newWidth/160.0f, by=(float)newHeight/120.0f; - for (int i = 0; i < g_box_count; ++i) { - if (g_boxes[i].cls == 1 || g_boxes[i].cls == 2) continue; - int x1=(int)((g_boxes[i].cx-g_boxes[i].w/2)*bx), y1=(int)((g_boxes[i].cy-g_boxes[i].h/2)*by); - int x2=(int)((g_boxes[i].cx+g_boxes[i].w/2)*bx), y2=(int)((g_boxes[i].cy+g_boxes[i].h/2)*by); - x1=std::max(0,std::min(newWidth-1,x1)); y1=std::max(0,std::min(newHeight-1,y1)); - 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); - } - - 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); + char lab[16]; 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); } - // ── 9. FPS + 分步计时 ── - { - clock_gettime(CLOCK_MONOTONIC,&t5); - static int fc=0; static timespec tf0; if(fc==0) clock_gettime(CLOCK_MONOTONIC,&tf0); - fc++; - if(fc%15==0){ timespec tf1; clock_gettime(CLOCK_MONOTONIC,&tf1); - double dt=(tf1.tv_sec-tf0.tv_sec)+(tf1.tv_nsec-tf0.tv_nsec)*1e-9; - double ms_r =(t1.tv_sec-t0.tv_sec)*1000.0+(t1.tv_nsec-t0.tv_nsec)*1e-6; - double ms_vis=(t2.tv_sec-t1.tv_sec)*1000.0+(t2.tv_nsec-t1.tv_nsec)*1e-6; - double ms_mdl=(t3.tv_sec-t2.tv_sec)*1000.0+(t3.tv_nsec-t2.tv_nsec)*1e-6; - double ms_ctl=(t4.tv_sec-t3.tv_sec)*1000.0+(t4.tv_nsec-t3.tv_nsec)*1e-6; - printf("fps=%.1f|rd=%.0f vi=%.0f md=%.0f ct=%.0f ms\r", - fc/dt, ms_r, ms_vis, ms_mdl, ms_ctl); - fflush(stdout); - } - } + // 状态指示 + char ztxt[8]; int zi = 0; + 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'; + else if (g_zstate == Z_COOLDOWN) ztxt[zi++] = 'C'; + else ztxt[zi++] = 'N'; + ztxt[zi] = 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); +} + +// ═══════════════════════════════════════════════════════════ +// FPS 统计 +// ═══════════════════════════════════════════════════════════ +static void fps_log(struct timespec *ts) +{ + static int fc = 0; + static struct timespec tf0; + if (fc == 0) clock_gettime(CLOCK_MONOTONIC, &tf0); + fc++; + if (fc % 15 != 0) return; + + struct timespec tf1; + clock_gettime(CLOCK_MONOTONIC, &tf1); + + double dt = (tf1.tv_sec - tf0.tv_sec) + (tf1.tv_nsec - tf0.tv_nsec) * 1e-9; + double ms_rd = (ts[1].tv_sec - ts[0].tv_sec) * 1000.0 + (ts[1].tv_nsec - ts[0].tv_nsec) * 1e-6; + double ms_vis = (ts[2].tv_sec - ts[1].tv_sec) * 1000.0 + (ts[2].tv_nsec - ts[1].tv_nsec) * 1e-6; + double ms_mdl = (ts[3].tv_sec - ts[2].tv_sec) * 1000.0 + (ts[3].tv_nsec - ts[2].tv_nsec) * 1e-6; + double ms_ctl = (ts[4].tv_sec - ts[3].tv_sec) * 1000.0 + (ts[4].tv_nsec - ts[3].tv_nsec) * 1e-6; + + printf("fps=%.1f | rd=%.0f vi=%.0f md=%.0f ct=%.0f ms\r", + fc / dt, ms_rd, ms_vis, ms_mdl, ms_ctl); + fflush(stdout); +} + + +// ═══════════════════════════════════════════════════════════ +// CameraHandler — 每帧主流程 +// ═══════════════════════════════════════════════════════════ +int CameraHandler(void) +{ + struct timespec ts[5]; // [0]=capture_end, [1]=vision_end, [2]=model_end, [3]=control_end, [4]=fps + + // 1. 取帧 + 解码 + if (capture_frame(&ts[0], &ts[1]) < 0) return -1; + + // 2. 保存图像 (debug) + save_image_if_requested(); + + // 3. 视觉巡线 + image_main(); + clock_gettime(CLOCK_MONOTONIC, &ts[2]); + + // 4. 模型推理 + run_model_inference(); + clock_gettime(CLOCK_MONOTONIC, &ts[3]); + + // 5. 场景识别 + bool zebra_block = zebra_process(); + bool tl_block = traffic_light_process(); + cone_detect_and_deform(); + + // 6. 舵机 + steering_update(); + + // 7. 电机 + motor_update(zebra_block, tl_block); + clock_gettime(CLOCK_MONOTONIC, &ts[4]); + + // 8. LCD + lcd_render(); + + // 9. FPS + fps_log(ts); + return 0; }