spdis/Loongson_2k0300_SmartCar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
d251f086707c5162d0f904184b02db79760c1f51
Loongson_2k0300_SmartCar/src/camera.cpp
T

538 lines
20 KiB
C++
Raw Blame History

#include "camera.h"
#include "model_v10.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
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缩放解码
int screenWidth, screenHeight, newWidth, newHeight;
int fb;
uint16_t *fb_buffer;
PwmController servo(1, 0);
#define calc_scale 2
// ── 模型检测参数 ──
#define ZEBRA_CLASS 3
#define CONE_CLASS 0
static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.90f};
// ── 斑马线去抖 ──
#define ZEBRA_MIN_FRAMES 5
#define 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 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;
static bool g_lcd_on = true;
double g_steer_deviation = 0;
// ── I2C 音频 ──
static int i2c_audio_fd = -1;
static bool i2c_audio_open()
{
if (i2c_audio_fd >= 0) return true;
i2c_audio_fd = open("/dev/i2c-2", O_RDWR);
if (i2c_audio_fd < 0) {
fprintf(stderr, "[ZEBRA] 无法打开 I2C-2: %s\n", strerror(errno));
return false;
}
if (ioctl(i2c_audio_fd, I2C_SLAVE, 0x34) < 0) {
fprintf(stderr, "[ZEBRA] 无法设置 I2C 从地址 0x34: %s\n", strerror(errno));
close(i2c_audio_fd); i2c_audio_fd = -1;
return false;
}
return true;
}
int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
{
servo.setPeriod(3000000);
servo.setDutyCycle(1500000);
servo.enable();
fb = open("/dev/fb0", O_RDWR);
if (fb == -1) { std::cerr << "无法打开帧缓冲区设备" << std::endl; return -1; }
struct fb_var_screeninfo vinfo;
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1) {
std::cerr << "无法获取帧缓冲区信息" << std::endl; close(fb); return -1;
}
screenWidth = vinfo.xres; screenHeight = vinfo.yres;
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
fb_buffer = (uint16_t *)mmap(NULL, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0);
if (fb_buffer == MAP_FAILED) {
std::cerr << "无法映射帧缓冲区到内存" << std::endl; close(fb); return -1;
}
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_HEIGHT, 480);
cap.set(cv::CAP_PROP_CONVERT_RGB, 0); // 后端直接返回原始 MJPEG 字节流, 由我们做 1/4 解码
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);
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);
printf("自适应分辨率: %d x %d\n", newWidth, newHeight);
double fps = cap.get(cv::CAP_PROP_FPS);
printf("Camera fps:%lf\n", fps);
line_tracking_width = newWidth / calc_scale;
line_tracking_height = newHeight / calc_scale;
if (!model_v10_init("./mild_v12.bin")) {
printf("[MODEL] 警告: 模型加载失败\n");
} else {
printf("[MODEL] Mild v12 模型已加载\n");
}
i2c_audio_open();
return static_cast<int>(1000.0 / std::min(fps, dest_fps));
}
void cameraDeInit(void)
{
cap.release();
struct fb_var_screeninfo vinfo;
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) != -1) {
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
munmap(fb_buffer, fb_size);
}
close(fb);
if (i2c_audio_fd >= 0) close(i2c_audio_fd);
model_v10_deinit();
}
int saved_frame_count = 0;
static bool saveCameraImage(cv::Mat frame, const std::string &directory)
{
if (frame.empty()) return false;
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;
}
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)
{
struct timespec t0,t1,t2,t3,t4,t5;
// ── 1. 取帧 ──
clock_gettime(CLOCK_MONOTONIC,&t0);
cap.read(raw_mat);
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);
}
if (g_decode_mode == 1) {
cv::imdecode(raw_mat, cv::IMREAD_REDUCED_COLOR_4, &decoded_frame);
if (decoded_frame.empty()) return -1;
raw_frame = decoded_frame;
} else {
raw_frame = raw_mat;
}
// ── 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直入) ──
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);
}
}
clock_gettime(CLOCK_MONOTONIC,&t3);
// ── 5. 斑马线去抖+状态机 ──
bool zebra_near = false;
bool zebra_seen = 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 (!zebra_seen) {
if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 2);
if (g_zc_frames == 0) g_zc_min_cy = 120;
}
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);
}
}
break;
case Z_STOP:
if (now - g_ztime >= 4) {
g_zstate = Z_COOLDOWN; g_ztime = now;
if (g_cfg.debug) printf("[ZEBRA] 起步\n");
}
break;
case Z_COOLDOWN:
if (now - g_ztime >= 5) {
g_zstate = Z_NORMAL;
if (g_cfg.debug) printf("[ZEBRA] 恢复\n");
}
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;
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));
}
}
}
// ── 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);
// ── 8. LCD ──
static int lcd_check = 0;
if (--lcd_check < 0) {
g_lcd_on = g_cfg.showImg;
lcd_check = 10;
}
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);
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 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);
}
// ── 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);
}
}
return 0;
}
Reference in New Issue View Git Blame Copy Permalink