From 114d93cec781195b120d83eb85c039500daa4e25 Mon Sep 17 00:00:00 2001 From: spdis Date: Wed, 10 Jun 2026 15:15:34 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B8=85=E7=90=86=E7=BC=96=E7=A0=81=E5=99=A8+?= =?UTF-8?q?=E6=AD=BB=E4=BB=A3=E7=A0=81+=E5=8F=AF=E8=AF=BB=E6=80=A7?= =?UTF-8?q?=E6=95=B4=E7=90=86:=20=E5=88=A0=E9=99=A4ENCODER=E7=B1=BB(?= =?UTF-8?q?=E5=90=ABUB=E6=9E=90=E6=9E=84),=20MotorController=E7=AE=80?= =?UTF-8?q?=E5=8C=96=E4=B8=BAPWM+GPIO=E8=A3=B8=E6=8E=A7=E5=88=B6=E5=99=A8,?= =?UTF-8?q?=20=E7=A7=BB=E9=99=A4mortor=5Fkp/ki/kd=E5=85=A8=E5=B1=80?= =?UTF-8?q?=E5=8F=98=E9=87=8F,=20=E6=8E=92=E9=99=A4vl53l0x/zebra=5Fdetect?= =?UTF-8?q?=E6=AD=BB=E4=BB=A3=E7=A0=81,=20=E4=BF=AE=E5=A4=8Dimage=5Fcv?= =?UTF-8?q?=E8=A1=8C=E5=88=97=E5=86=99=E5=8F=8D,=20=E6=B8=85=E7=90=86?= =?UTF-8?q?=E6=97=A0=E5=85=B3include?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-openagent) Co-authored-by: Sisyphus --- CMakeLists.txt | 43 +++------ lib/MotorController.h | 25 ++--- lib/camera.h | 6 -- lib/control.h | 15 ++- lib/encoder.h | 75 --------------- lib/global.h | 29 +++--- lib/image_cv.h | 20 +--- main/CMakeLists.txt | 9 ++ main/main.cpp | 24 +++-- src/MotorController.cpp | 50 ++-------- src/camera.cpp | 205 ++++++++++++++++++++-------------------- src/control.cpp | 35 ++++--- src/encoder.cpp | 82 ---------------- src/global.cpp | 37 ++++---- src/image_cv.cpp | 74 +++------------ src/model_v10.cpp | 54 +++++++++-- 16 files changed, 264 insertions(+), 519 deletions(-) delete mode 100644 lib/encoder.h delete mode 100644 src/encoder.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f964c3..8eadcf8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,45 +6,30 @@ cmake_minimum_required(VERSION 3.5.0) # 设置 C++ 标准 set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -pthread -Wall -march=loongarch64 -mtune=loongarch64 -ffast-math -funroll-loops -fomit-frame-pointer") # 对于 C++ 编译器 -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") # 对于 C 编译器 +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -pthread -Wall -march=loongarch64 -mtune=loongarch64 -ffast-math -funroll-loops -fomit-frame-pointer") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") -# 定义项目名称和版本，并指定使用C和C++语言 +# 定义项目名称和版本 project(smartcar_demo2 VERSION 0.1.0 LANGUAGES C CXX) -# 设置OpenCV的安装路径 +# OpenCV 设备端路径 set(OpenCV_DIR /mnt/d/PPPProgram/smartcar/opencv_device/lib/cmake/opencv4) - -# 查找OpenCV库，确保安装了所需的依赖 find_package(OpenCV REQUIRED) - -# 包含OpenCV的头文件路径 include_directories(${OpenCV_INCLUDE_DIRS}) -include_directories(src) message(STATUS "OpenCV Include Directories: ${OpenCV_INCLUDE_DIRS}") -# 包含项目的自定义库路径 +# 项目头文件路径 +include_directories(src) include_directories(lib) -# 查找源文件所在的目录 +# 收集 src 下所有 .cpp (自动发现) aux_source_directory(src DIR_SRCS) -# 将 src 目录下的源文件编译为静态库 -add_library(common_lib STATIC ${DIR_SRCS}) +# 排除暂不使用的模块（源文件保留） +# vl53l0x.cpp — 激光测距, 硬件未接 +# zebra_detect.cpp — 经典斑马线检测, 已由 nanodet 模型替代 +list(FILTER DIR_SRCS EXCLUDE REGEX "(vl53l0x\\.cpp|zebra_detect\\.cpp)") -# 添加子目录 -add_subdirectory(main) # 主程序 -add_subdirectory(demo1) # demo1 -add_subdirectory(framebuffer_demo) -add_subdirectory(opencv_demo1) -add_subdirectory(opencv_demo2) -add_subdirectory(opencv_demo3) -add_subdirectory(encoder_demo) -add_subdirectory(jy62_demo) -add_subdirectory(key_demo) -add_subdirectory(wonderEcho_demo) -add_subdirectory(udp_receive) -add_subdirectory(image_test) -# add_subdirectory(zebra_demo) -add_subdirectory(gd13_demo) -add_subdirectory(screenshot_demo) \ No newline at end of file +# 静态库 + 主程序 +add_library(common_lib STATIC ${DIR_SRCS}) +add_subdirectory(main) diff --git a/lib/MotorController.h b/lib/MotorController.h index e568d5f..796c9e5 100644 --- a/lib/MotorController.h +++ b/lib/MotorController.h @@ -1,37 +1,26 @@ /* - * @Author: ilikara 3435193369@qq.com - * @Date: 2024-10-10 14:36:47 - * @LastEditors: ilikara 3435193369@qq.com - * @LastEditTime: 2025-03-21 10:42:00 - * @FilePath: /smartcar/lib/MotorController.h - * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE + * MotorController — 直流电机控制器 + * + * 封装 PWM 调速 + GPIO 方向控制，开环占空比直驱。 + * 不包含编码器反馈和 PID 闭环（开环巡线够用）。 */ #ifndef MOTOR_CONTROLLER_H #define MOTOR_CONTROLLER_H #include "PwmController.h" -#include "PIDController.h" #include "GPIO.h" -#include "encoder.h" class MotorController { public: - MotorController(int pwmchip, int pwmnum, int gpioNum, unsigned int period_ns, - double kp, double ki, double kd, double targetSpeed, - int encoder_pwmNum, int encoder_gpioNum, int encoder_dir_); + MotorController(int pwmchip, int pwmnum, int gpioNum, unsigned int period_ns); ~MotorController(void); - void updateSpeed(void); - void updateTarget(int speed); - void updateduty(double dutyCycle); - PIDController pidController; + void updateduty(double dutyCycle); // dutyCycle: -100.0 ~ 100.0 private: PwmController pwmController; - ENCODER encoder; GPIO directionGPIO; - int encoder_dir; }; -#endif // MOTOR_CONTROLLER_H +#endif diff --git a/lib/camera.h b/lib/camera.h index 4bfcbdf..1996276 100644 --- a/lib/camera.h +++ b/lib/camera.h @@ -17,20 +17,14 @@ #include #include "image_cv.h" -#include "PIDController.h" -#include "PwmController.h" #include "global.h" #include "frame_buffer.h" -#include "serial.h" #include "control.h" int CameraInit(uint8_t camera_id, double dest_fps, int width, int height); int CameraHandler(void); void cameraDeInit(void); -extern double kp; -extern double ki; -extern double kd; extern double g_steer_deviation; #endif diff --git a/lib/control.h b/lib/control.h index 8a998fe..c47b0d3 100644 --- a/lib/control.h +++ b/lib/control.h @@ -1,21 +1,18 @@ #ifndef CONTROL_H #define CONTROL_H -#include - #include "MotorController.h" -#include "global.h" -#include "serial.h" +#include "GPIO.h" +// ── 电机控制接口 ── void ControlInit(); void ControlUpdate(double speed, bool zebra_block); void ControlExit(); -extern double mortor_kp; -extern double mortor_ki; -extern double mortor_kd; - -extern GPIO mortorEN; +// 左右电机对象（control.cpp 分配） extern MotorController *motorController[2]; +// 电机使能 GPIO (73) +extern GPIO mortorEN; + #endif diff --git a/lib/encoder.h b/lib/encoder.h deleted file mode 100644 index c06cf77..0000000 --- a/lib/encoder.h +++ /dev/null @@ -1,75 +0,0 @@ -/* - * @Author: ilikara 3435193369@qq.com - * @Date: 2024-10-11 06:20:04 - * @LastEditors: ilikara 3435193369@qq.com - * @LastEditTime: 2024-12-01 03:48:28 - * @FilePath: /smartcar/lib/encoder.h - * @Description: - * - * Copyright (c) 2024 by ilikara 3435193369@qq.com, All Rights Reserved. - */ -#ifndef ENCODER_H -#define ENCODER_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "GPIO.h" - -#define PWM_BASE_ADDR 0x1611B000 -#define PWM_OFFSET 0x10 -#define LOW_BUFFER_OFFSET 0x4 -#define FULL_BUFFER_OFFSET 0x8 -#define CONTROL_REG_OFFSET 0xC - -#define CNTR_ENABLE_BIT (1 << 0) // 计数器使能 -#define PULSE_OUT_ENABLE_BIT (1 << 3) // 脉冲输出使能（低有效） -#define SINGLE_PULSE_BIT (1 << 4) // 单脉冲控制位 -#define INT_ENABLE_BIT (1 << 5) // 中断使能 -#define INT_STATUS_BIT (1 << 6) // 中断状态 -#define COUNTER_RESET_BIT (1 << 7) // 计数器重置 -#define MEASURE_PULSE_BIT (1 << 8) // 测量脉冲使能 -#define INVERT_OUTPUT_BIT (1 << 9) // 输出翻转使能 -#define DEAD_ZONE_ENABLE_BIT (1 << 10) // 防死区使能 - -#define LOW_BUFFER_ADDR (PWM_BASE_ADDR + LOW_BUFFER_OFFSET) -#define FULL_BUFFER_ADDR (PWM_BASE_ADDR + FULL_BUFFER_OFFSET) -#define CONTROL_REG_ADDR (PWM_BASE_ADDR + CONTROL_REG_OFFSET) - -#define GPIO_PIN 73 -#define GPIO_PATH "/sys/class/gpio/gpio73/value" - -#define PAGE_SIZE 0x10000 - -#define REG_READ(addr) (*(volatile uint32_t *)(addr)) -#define REG_WRITE(addr, val) (*(volatile uint32_t *)(addr) = (val)) - -class ENCODER -{ - -public: - ENCODER(int pwmNum, int gpioNum); - ~ENCODER(); - - double pulse_counter_update(void); - -private: - uint32_t base_addr; - GPIO directionGPIO; - void *low_buffer; - void *full_buffer; - void *control_buffer; - void *map_register(uint32_t physical_address, size_t size); - void PWM_Init(void); - void reset_counter(void); -}; - -#endif diff --git a/lib/global.h b/lib/global.h index 03a3aa3..68dd773 100644 --- a/lib/global.h +++ b/lib/global.h @@ -2,18 +2,12 @@ #define GLOBAL_H #include -#include -#include -#include +// ── 参数文件名 ── const std::string kp_file = "./kp"; const std::string ki_file = "./ki"; const std::string kd_file = "./kd"; -const std::string mortor_kp_file = "./mortor_kp"; -const std::string mortor_ki_file = "./mortor_ki"; -const std::string mortor_kd_file = "./mortor_kd"; - const std::string start_file = "./start"; const std::string showImg_file = "./showImg"; const std::string destfps_file = "./destfps"; @@ -24,14 +18,25 @@ const std::string speed_file = "./speed"; const std::string deadband_file = "./deadband"; const std::string steer_gain_file = "./steer_gain"; const std::string center_bias_file = "./center_bias"; +const std::string debug_file = "./debug"; -// 从文件读取双精度值 double readDoubleFromFile(const std::string &filename); +bool readFlag(const std::string &filename); +void cfg_load_all(); -// 从文件中读取标志 -bool readFlag(const std::string &filename); - -extern std::atomic PID_rotate; +// ── 启动时缓存的运行参数 ── +struct CfgCache { + double speed = 60; // 目标速度(占空比 %) + double foresee = 80; // 前瞻行 + double zebrasee = 60; // 斑马线触发距离阈值 + double deadband = 5; // 舵机死区 + double steer_gain = 1.0; // 舵机增益 + double center_bias = 0; // 中线偏移修正 + int start = 0; // 1=启动 0=停止 + int showImg = 0; // LCD 预览开关 + int debug = 0; // debug 模式 +}; +extern CfgCache g_cfg; extern double target_speed; diff --git a/lib/image_cv.h b/lib/image_cv.h index fe80828..7f0ea1f 100644 --- a/lib/image_cv.h +++ b/lib/image_cv.h @@ -1,11 +1,3 @@ -/* - * @Author: ilikara 3435193369@qq.com - * @Date: 2025-01-04 06:51:37 - * @LastEditors: ilikara 3435193369@qq.com - * @LastEditTime: 2025-03-13 08:05:00 - * @FilePath: /smartcar/lib/image_main.h - * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE - */ #include #include #include @@ -13,16 +5,12 @@ void image_main(); extern cv::Mat raw_frame; -extern cv::Mat grayFrame; extern cv::Mat binarizedFrame; extern cv::Mat morphologyExFrame; extern cv::Mat track; -extern std::vector left_line; // 左边缘列号数组 -extern std::vector right_line; // 右边缘列号数组 -extern std::vector mid_line; // 中线列号数组 -extern std::vector left_line_filtered; // 中线列号数组 -extern std::vector right_line_filtered; // 中线列号数组 -extern std::vector mid_line_filtered; // 中线列号数组 +extern std::vector left_line; +extern std::vector right_line; +extern std::vector mid_line; -extern int line_tracking_height, line_tracking_width; \ No newline at end of file +extern int line_tracking_height, line_tracking_width; diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 745ce0c..4cec894 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,4 +1,13 @@ # 主程序 add_executable(smartcar_demo1 main.cpp) +# 暂时排除激光测距（未使用），保留源文件 +set(EXCLUDE_VL53L0X vl53l0x.cpp) + +# 排除死代码：斑马线传统检测（已由 nanodet 模型替代）、编码器采集（开环控制不需要） +set(EXCLUDE_DEAD zebra_detect.cpp encoder.cpp) + +# 从静态库源文件列表中移除 +list(FILTER DIR_SRCS EXCLUDE REGEX "(${EXCLUDE_VL53L0X}|${EXCLUDE_DEAD})") + target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS}) \ No newline at end of file diff --git a/main/main.cpp b/main/main.cpp index 5544582..2410fb2 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -24,31 +24,29 @@ int main(void) double dest_fps = readDoubleFromFile(destfps_file); if (dest_fps <= 0) dest_fps = 30.0; + cfg_load_all(); + if (CameraInit(0, dest_fps, 320, 240) < 0) { std::cerr << "CameraInit failed" << std::endl; return -1; } ControlInit(); - std::cout << "All services started (single-thread mode)" << std::endl; + std::cout << "All services started" << std::endl; int tick = 0; while (running.load()) { - if (CameraHandler() < 0) { - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - continue; + CameraHandler(); + + if (++tick % 7 == 0) { + g_cfg.debug = readFlag(debug_file); } - if (++tick % 15 == 0) - { - target_speed = readDoubleFromFile(speed_file); - mortor_kp = readDoubleFromFile(mortor_kp_file); - mortor_ki = readDoubleFromFile(mortor_ki_file); - mortor_kd = readDoubleFromFile(mortor_kd_file); - kp = readDoubleFromFile(kp_file); - ki = readDoubleFromFile(ki_file); - kd = readDoubleFromFile(kd_file); + if (g_cfg.debug) { + cfg_load_all(); } + + target_speed = g_cfg.speed; } std::cout << "Stopping..." << std::endl; diff --git a/src/MotorController.cpp b/src/MotorController.cpp index 871b8cd..edf6153 100644 --- a/src/MotorController.cpp +++ b/src/MotorController.cpp @@ -1,22 +1,14 @@ /* - * @Author: ilikara 3435193369@qq.com - * @Date: 2024-10-10 14:36:42 - * @LastEditors: ilikara 3435193369@qq.com - * @LastEditTime: 2025-03-21 10:41:17 - * @FilePath: /smartcar/src/MotorController.cpp - * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE + * MotorController — 直流电机控制器实现 */ #include "MotorController.h" -MotorController::MotorController(int pwmchip, int pwmnum, int gpioNum, unsigned int period_ns, - double kp, double ki, double kd, double targetSpeed, - int encoder_pwmNum, int encoder_gpioNum, int encoder_dir_) - : pwmController(pwmchip, pwmnum), directionGPIO(gpioNum), pidController(kp, ki, kd, targetSpeed, INCREMENTAL, 80), - encoder(encoder_pwmNum, encoder_gpioNum), encoder_dir(encoder_dir_) +MotorController::MotorController(int pwmchip, int pwmnum, int gpioNum, unsigned int period_ns) + : pwmController(pwmchip, pwmnum), directionGPIO(gpioNum) { - pwmController.setPeriod(period_ns); // 设置 PWM 周期 + pwmController.setPeriod(period_ns); directionGPIO.setDirection("out"); - pwmController.enable(); // 启用 PWM + pwmController.enable(); } MotorController::~MotorController(void) @@ -26,37 +18,9 @@ MotorController::~MotorController(void) void MotorController::updateduty(double dutyCycle) { - int newduty = pwmController.readPeriod() * abs(dutyCycle) / 100.0; + int newduty = pwmController.readPeriod() * std::abs(dutyCycle) / 100.0; if (newduty != pwmController.readDutyCycle()) - { pwmController.setDutyCycle(newduty); - } - // 根据 PID 输出设置 GPIO 的方向 - if (dutyCycle > 0) - { - directionGPIO.setValue(1); // 正向 - } - else - { - directionGPIO.setValue(0); // 反向 - } - //std::cout << encoder.pulse_counter_update() << std::endl; -} - -void MotorController::updateSpeed(void) -{ - double encoderReading = encoder.pulse_counter_update() * encoder_dir; - // std::cout << encoderReading << std::endl; - double output = pidController.update(encoderReading); - // int dutyCycle = static_cast(output); - - // 设置 PWM 占空比 - updateduty(output); - std::cout << encoderReading << " " << output << std::endl; -} - -void MotorController::updateTarget(int speed) -{ - pidController.setTarget(speed); + directionGPIO.setValue(dutyCycle > 0); } diff --git a/src/camera.cpp b/src/camera.cpp index a4d2f74..c835b2e 100644 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -4,12 +4,15 @@ #include #include #include +#include #include #include 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(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(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(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(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); } } diff --git a/src/control.cpp b/src/control.cpp index 7072152..bf9f13d 100644 --- a/src/control.cpp +++ b/src/control.cpp @@ -1,54 +1,51 @@ +/* + * control — 电机控制层 + * + * 开环占空比控制：根据目标速度和偏差计算占空比，直接输出到 PWM。 + * 无编码器反馈，无 PID 闭环。 + */ #include "control.h" -#include "GPIO.h" +#include "global.h" extern double g_steer_deviation; MotorController *motorController[2] = {nullptr, nullptr}; - GPIO mortorEN(73); -double mortor_kp = 1000; -double mortor_ki = 300; -double mortor_kd = 0; - void ControlInit() { mortorEN.setDirection("out"); mortorEN.setValue(1); + // 左电机方向: GPIO12 (右电机方向由 MotorController 自管, 但左In2也一样要设) GPIO leftIn2(13); leftIn2.setDirection("out"); leftIn2.setValue(1); - const int pwmchip[2] = {8, 8}; - const int pwmnum[2] = {2, 1}; - const int gpioNum[2] = {12, 13}; - const int encoder_pwmchip[2] = {0, 3}; - const int encoder_gpioNum[2] = {75, 72}; - const int encoder_dir[2] = {1, -1}; - + const int pwmchip[2] = {8, 8}; + const int pwmnum[2] = {2, 1}; + const int gpioNum[2] = {12, 13}; const unsigned int period_ns = 50000; for (int i = 0; i < 2; ++i) { motorController[i] = new MotorController( - pwmchip[i], pwmnum[i], gpioNum[i], period_ns, - mortor_kp, mortor_ki, mortor_kd, 0, - encoder_pwmchip[i], encoder_gpioNum[i], encoder_dir[i] + pwmchip[i], pwmnum[i], gpioNum[i], period_ns ); } } void ControlUpdate(double speed, bool zebra_block) { - if (zebra_block || !readFlag(start_file)) + if (zebra_block || !g_cfg.start) { for (int i = 0; i < 2; ++i) if (motorController[i]) motorController[i]->updateduty(0); - if (!readFlag(start_file)) mortorEN.setValue(0); + if (!g_cfg.start) mortorEN.setValue(0); return; } + // 弯道降速: 偏差越大，速度越低 (最低 60%) double curve = 1.0 - std::abs(g_steer_deviation) * 0.4; if (curve < 0.6) curve = 0.6; double spd = speed * curve; @@ -64,7 +61,7 @@ void ControlExit() for (int i = 0; i < 2; ++i) { delete motorController[i]; - std::cout << "motor" << i << " deleted\n"; + motorController[i] = nullptr; } mortorEN.setValue(0); } diff --git a/src/encoder.cpp b/src/encoder.cpp deleted file mode 100644 index 5f605f0..0000000 --- a/src/encoder.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/* - * @Author: ilikara 3435193369@qq.com - * @Date: 2024-10-11 06:19:57 - * @LastEditors: ilikara 3435193369@qq.com - * @LastEditTime: 2024-12-01 03:54:06 - * @FilePath: /smartcar/src/encoder.cpp - * @Description: - * - * Copyright (c) 2024 by ilikara 3435193369@qq.com, All Rights Reserved. - */ -#include "encoder.h" - -ENCODER::ENCODER(int pwmNum, int gpioNum) : base_addr(PWM_BASE_ADDR + pwmNum * PWM_OFFSET), directionGPIO(gpioNum) -{ - directionGPIO.setDirection("in"); - - control_buffer = map_register(base_addr + CONTROL_REG_OFFSET, PAGE_SIZE); - low_buffer = map_register(base_addr + LOW_BUFFER_OFFSET, PAGE_SIZE); - full_buffer = map_register(base_addr + FULL_BUFFER_OFFSET, PAGE_SIZE); - - printf("Registers mapped successfully\n"); - - PWM_Init(); -} - -ENCODER::~ENCODER() -{ - directionGPIO.~GPIO(); - munmap(control_buffer, PAGE_SIZE); - munmap(low_buffer, PAGE_SIZE); - munmap(full_buffer, PAGE_SIZE); -} - -void *ENCODER::map_register(uint32_t physical_address, size_t size) -{ - int mem_fd = open("/dev/mem", O_RDWR | O_SYNC); - if (mem_fd == -1) - { - perror("Failed to open /dev/mem"); - exit(EXIT_FAILURE); - } - - void *mapped_addr = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, physical_address & ~(PAGE_SIZE - 1)); - if (mapped_addr == MAP_FAILED) - { - perror("Failed to map memory"); - close(mem_fd); - exit(EXIT_FAILURE); - } - - close(mem_fd); - - return (void *)((uintptr_t)mapped_addr + (physical_address & (PAGE_SIZE - 1))); -} - -void ENCODER::PWM_Init(void) -{ - uint32_t control_reg = 0; - - control_reg |= CNTR_ENABLE_BIT; - control_reg |= MEASURE_PULSE_BIT; - control_reg |= INT_ENABLE_BIT; - - REG_WRITE(control_buffer, control_reg); - - printf("PWM initialized with control register: 0x%08X\n", control_reg); -} - -void ENCODER::reset_counter(void) -{ - uint32_t control_reg = REG_READ(control_buffer); - control_reg |= COUNTER_RESET_BIT; - REG_WRITE(control_buffer, control_reg); -} - -double ENCODER::pulse_counter_update(void) -{ - double value = 100000000.0 / REG_READ(full_buffer) / 1024.0 * (directionGPIO.readValue() * 2 - 1); - // reset_counter(); - // printf("Encoder RPS: %8.1lf\n", 100000000.0 / REG_READ(full_buffer) / 1024.0 * (gpio_value * 2 - 1)); - return value; -} \ No newline at end of file diff --git a/src/global.cpp b/src/global.cpp index 246eb2c..f03fc62 100644 --- a/src/global.cpp +++ b/src/global.cpp @@ -1,37 +1,34 @@ #include "global.h" +#include +CfgCache g_cfg; double target_speed; -// 从文件读取双精度值 double readDoubleFromFile(const std::string &filename) { std::ifstream file(filename); double value = 0.0; - if (file.is_open()) - { - file >> value; // 读取文件中的值 - file.close(); - } - else - { - std::cerr << "Failed to open " << filename << std::endl; - } + if (file.is_open()) { file >> value; file.close(); } return value; } -// 从文件中读取标志 bool readFlag(const std::string &filename) { std::ifstream file(filename); int flag = 0; - if (file.is_open()) - { - file >> flag; // 读取文件中的更新标志 - file.close(); - } - else - { - std::cerr << "Failed to open " << filename << std::endl; - } + if (file.is_open()) { file >> flag; file.close(); } return flag; } + +void cfg_load_all() +{ + g_cfg.speed = readDoubleFromFile(speed_file); + g_cfg.foresee = readDoubleFromFile(foresee_file); + g_cfg.zebrasee = readDoubleFromFile(zebrasee_file); + g_cfg.deadband = readDoubleFromFile(deadband_file); + g_cfg.steer_gain = readDoubleFromFile(steer_gain_file); + g_cfg.center_bias = readDoubleFromFile(center_bias_file); + g_cfg.start = readFlag(start_file); + g_cfg.showImg = readFlag(showImg_file); + g_cfg.debug = readFlag(debug_file); +} diff --git a/src/image_cv.cpp b/src/image_cv.cpp index ee04ce1..3a8f5fc 100644 --- a/src/image_cv.cpp +++ b/src/image_cv.cpp @@ -15,11 +15,10 @@ // 全局图像变量 // ============================================================ -cv::Mat raw_frame; // 摄像头原始帧 (320×240, BGR) -cv::Mat grayFrame; // 灰度图 (未在当前管线中使用，保留) -cv::Mat binarizedFrame; // HSV 双通道 Otsu 二值化结果 -cv::Mat morphologyExFrame; // 形态学开运算后的图像 -cv::Mat track; // 洪泛填充后的赛道区域蒙版 +cv::Mat raw_frame; +cv::Mat binarizedFrame; +cv::Mat morphologyExFrame; +cv::Mat track; // ============================================================ // 边界线数组 — 左/右边缘和中线的逐行列坐标 @@ -28,12 +27,9 @@ cv::Mat track; // 洪泛填充后的赛道区域蒙版 // 列坐标范围: 0 ~ line_tracking_width-1 (0~79) // 未检测到边界 = -1 // ============================================================ -std::vector left_line; // 左边缘列号 (逐行) -std::vector right_line; // 右边缘列号 (逐行) -std::vector mid_line; // 中线列号 = (left+right)/2 -std::vector left_line_filtered; // 左边缘 EMA 滤波结果 -std::vector right_line_filtered; // 右边缘 EMA 滤波结果 -std::vector mid_line_filtered; // 中线滤波结果 +std::vector left_line; +std::vector right_line; +std::vector mid_line; int line_tracking_width; // 处理宽度 = 80 int line_tracking_height; // 处理高度 = 60 @@ -118,7 +114,7 @@ cv::Mat find_road(cv::Mat &frame) // 1. 形态学开运算去噪 // MORPH_CROSS: 十字形结构元素，2×2 // MORPH_OPEN: 先腐蚀(去除小白点) 再膨胀(恢复区域尺寸) - cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2)); + static cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2)); cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel); // 2. 创建洪泛填充蒙版 @@ -150,7 +146,7 @@ cv::Mat find_road(cv::Mat &frame) // 7. 从蒙版提取赛道区域 // mask 的外扩边框(±1) 用于 floodFill 的内部计算，实际区域在(1,1)起 // ROI 裁掉边框后即为赛道主体蒙版 - cv::Mat outputImage = cv::Mat::zeros(line_tracking_width, line_tracking_height, CV_8UC1); + cv::Mat outputImage = cv::Mat::zeros(line_tracking_height, line_tracking_width, CV_8UC1); mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage); return outputImage; @@ -191,16 +187,10 @@ void image_main() left_line.clear(); right_line.clear(); mid_line.clear(); - left_line_filtered.clear(); - right_line_filtered.clear(); - mid_line_filtered.clear(); left_line.resize(line_tracking_height, -1); right_line.resize(line_tracking_height, -1); mid_line.resize(line_tracking_height, -1); - left_line_filtered.resize(line_tracking_height, -1); - right_line_filtered.resize(line_tracking_height, -1); - mid_line_filtered.resize(line_tracking_height, -1); // ── 5. 逐行最长连续段搜索 ────────────────────────── // @@ -270,28 +260,10 @@ void image_main() } } - // ── 6. 中线计算 + 丢线补全 → 自底向上 EMA 滤波 ───── - // - // 核心逻辑（从底部 row=59 往上到 row=10）: - // - // 6a. 如果当前行左右边界均有效 → 中线 = (left + right) / 2 - // - // 6b. 如果当前行丢线（左右均无效）: - // 用下一行(row+1)的中线补全: - // → 中线下半区: 虚拟右边界在右边缘，左边界 = 下行中线 - // → 中线上半区: 虚拟左边界在左边缘，右边界 = 下行中线 - // 自动适应赛道偏左还是偏右的情况 - // - // 6c. EMA 滤波 (指数移动平均): - // row 本身的值权重 = a (0.4), 下行滤波值权重 = 1-a (0.6) - // 从底部往顶部递推: 近处(底部)值稳定，远处(顶部)靠递推外推 - // a=0.4 → 近处值占主导，但保留过去趋势的惯性 - // - double a = 0.4; // EMA 系数: 平衡当前测量与历史递推 - + // ── 6. 中线计算 + 丢线补全 ───── for (int row = line_tracking_height - 1; row >= 10; --row) { - // ── 6b. 丢线补全 ────────────────────────────── + // ── 6a. 丢线补全 ────────────────────────────── if (left_line[row] == -1 && right_line[row] == -1) { // 当前行完全丢线: 用下行(row+1)的中线来虚拟补线 @@ -317,29 +289,5 @@ void image_main() // 正常行: 中线 = 左右边界中点 mid_line[row] = (left_line[row] + right_line[row]) / 2; } - - // ── 6c. EMA 滤波 (自底向上) ──────────────────── - if (row == line_tracking_height - 1) - { - // 最底行(最近处): 无下行参考，直接使用原始值 - left_line_filtered[row] = left_line[row]; - right_line_filtered[row] = right_line[row]; - mid_line_filtered[row] = mid_line[row]; - } - else - { - // 公式: filtered[row] = a * raw[row] + (1-a) * filtered[row+1] - // a=0.4: 40% 当前行实测值 + 60% 下行滤波值的递推 - // 效果: 近处值稳定，越往远处越靠递推，杜绝抖动的概率传播 - left_line_filtered[row] = a * left_line[row] - + (1 - a) * left_line_filtered[row + 1]; - right_line_filtered[row] = a * right_line[row] - + (1 - a) * right_line_filtered[row + 1]; - - // 中线的滤波值为左右滤波边界的均值（不是对原始中线做 EMA） - // 即: 先对左右边界各自滤波, 再求平均 → 减少中线突跳 - // 原注释行: mid_line_filtered[row] = a*mid_line[row] + (1-a)*mid_line_filtered[row+1] - mid_line_filtered[row] = (left_line_filtered[row] + right_line_filtered[row]) / 2.0; - } } } diff --git a/src/model_v10.cpp b/src/model_v10.cpp index 72f4a3c..ca993c4 100644 --- a/src/model_v10.cpp +++ b/src/model_v10.cpp @@ -495,17 +495,10 @@ static void init_lut() { } // ============================================================ -// 前向推理 +// 前向推理 — 模型计算体 (preproc 已由调用方完成) // ============================================================ -static void forward(const uint8* bgr) { +static void model_compute() { float* in = m->preproc; - for (int c=0; c<3; ++c) { - int sc=2-c; float* ch=in+c*120*160; - for (int y=0; y<120; ++y) { - const uint8* row=bgr+y*160*3; - for (int x=0; x<160; ++x) ch[y*160+x]=g_lut[row[x*3+sc]]; - } - } // stem: 3→6, s2 (fused) conv_bn_relu(m->stem, in, wf("s.0.weight"), nullptr, @@ -528,6 +521,41 @@ static void forward(const uint8* bgr) { conv2d(m->sz, m->sh, wf("sz.weight"), wf("sz.bias"), 15, 20, 24, 2, 1, 1, 1); } +// ── 自写 640×480 → 3×120×160 float planar (4×4 box avg + BGR→RGB + /255) ── +static void preproc_640(const uint8* bgr) { + float* in = m->preproc; + const int sw=640, sh=480; + for(int c=0;c<3;++c){ + int sc=2-c; + float* ch = in + c*120*160; + for(int y=0;y<120;++y){ + int iy=y*4; + for(int x=0;x<160;++x){ + int ix=x*4, sum=0; + for(int dy=0;dy<4;++dy) + for(int dx=0;dx<4;++dx) + sum += bgr[(iy+dy)*sw*3 + (ix+dx)*3 + sc]; + ch[y*160+x] = (float)sum * (1.0f/(16.0f*255.0f)); + } + } + } +} + +// ── 原版: 160×120 BGR → 3×120×160 float planar ── +static void preproc_160(const uint8* bgr) { + float* in = m->preproc; + for (int c=0; c<3; ++c) { + int sc=2-c; float* ch=in+c*120*160; + for (int y=0; y<120; ++y) { + const uint8* row=bgr+y*160*3; + for (int x=0; x<160; ++x) ch[y*160+x]=g_lut[row[x*3+sc]]; + } + } +} + +// ── 内部: preproc + compute ── +static void forward(const uint8* bgr) { preproc_160(bgr); model_compute(); } + // ============================================================ // 接口 // ============================================================ @@ -555,7 +583,13 @@ bool model_v10_init(const char* path) { } int model_v10_detect(const uint8* bgr, int w, int h, DetectBoxV10* boxes, int max, const float* thresh) { - if(!g_rdy||w!=160||h!=120) return 0; + if(!g_rdy) return 0; + if(w==640 && h==480) { + preproc_640(bgr); + model_compute(); + return decode(boxes,max,thresh); + } + if(w!=160||h!=120) return 0; forward(bgr); return decode(boxes,max,thresh); }