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

代码优雅化重构: CameraHandler拆9个函数+删serial死代码+常量统一+死亡参数清理+camera.h精简

Browse Source
This commit is contained in:
spdis
2026-06-13 14:07:35 +08:00
parent d251f08670
commit 54a9915bf5
5 changed files with 381 additions and 318 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+2 -1
View File
@@ -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})
ctl.sh
-6
View File
@@ -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
lib/camera.h
+2 -14
View File
@@ -1,27 +1,15 @@
#ifndef CAMERA_H_
#define CAMERA_H_
#include <cstdint>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <mutex>
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <linux/fb.h>
#include <algorithm>
#include <chrono>
#include <thread>
#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);
main/main.cpp
+1 -1
View File
@@ -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;
src/camera.cpp
+376 -296
View File
@@ -7,61 +7,81 @@
#include <sys/mman.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
#include <linux/fb.h>
#include <algorithm>
#include <chrono>
#include <thread>
#include <cstring>
#include <sstream>
#include <iomanip>
// ═══════════════════════════════════════════════════════════
// 模型检测类别
// ═══════════════════════════════════════════════════════════
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<double>(screenWidth) / cameraWidth;
double heightRatio = static_cast<double>(screenHeight) / cameraHeight;
double scale = std::min(widthRatio, heightRatio);
newWidth = static_cast<int>(cameraWidth * scale);
newHeight = static_cast<int>(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<int>(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<unsigned int>(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<int>(left_line[y]*calc_scale), sRX=static_cast<int>(right_line[y]*calc_scale);
int sMX=static_cast<int>(mid_line[y]*calc_scale), sY=static_cast<int>(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;
}