清理编码器+死代码+可读性整理: 删除ENCODER类(含UB析构), MotorController简化为PWM+GPIO裸控制器, 移除mortor_kp/ki/kd全局变量, 排除vl53l0x/zebra_detect死代码, 修复image_cv行列写反, 清理无关include
Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-openagent) Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>
This commit is contained in:
spdis
+101
-104
@@ -4,12 +4,15 @@
|
||||
#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缩放解码
|
||||
|
||||
double kp = 0, ki = 0, kd = 0;
|
||||
int screenWidth, screenHeight, newWidth, newHeight;
|
||||
int fb;
|
||||
uint16_t *fb_buffer;
|
||||
@@ -21,12 +24,11 @@ PwmController servo(1, 0);
|
||||
#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(最远)
|
||||
// ── 斑马线去抖 ──
|
||||
#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 };
|
||||
@@ -40,26 +42,8 @@ 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 音频 (持久打开) ──
|
||||
// ── I2C 音频 ──
|
||||
static int i2c_audio_fd = -1;
|
||||
|
||||
static bool i2c_audio_open()
|
||||
@@ -100,17 +84,14 @@ 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_FRAME_WIDTH, 320);
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, 240);
|
||||
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;
|
||||
}
|
||||
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);
|
||||
@@ -137,23 +118,12 @@ int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
|
||||
|
||||
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) {
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
|
||||
@@ -180,8 +150,7 @@ static void play_zebra_audio()
|
||||
printf("[ZEBRA] 语音失败: I2C 未打开\n");
|
||||
return;
|
||||
}
|
||||
|
||||
ioctl(i2c_audio_fd, I2C_SLAVE, 0x34); // 每次重设从地址
|
||||
ioctl(i2c_audio_fd, I2C_SLAVE, 0x34);
|
||||
|
||||
union i2c_smbus_data d;
|
||||
struct i2c_smbus_ioctl_data a;
|
||||
@@ -198,40 +167,66 @@ static void play_zebra_audio()
|
||||
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)
|
||||
{
|
||||
// ── 1. 取最新帧 (背景线程持续采集, 写全局 raw_frame 供 image_main 使用) ──
|
||||
frameMutex.lock();
|
||||
raw_frame = pubframe;
|
||||
frameMutex.unlock();
|
||||
if (raw_frame.empty()) { return -1; }
|
||||
struct timespec t0,t1,t2,t3,t4,t5;
|
||||
|
||||
// ── 2. 保存图像 ──
|
||||
if (readFlag(saveImg_file)) {
|
||||
if (saveCameraImage(raw_frame, "./image"))
|
||||
printf("图像%d已保存\n", saved_frame_count);
|
||||
// ── 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);
|
||||
}
|
||||
|
||||
// ── 3. 视觉巡线 (禁止动) ──
|
||||
image_main();
|
||||
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;
|
||||
}
|
||||
|
||||
// ── 4. 模型推理 (每2帧一次) ──
|
||||
// ── 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()) {
|
||||
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);
|
||||
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;
|
||||
// ── 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) {
|
||||
@@ -245,8 +240,7 @@ int CameraHandler(void)
|
||||
}
|
||||
zebra_seen = true;
|
||||
|
||||
int foresee = (int)readDoubleFromFile(zebrasee_file);
|
||||
if (zebra_cy > foresee)
|
||||
if (zebra_cy > g_cfg.zebrasee)
|
||||
zebra_near = true;
|
||||
break;
|
||||
}
|
||||
@@ -266,11 +260,11 @@ int CameraHandler(void)
|
||||
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);
|
||||
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 {
|
||||
printf("[ZEBRA] cy=%d 拒绝: f=%d/%d mc=%d/%d\n",
|
||||
} 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);
|
||||
}
|
||||
}
|
||||
@@ -278,33 +272,30 @@ int CameraHandler(void)
|
||||
case Z_STOP:
|
||||
if (now - g_ztime >= 4) {
|
||||
g_zstate = Z_COOLDOWN; g_ztime = now;
|
||||
printf("[ZEBRA] 起步\n");
|
||||
if (g_cfg.debug) printf("[ZEBRA] 起步\n");
|
||||
}
|
||||
break;
|
||||
case Z_COOLDOWN:
|
||||
if (now - g_ztime >= 5) {
|
||||
g_zstate = Z_NORMAL;
|
||||
printf("[ZEBRA] 恢复\n");
|
||||
if (g_cfg.debug) printf("[ZEBRA] 恢复\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
// ── 6. 舵机 ──
|
||||
if (readFlag(start_file)) {
|
||||
int foresee = (int)readDoubleFromFile(foresee_file);
|
||||
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] != 255) {
|
||||
double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
|
||||
double bias = readDoubleFromFile(center_bias_file);
|
||||
deviation -= bias;
|
||||
deviation -= g_cfg.center_bias;
|
||||
g_steer_deviation = deviation / (newWidth / 2.0);
|
||||
double deadband = readDoubleFromFile(deadband_file);
|
||||
if (std::abs(deviation) < deadband) {
|
||||
if (std::abs(deviation) < g_cfg.deadband) {
|
||||
servo.setDutyCycle(1500000);
|
||||
} else {
|
||||
double steer_gain = readDoubleFromFile(steer_gain_file);
|
||||
double norm = deviation / (newWidth / 2.0);
|
||||
double offset = norm * steer_gain * 300000;
|
||||
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));
|
||||
@@ -312,32 +303,32 @@ int CameraHandler(void)
|
||||
}
|
||||
}
|
||||
|
||||
// ── 7. 电机控制 (单出口) ──
|
||||
// ── 7. 电机控制 ──
|
||||
ControlUpdate(target_speed, g_zstate == Z_STOP);
|
||||
clock_gettime(CLOCK_MONOTONIC,&t4);
|
||||
|
||||
// ── 8. LCD ──
|
||||
{
|
||||
static int lcd_check = 0;
|
||||
if (--lcd_check < 0) { g_lcd_on = readFlag(showImg_file); lcd_check = 10; }
|
||||
static int lcd_check = 0;
|
||||
if (--lcd_check < 0) {
|
||||
g_lcd_on = g_cfg.showImg;
|
||||
lcd_check = 10;
|
||||
}
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
fbImage.setTo(cv::Scalar(0, 0, 0));
|
||||
cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
|
||||
coloredResizedFrame.copyTo(fbImage(roi));
|
||||
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(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);
|
||||
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;
|
||||
@@ -349,26 +340,32 @@ int CameraHandler(void)
|
||||
x2=std::max(0,std::min(newWidth-1,x2)); y2=std::max(0,std::min(newHeight-1,y2));
|
||||
cv::Scalar color(0,255,0);
|
||||
if (g_boxes[i].cls == ZEBRA_CLASS) color=cv::Scalar(255,0,255);
|
||||
cv::rectangle(fbImage(roi), cv::Point(x1,y1), cv::Point(x2,y2), color, 2);
|
||||
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(fbImage(roi), lab, cv::Point(x1+2,y1+10), cv::FONT_HERSHEY_SIMPLEX,0.3,color,1);
|
||||
cv::putText(lcd_fbImage(roi), lab, cv::Point(x1+2,y1+10), cv::FONT_HERSHEY_SIMPLEX,0.3,color,1);
|
||||
}
|
||||
|
||||
const char* ztxt="N";
|
||||
if (g_zstate==Z_STOP) ztxt="S";
|
||||
else if (g_zstate==Z_COOLDOWN) ztxt="C";
|
||||
cv::putText(fbImage(roi), ztxt, cv::Point(2,newHeight-4), cv::FONT_HERSHEY_SIMPLEX,0.4,cv::Scalar(0,255,255),1);
|
||||
cv::putText(lcd_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);
|
||||
convertMatToRGB565(lcd_fbImage, fb_buffer, screenWidth, screenHeight);
|
||||
}
|
||||
|
||||
// ── 9. FPS ──
|
||||
// ── 9. FPS + 分步计时 ──
|
||||
{
|
||||
static int fc=0; static timespec t0; if(fc==0) clock_gettime(CLOCK_MONOTONIC,&t0);
|
||||
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 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':' ');
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user