v10模型集成+去抖+架构重构: 单线程+背景采集, ControlUpdate单出口电机控制, I2C音频持久fd, 斑马线接近去抖
This commit is contained in:
spdis
+267
-175
@@ -1,284 +1,376 @@
|
||||
#include "camera.h"
|
||||
#include "model_v10.hpp"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/i2c-dev.h>
|
||||
#include <linux/i2c.h>
|
||||
|
||||
cv::VideoCapture cap;
|
||||
|
||||
double kp = 0;
|
||||
double ki = 0;
|
||||
double kd = 0;
|
||||
|
||||
int screenWidth, screenHeight;
|
||||
int newWidth, newHeight;
|
||||
double kp = 0, ki = 0, kd = 0;
|
||||
int screenWidth, screenHeight, newWidth, newHeight;
|
||||
int fb;
|
||||
// 创建帧缓冲区
|
||||
uint16_t *fb_buffer;
|
||||
PwmController servo(1, 0);
|
||||
|
||||
#define calc_scale 2
|
||||
|
||||
// ── 模型检测参数 ──
|
||||
#define ZEBRA_CLASS 3
|
||||
static float g_thresh[4] = {0.80f, 0.80f, 0.80f, 0.75f};
|
||||
|
||||
// ── 斑马线去抖: 远处→近处接近逻辑, 防反光误触发 ──
|
||||
#define ZEBRA_MIN_FRAMES 5 // 累计检测至少5帧
|
||||
#define ZEBRA_FAR_CY 50 // 必须在cy≤50处出现过(远处)
|
||||
|
||||
static int g_zc_frames = 0; // 当前接近episode中检测帧数
|
||||
static int g_zc_min_cy = 120; // 当前episode中最小cy(最远)
|
||||
|
||||
// ── 斑马线状态机 ──
|
||||
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;
|
||||
|
||||
// ── 模型检测结果缓存 ──
|
||||
static DetectBoxV10 g_boxes[16];
|
||||
static int g_box_count = 0;
|
||||
static bool g_lcd_on = true;
|
||||
|
||||
double g_steer_deviation = 0;
|
||||
PIDController ServoControl(1.0, 0.0, 2.0, 0.0, POSITION, 1250000);
|
||||
|
||||
// ── 背景采集线程 (只做 cap.read, 不参与控制) ──
|
||||
static std::mutex frameMutex;
|
||||
static cv::Mat pubframe;
|
||||
static bool captureRunning;
|
||||
static std::thread captureWorker;
|
||||
|
||||
void streamCapture(void)
|
||||
{
|
||||
cv::Mat tmp;
|
||||
while (captureRunning) {
|
||||
cap.read(tmp);
|
||||
frameMutex.lock();
|
||||
pubframe = tmp;
|
||||
frameMutex.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
// ── 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;
|
||||
}
|
||||
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;
|
||||
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1) {
|
||||
std::cerr << "无法获取帧缓冲区信息" << std::endl; close(fb); return -1;
|
||||
}
|
||||
|
||||
// 动态设置屏幕分辨率
|
||||
screenWidth = vinfo.xres;
|
||||
screenHeight = vinfo.yres;
|
||||
|
||||
// 计算帧缓冲区大小
|
||||
screenWidth = vinfo.xres; screenHeight = vinfo.yres;
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
|
||||
|
||||
// 使用 mmap 映射帧缓冲区到内存
|
||||
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;
|
||||
if (fb_buffer == MAP_FAILED) {
|
||||
std::cerr << "无法映射帧缓冲区到内存" << std::endl; close(fb); return -1;
|
||||
}
|
||||
|
||||
// 打开默认摄像头（设备编号 0）
|
||||
cap.open(0, cv::CAP_V4L2);
|
||||
if (!cap.isOpened()) cap.open(0);
|
||||
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, 320);
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, 240);
|
||||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
|
||||
// 检查摄像头是否成功打开
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
if (!cap.isOpened()) {
|
||||
printf("无法打开摄像头\n");
|
||||
munmap(fb_buffer, fb_size);
|
||||
close(fb);
|
||||
return -1;
|
||||
munmap(fb_buffer, fb_size); close(fb); return -1;
|
||||
}
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, width);
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, height);
|
||||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
cap.set(cv::CAP_PROP_AUTO_EXPOSURE, -1);
|
||||
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, width); // 宽度
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, height); // 高度
|
||||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); // 视频流格式
|
||||
cap.set(cv::CAP_PROP_AUTO_EXPOSURE, -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);
|
||||
|
||||
// 计算 newWidth 和 newHeight，确保图像适应屏幕
|
||||
double widthRatio = static_cast<double>(screenWidth) / cameraWidth;
|
||||
double heightRatio = static_cast<double>(screenHeight) / cameraHeight;
|
||||
double scale = std::min(widthRatio, heightRatio); // 选择较小的比例，确保图像不超出屏幕
|
||||
|
||||
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;
|
||||
|
||||
// 计算每帧的延迟时间（ms）
|
||||
if (!model_v10_init("./nanodetv10.bin")) {
|
||||
printf("[MODEL] 警告: 模型加载失败\n");
|
||||
} else {
|
||||
printf("[MODEL] v10 模型已加载\n");
|
||||
}
|
||||
|
||||
i2c_audio_open();
|
||||
|
||||
captureRunning = true;
|
||||
captureWorker = std::thread(streamCapture);
|
||||
|
||||
// 等待第一帧就绪
|
||||
for (int i = 0; i < 60 && pubframe.empty(); ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
if (pubframe.empty()) { printf("警告: 摄像头首帧超时\n"); }
|
||||
|
||||
return static_cast<int>(1000.0 / std::min(fps, dest_fps));
|
||||
}
|
||||
|
||||
void cameraDeInit(void)
|
||||
{
|
||||
captureRunning = false;
|
||||
cap.release();
|
||||
|
||||
// 获取帧缓冲区设备信息
|
||||
if (captureWorker.joinable()) captureWorker.join();
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
|
||||
{
|
||||
std::cerr << "无法获取帧缓冲区信息" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 计算帧缓冲区大小
|
||||
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;
|
||||
bool saveCameraImage(cv::Mat frame, const std::string &directory)
|
||||
static bool saveCameraImage(cv::Mat frame, const std::string &directory)
|
||||
{
|
||||
if (frame.empty())
|
||||
{
|
||||
std::cerr << "Save Error: Frame is empty." << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 构建文件名
|
||||
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);
|
||||
}
|
||||
|
||||
std::mutex frameMutex;
|
||||
cv::Mat pubframe;
|
||||
bool streamCaptureRunning;
|
||||
void streamCapture(void)
|
||||
static void play_zebra_audio()
|
||||
{
|
||||
cv::Mat frame;
|
||||
while (streamCaptureRunning)
|
||||
{
|
||||
cap.read(frame);
|
||||
frameMutex.lock();
|
||||
pubframe = frame;
|
||||
frameMutex.unlock();
|
||||
if (!i2c_audio_open()) {
|
||||
printf("[ZEBRA] 语音失败: I2C 未打开\n");
|
||||
return;
|
||||
}
|
||||
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");
|
||||
}
|
||||
|
||||
// ===================================================
|
||||
// PIDController ServoControl(P=1.0, I=0, D=2.0, target=0, 位置式, 输出限幅=1,250,000)
|
||||
// 输出单位: 百分之一脉宽周期 (÷100 × period_ns → ns)
|
||||
// 实际等效线性增益: Kp=1.0 起主导, I=0 无积分, D=2.0 微分量抑制过冲
|
||||
// ===================================================
|
||||
double g_steer_deviation = 0; // 全局偏差, 供速度控制用
|
||||
|
||||
PIDController ServoControl(1.0, 0.0, 2.0, 0.0, POSITION, 1250000);
|
||||
int CameraHandler(void)
|
||||
{
|
||||
cv::Mat resizedFrame;
|
||||
|
||||
// ── 1. 取最新帧 (背景线程持续采集, 写全局 raw_frame 供 image_main 使用) ──
|
||||
frameMutex.lock();
|
||||
raw_frame = pubframe;
|
||||
frameMutex.unlock();
|
||||
if (raw_frame.empty()) { return -1; }
|
||||
|
||||
if (raw_frame.empty())
|
||||
{
|
||||
printf("无法捕获图像\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (readFlag(saveImg_file))
|
||||
{
|
||||
// ── 2. 保存图像 ──
|
||||
if (readFlag(saveImg_file)) {
|
||||
if (saveCameraImage(raw_frame, "./image"))
|
||||
{
|
||||
printf("图像%d已保存\n", saved_frame_count);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("图像保存失败\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// ── 3. 视觉巡线 (禁止动) ──
|
||||
image_main();
|
||||
|
||||
// ── 4. 模型推理 (每2帧一次) ──
|
||||
static int infer_skip = 0;
|
||||
if (++infer_skip >= 2) {
|
||||
infer_skip = 0;
|
||||
g_box_count = 0;
|
||||
if (model_v10_ready()) {
|
||||
cv::Mat mInput;
|
||||
cv::resize(raw_frame, mInput, cv::Size(160, 120), 0, 0, cv::INTER_AREA);
|
||||
g_box_count = model_v10_detect(mInput.data, 160, 120, g_boxes, 16, g_thresh);
|
||||
}
|
||||
}
|
||||
|
||||
// ── 1. 视觉巡线 ──────────────────────────────────
|
||||
// image_main() 处理 raw_frame → 80×60 图
|
||||
// 产出: left_line[60], right_line[60], mid_line[60] (EMA 滤波后)
|
||||
{ // 图像计算
|
||||
image_main();
|
||||
}
|
||||
// ── 5. 斑马线去抖+停/走状态机 ──
|
||||
bool zebra_near = false;
|
||||
bool zebra_seen = false;
|
||||
int zebra_cy = 0;
|
||||
float zebra_cf = 0;
|
||||
g_zebra_ever = false;
|
||||
|
||||
// ── 2. 舵机转向控制 ──────────────────────────────
|
||||
// 仅在 start 文件为 1 时执行 (readFlag(start_file))
|
||||
// 否则保持上一次的脉宽 (不做任何转向)
|
||||
if (readFlag(start_file))
|
||||
{
|
||||
// 2a. 前瞻行号换算
|
||||
// foresee 是 newWidth×newHeight (160×120) 坐标系下的行号
|
||||
// calc_scale=2, 除以 2 得到 80×60 (line_tracking) 下的行号
|
||||
int foresee = readDoubleFromFile(foresee_file);
|
||||
int check_row = foresee / calc_scale;
|
||||
|
||||
// 2b. 单行偏差计算 (80×60 坐标系 → 160×120 像素偏差)
|
||||
if (check_row >= 0 && check_row < line_tracking_height && mid_line[check_row] != 255)
|
||||
{
|
||||
double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
|
||||
double bias = readDoubleFromFile(center_bias_file); // 中位偏置(像素), 正=偏右
|
||||
deviation -= bias;
|
||||
double norm = deviation / (newWidth / 2.0); // 归一化到 ±1
|
||||
g_steer_deviation = norm; // 给速度控制用
|
||||
|
||||
// 2c. 死区 (像素)
|
||||
double deadband = readDoubleFromFile(deadband_file);
|
||||
if (std::abs(deviation) < deadband)
|
||||
{
|
||||
servo.setDutyCycle(1500000); // 中位 1.50ms
|
||||
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++;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 2d. 比例转向: 像素偏差 → 归一化 → 舵机脉宽
|
||||
zebra_seen = true;
|
||||
|
||||
int foresee = (int)readDoubleFromFile(zebrasee_file);
|
||||
if (zebra_cy > foresee)
|
||||
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;
|
||||
printf("[ZEBRA] cy=%d cf=%.2f f=%d mc=%d 停车4s 冷却5s\n",
|
||||
zebra_cy, zebra_cf, g_zc_frames, g_zc_min_cy);
|
||||
g_zc_frames = 0; g_zc_min_cy = 120;
|
||||
} else {
|
||||
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;
|
||||
printf("[ZEBRA] 起步\n");
|
||||
}
|
||||
break;
|
||||
case Z_COOLDOWN:
|
||||
if (now - g_ztime >= 5) {
|
||||
g_zstate = Z_NORMAL;
|
||||
printf("[ZEBRA] 恢复\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
// ── 6. 舵机 ──
|
||||
if (readFlag(start_file)) {
|
||||
int foresee = (int)readDoubleFromFile(foresee_file);
|
||||
int check_row = foresee / calc_scale;
|
||||
if (check_row >= 0 && check_row < line_tracking_height && mid_line[check_row] != 255) {
|
||||
double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
|
||||
double bias = readDoubleFromFile(center_bias_file);
|
||||
deviation -= bias;
|
||||
g_steer_deviation = deviation / (newWidth / 2.0);
|
||||
double deadband = readDoubleFromFile(deadband_file);
|
||||
if (std::abs(deviation) < deadband) {
|
||||
servo.setDutyCycle(1500000);
|
||||
} else {
|
||||
double steer_gain = readDoubleFromFile(steer_gain_file);
|
||||
double norm = deviation / (newWidth / 2.0); // 归一化到 ±1
|
||||
double offset = norm * steer_gain * 300000; // 半行程 0.30ms
|
||||
double norm = deviation / (newWidth / 2.0);
|
||||
double offset = norm * 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));
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
// ── 7. 电机控制 (单出口) ──
|
||||
ControlUpdate(target_speed, g_zstate == Z_STOP);
|
||||
|
||||
// ── 8. LCD ──
|
||||
{
|
||||
// servo.setDutyCycle(1520000);
|
||||
static int lcd_check = 0;
|
||||
if (--lcd_check < 0) { g_lcd_on = readFlag(showImg_file); lcd_check = 10; }
|
||||
}
|
||||
|
||||
// 显示图片
|
||||
if (readFlag(showImg_file))
|
||||
{
|
||||
if (g_lcd_on) {
|
||||
cv::Mat fbImage(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0));
|
||||
|
||||
// 缩放视频到新尺寸
|
||||
cv::Mat resizedFrame;
|
||||
cv::resize(track, resizedFrame, cv::Size(newWidth, newHeight));
|
||||
// 将单通道的二值化图像转换为三通道的彩色图像
|
||||
cv::Mat coloredResizedFrame;
|
||||
cv::cvtColor(resizedFrame, coloredResizedFrame, cv::COLOR_GRAY2BGR); // 转换为彩色图像
|
||||
cv::cvtColor(resizedFrame, coloredResizedFrame, cv::COLOR_GRAY2BGR);
|
||||
|
||||
// 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
|
||||
fbImage.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
|
||||
fbImage.setTo(cv::Scalar(0, 0, 0));
|
||||
cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
|
||||
coloredResizedFrame.copyTo(fbImage(roi));
|
||||
|
||||
// 绘制左右边界线和中线
|
||||
int scaledLeftX, scaledRightX, scaledMidX, scaledY;
|
||||
|
||||
for (int y = 0; y < line_tracking_height; y++)
|
||||
{
|
||||
// 根据缩放比例调整X坐标
|
||||
scaledLeftX = static_cast<int>(left_line[y] * calc_scale);
|
||||
scaledRightX = static_cast<int>(right_line[y] * calc_scale);
|
||||
scaledMidX = static_cast<int>(mid_line[y] * calc_scale);
|
||||
scaledY = static_cast<int>(y * calc_scale);
|
||||
|
||||
// 绘制左边界（红）
|
||||
cv::line(fbImage(roi), cv::Point(scaledLeftX, scaledY), cv::Point(scaledLeftX, scaledY), cv::Scalar(0, 0, 255), calc_scale);
|
||||
// 绘制右边界（绿）
|
||||
cv::line(fbImage(roi), cv::Point(scaledRightX, scaledY), cv::Point(scaledRightX, scaledY), cv::Scalar(0, 255, 0), calc_scale);
|
||||
// 绘制中线 （蓝）
|
||||
cv::line(fbImage(roi), cv::Point(scaledMidX, scaledY), cv::Point(scaledMidX, scaledY), cv::Scalar(255, 0, 0), calc_scale);
|
||||
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(fbImage(roi), cv::Point(sLX,sY), cv::Point(sLX,sY), cv::Scalar(0,0,255), calc_scale);
|
||||
cv::line(fbImage(roi), cv::Point(sRX,sY), cv::Point(sRX,sY), cv::Scalar(0,255,0), calc_scale);
|
||||
cv::line(fbImage(roi), cv::Point(sMX,sY), cv::Point(sMX,sY), cv::Scalar(255,0,0), calc_scale);
|
||||
}
|
||||
|
||||
// 将帧缓冲区图像转换为RGB565格式
|
||||
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);
|
||||
cv::rectangle(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(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";
|
||||
cv::putText(fbImage(roi), ztxt, cv::Point(2,newHeight-4), cv::FONT_HERSHEY_SIMPLEX,0.4,cv::Scalar(0,255,255),1);
|
||||
|
||||
convertMatToRGB565(fbImage, fb_buffer, screenWidth, screenHeight);
|
||||
}
|
||||
|
||||
// ── 9. FPS ──
|
||||
{
|
||||
static int fc=0; static timespec t0; if(fc==0) clock_gettime(CLOCK_MONOTONIC,&t0);
|
||||
fc++;
|
||||
if(fc%15==0){ timespec t1; clock_gettime(CLOCK_MONOTONIC,&t1);
|
||||
double dt=(t1.tv_sec-t0.tv_sec)+(t1.tv_nsec-t0.tv_nsec)*1e-9;
|
||||
printf("fps=%.1f zc=%c lcd=%c \r", fc/dt, g_zebra_ever?'Y':' ', g_lcd_on?'Y':' ');
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user