diff --git a/.gitignore b/.gitignore index 6f937ac..0a05b66 100644 Binary files a/.gitignore and b/.gitignore differ diff --git a/control/imu.cpp b/control/imu.cpp index 443aa81..2378b0c 100644 --- a/control/imu.cpp +++ b/control/imu.cpp @@ -3,14 +3,19 @@ #include #include #include +#include #include YawTracker::YawTracker() : _fd(-1), _yaw(0), _unbounded_yaw(0), _gyro_z(0), _gyro_bias(0), _ready(false) {} bool YawTracker::begin(const char* device, int baud) { - _fd = open(device, O_RDWR | O_NOCTTY); - if (_fd < 0) return false; + _fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (_fd < 0) { + printf("[IMU] 无法打开 %s, 继续无IMU运行\n", device); + _ready = false; + return false; + } termios opt; tcgetattr(_fd, &opt); @@ -30,16 +35,18 @@ bool YawTracker::begin(const char* device, int baud) void YawTracker::calibrate(int samples) { float sum = 0; - for (int i = 0; i < samples; ++i) + int valid = 0, fail = 0; + for (int i = 0; i < samples && fail < 10; ++i) { int ret = _parseJY62(); - if (ret > 0) sum += _gyro_z; - usleep(5000); + if (ret > 0) { sum += _gyro_z; ++valid; fail = 0; } + else { ++fail; } + usleep(2000); } - _gyro_bias = sum / samples; - _yaw = 0; - _unbounded_yaw = 0; - _ready = true; + _gyro_bias = valid > 0 ? sum / valid : 0; + _yaw = 0; _unbounded_yaw = 0; + _ready = (valid > 10); + if (!_ready) printf("[IMU] 校准失败 (仅 %d/%d 样本), 继续无IMU运行\n", valid, samples); } void YawTracker::update(float dt) @@ -60,18 +67,20 @@ int YawTracker::_parseJY62() { uint8 buf[11]; int total = 0; - while (total < 11) + for (int tries = 0; total < 11 && tries < 20; ++tries) { int n = read(_fd, buf + total, 11 - total); - if (n <= 0) return -1; + if (n < 0) { usleep(2000); continue; } // EAGAIN, 等 2ms + if (n == 0) break; total += n; } + if (total < 11) return -1; - while (buf[0] != 0x55 && total > 10) + for (int tries = 0; buf[0] != 0x55 && total > 10 && tries < 10; ++tries) { memmove(buf, buf + 1, 10); int n = read(_fd, buf + 10, 1); - if (n <= 0) return -1; + if (n <= 0) { usleep(1000); continue; } } if (buf[0] != 0x55) return -1; diff --git a/control/servo.cpp b/control/servo.cpp index 43f156f..5a89b4b 100644 --- a/control/servo.cpp +++ b/control/servo.cpp @@ -4,13 +4,31 @@ #include #include -static PWM* servo_pwm = nullptr; -static PID* servo_pid = nullptr; +// ── 全局对象 ────────────────────────────────────────── +static PWM* servo_pwm = nullptr; // pwmchip1/pwm0, 3ms 周期 +static PID* servo_pid = nullptr; // 位置式 PID, 偏差→脉宽偏移量 (ns) static bool serv_initialized = false; -static float g_deadband = 0.03f; -static float g_steer_gain = 1.5f; +// ── 运行时热调参数 ────────────────────────────────── +// 这些值会被 scheduler.cpp 每 1 秒从文件热读覆盖 +static float g_deadband = 0.03f; // 死区 (归一化偏差, 默认 ±0.03) +static float g_steer_gain = 1.5f; // 转向增益 (偏差放大系数) +// ============================================================ +// servo_init — 初始化舵机 PWM + PID +// +// 调用时机: scheduler_init() 启动时调用一次 +// +// 硬件: pwmchip1/pwm0 +// 周期 = 3ms (333Hz) — 舵机标准周期 +// 中位 = 1.52ms — 前轮摆正 +// 行程 = 1.52 ± 0.21ms — 约 ±45° 物理摆角 +// +// PID: 位置式 +// Kp=400000, Ki=200000, Kd=0 +// 输出限幅 = ±SERVO_MAX_DELTA_NS (±210000ns = ±0.21ms) +// 这组参数等效于线性增益 (因为 Ki 很大, 积分瞬间到位) +// ============================================================ void servo_init() { if (serv_initialized) return; @@ -26,10 +44,33 @@ void servo_init() serv_initialized = true; } -void servo_set_deadband(float v) { g_deadband = std::max(0.0f, v); } +// ── 热调接口 ───────────────────────────────────────── +void servo_set_deadband(float v) { g_deadband = std::max(0.0f, v); } void servo_set_steer_gain(float v) { g_steer_gain = std::max(0.1f, v); } void servo_set_pid(double kp, double ki, double kd) { if (servo_pid) servo_pid->setPID(kp, ki, kd); } +// ============================================================ +// servo_set_angle — 偏差 → 舵机脉宽 (核心控制回路) +// +// 调用频率: scheduler.cpp sched_control() 每 5ms 一次 +// +// 输入: deviation — 归一化中线偏差 (-1=偏左极限, 0=正中, +1=偏右极限) +// 由 strategy/deviation.cpp 的 calc_deviation() 计算 +// +// 处理流程: +// 1. 钳位偏差至 [-1, +1] +// 2. 死区: |deviation| < g_deadband → 直接回中, 跳过 PID +// 典型值 g_deadband=0.03, 即偏差 < 3% 时不转向 +// 3. 增益缩放: deviation *= g_steer_gain +// 典型值 g_steer_gain=1.5, 把微小偏差放大供 PID 处理 +// 4. PID 计算: out = PID(deviation) +// 位置式 PID, 输出为脉宽偏移量 (ns) +// 5. 叠加中位: duty_ns = 1520000 + out +// 正偏差(偏右) → out>0 → 脉宽>中位 → 舵机右转 +// 负偏差(偏左) → out<0 → 脉宽<中位 → 舵机左转 +// 6. 限幅: 1.52 ± 0.21ms (1310000ns ~ 1730000ns) +// 7. 写入 PWM sysfs +// ============================================================ void servo_set_angle(float deviation) { if (!serv_initialized) return; @@ -42,7 +83,7 @@ void servo_set_angle(float deviation) } float scaled = deviation * g_steer_gain; - float out = servo_pid->update(scaled); + float out = -servo_pid->update(scaled); // 负号: 偏差正(右偏)→输出负→左转回正 float duty_ns = SERVO_CENTER_NS + out; duty_ns = std::clamp(duty_ns, static_cast(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS), @@ -50,6 +91,7 @@ void servo_set_angle(float deviation) servo_pwm->setDutyCycle(static_cast(duty_ns)); } +// servo_center — 紧急回中 (停止时调用) void servo_center() { if (serv_initialized) diff --git a/ctl.sh b/ctl.sh index 5ebf023..cbf7509 100644 --- a/ctl.sh +++ b/ctl.sh @@ -36,7 +36,7 @@ init_pins() { echo 1 > /sys/class/pwm/pwmchip1/pwm0/enable 2>/dev/null echo 1520000 > /sys/class/pwm/pwmchip1/pwm0/duty_cycle 2>/dev/null - echo 11 > "$DIR/speed" 2>/dev/null + echo 12 > "$DIR/speed" 2>/dev/null echo 0.03 > "$DIR/deadband" 2>/dev/null echo 1.5 > "$DIR/steer_gain" 2>/dev/null echo 400000 > "$DIR/kp" 2>/dev/null diff --git a/main.cpp b/main.cpp index 55c76b1..3d9aa8a 100644 --- a/main.cpp +++ b/main.cpp @@ -71,14 +71,6 @@ int main() { std::this_thread::sleep_for(std::chrono::milliseconds(500)); - // 热调参数 (读取文件) - bool running = read_config_flag(CFG_START, true); - if (!running) - { - motor_stop_all(); - servo_center(); - } - // 打印状态 static int last_print = 0; if (g_print_flag.exchange(0)) diff --git a/model/model.cpp b/model/model.cpp index 6ca4c17..d9cec6d 100644 --- a/model/model.cpp +++ b/model/model.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include // ============================================================ @@ -18,7 +19,7 @@ struct M { float sh[24*15*20]; float cls[4*15*20]; float sz[2*15*20]; - float dw_out[64*60*80]; // max(8*60*80, 16*30*40, ...) = 38400 + float dw_out[16*60*80]; // max(block1 skip 16×60×80, other dw) }; static M* m = nullptr; @@ -220,8 +221,8 @@ static int decode(DetectBox* boxes, int max, float th) // ============================================================ static void forward(const uint8* bgr) { - // 预处理 BGR→RGB float [0,1], CHW - float in[3*120*160]; + // 预处理 BGR→RGB float [0,1], CHW (堆分配, 避免栈溢出) + float* in = new float[3*120*160]; for (int c=0; c<3; ++c) { int sc=2-c; float* ch=in+c*120*160; for (int y=0; y<120; ++y) { @@ -251,6 +252,8 @@ static void forward(const uint8* bgr) // heads (1x1 conv, 无 BN/ReLU) conv2d(m->cls, m->sh, wf("cls_head.weight"), wf("cls_head.bias"), 15,20, 24,4, 1,1,1); conv2d(m->sz, m->sh, wf("size_head.weight"),wf("size_head.bias"),15,20, 24,2, 1,1,1); + + delete[] in; } // ============================================================ @@ -269,7 +272,9 @@ bool model_init(const char* path) { t.d=new float[tot]; fread(t.d,4,tot,f); } fclose(f); - m=new M(); std::memset(m,0,sizeof(M)); + m=new (std::nothrow) M(); + if (!m) { printf("[MODEL] OOM: 无法分配推理内存\n"); fclose(f); delete[] gw; gw=nullptr; return false; } + std::memset(m,0,sizeof(M)); g_rdy=true; printf("[MODEL] load ok: %d layers\n",gn); return true; } diff --git a/model/test_model b/model/test_model new file mode 100644 index 0000000..faa2b6c Binary files /dev/null and b/model/test_model differ diff --git a/model/test_model.cpp b/model/test_model.cpp new file mode 100644 index 0000000..fa2e258 --- /dev/null +++ b/model/test_model.cpp @@ -0,0 +1,30 @@ +#include "model/model.hpp" +#include +#include +#include + +int main() +{ + if (!model_init("./model/nanodet.bin")) { + printf("model_init failed\n"); + return 1; + } + + // 造一张假 160×120×3 测试图 + uint8_t* img = new uint8_t[160*120*3]; + std::memset(img, 128, 160*120*3); + + DetectBox boxes[16]; + for (int i = 0; i < 5; ++i) { + int n = model_detect(img, 160, 120, boxes, 16, 0.3f); + printf("run %d: %d detections\n", i, n); + for (int j = 0; j < n; ++j) + printf(" cls=%d conf=%.3f xy=(%.0f,%.0f)\n", + boxes[j].cls, boxes[j].conf, boxes[j].cx, boxes[j].cy); + } + + delete[] img; + model_deinit(); + printf("OK\n"); + return 0; +} diff --git a/scheduler.cpp b/scheduler.cpp index 6b36bf2..0c93943 100644 --- a/scheduler.cpp +++ b/scheduler.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -45,7 +46,7 @@ std::atomic g_print_flag{0}; static int g_timer_fd = -1; static int g_epoll_fd = -1; static std::thread g_sched_thread; -static std::thread g_model_thread; +static pthread_t g_model_thread; static FrameBuffer* g_fb_ptr = nullptr; static int g_tick = 0; @@ -105,6 +106,7 @@ static void sched_vision() TrackInfo info; element_recognize(sr, info); + ipm_correct_lines(sr.lines); // 搜线坐标做透视矫正 fit_midline(sr.lines); info.deviation = calc_deviation(sr.lines); info.base_speed = calc_base_speed(info); @@ -117,7 +119,7 @@ static void sched_vision() bool line_ok = (sr.lines.left_valid || sr.lines.right_valid); if (!line_ok) ++g_bad_frame; - // LCD 预览 — 原始摄像头画面 + 车道线 + // LCD 预览 — 摄像头画面 + 逆IPM车道线 if (read_config_flag(CFG_SHOW_IMG, false) && g_fb_ptr) { cv::Mat rgb; @@ -125,40 +127,67 @@ static void sched_vision() cv::Mat disp; cv::resize(rgb, disp, cv::Size(g_fb_ptr->width(), g_fb_ptr->height())); - int fw = g_fb_ptr->width(); - int fh = g_fb_ptr->height(); - float sx = (float)fw / IMAGE_WIDTH; - float sy = (float)fh / IMAGE_HEIGHT; + int dw = g_fb_ptr->width(); + int dh = g_fb_ptr->height(); + float dsw = (float)dw / 80.0f; // 80×60 相机坐标 → 显示 + float dsh = (float)dh / 60.0f; + float bsw = 1.0f / 2.0f; // 160×120 鸟瞰 → 80×60 + + // 逆IPM: 鸟瞰(80×60) → 相机(80×60) via g_ipm_matrix + bool ipm_ok = !g_ipm_matrix.empty() && g_ipm_matrix.type() == CV_64F; + auto ipm_map = [&](float bx, float by, float& cx, float& cy) { + if (!ipm_ok) { cx = bx; cy = by; return; } + double* m = (double*)g_ipm_matrix.ptr(); + double sx = bx * bsw; + double sy = by * bsw; + double w = m[6]*sx + m[7]*sy + m[8]; + cx = (float)((m[0]*sx + m[1]*sy + m[2]) / (w + 1e-9)); + cy = (float)((m[3]*sx + m[4]*sy + m[5]) / (w + 1e-9)); + if (cx < -100 || cx > 200 || cy < -100 || cy > 200) { cx = bx; cy = by; } + }; for (int y = 0; y < IPM_ROW_COUNT; ++y) { - int dy = (int)(y * sy); if (sr.lines.left[y] > 0) - cv::line(disp, cv::Point((int)(sr.lines.left[y] * sx), dy), - cv::Point((int)(sr.lines.left[y] * sx), dy), cv::Scalar(255,0,0), 5); + { + float cx, cy; + ipm_map((float)sr.lines.left[y], (float)y, cx, cy); + int dx = (int)(cx * dsw), dy = (int)(cy * dsh); + cv::line(disp, cv::Point(dx, dy), cv::Point(dx, dy), cv::Scalar(255,0,0), 5); + } if (sr.lines.right[y] > 0) - cv::line(disp, cv::Point((int)(sr.lines.right[y] * sx), dy), - cv::Point((int)(sr.lines.right[y] * sx), dy), cv::Scalar(0,255,0), 5); + { + float cx, cy; + ipm_map((float)sr.lines.right[y], (float)y, cx, cy); + int dx = (int)(cx * dsw), dy = (int)(cy * dsh); + cv::line(disp, cv::Point(dx, dy), cv::Point(dx, dy), cv::Scalar(0,255,0), 5); + } if (sr.lines.mid[y] > 0) - cv::line(disp, cv::Point((int)(sr.lines.mid[y] * sx), dy), - cv::Point((int)(sr.lines.mid[y] * sx), dy), cv::Scalar(255,255,0), 5); + { + float cx, cy; + ipm_map((float)sr.lines.mid[y], (float)y, cx, cy); + int dx = (int)(cx * dsw), dy = (int)(cy * dsh); + cv::line(disp, cv::Point(dx, dy), cv::Point(dx, dy), cv::Scalar(255,255,0), 5); + } } // 帧计数闪烁点 static int blink = 0; blink = (blink + 1) % 20; - cv::circle(disp, cv::Point(10, fh - 10), 5, + cv::circle(disp, cv::Point(10, dh - 10), 5, blink < 10 ? cv::Scalar(255,255,255) : cv::Scalar(0,0,0), -1); - // 绘制模型检测框 + // 绘制模型检测框 (160×120 相机坐标 → 显示) + float bsx = (float)dw / 160.0f; + float bsy = (float)dh / 120.0f; int di = g_det_idx.load(); const DetResult& dr = g_det[di]; for (int i = 0; i < dr.count; ++i) { - int x1 = (int)((dr.boxes[i].cx - dr.boxes[i].w/2) * sx); - int y1 = (int)((dr.boxes[i].cy - dr.boxes[i].h/2) * sy); - int x2 = (int)((dr.boxes[i].cx + dr.boxes[i].w/2) * sx); - int y2 = (int)((dr.boxes[i].cy + dr.boxes[i].h/2) * sy); + int x1 = (int)((dr.boxes[i].cx - dr.boxes[i].w/2) * bsx); + int y1 = (int)((dr.boxes[i].cy - dr.boxes[i].h/2) * bsy); + int x2 = (int)((dr.boxes[i].cx + dr.boxes[i].w/2) * bsx); + int y2 = (int)((dr.boxes[i].cy + dr.boxes[i].h/2) * bsy); cv::Scalar color = (dr.boxes[i].cls == 0) ? cv::Scalar(0,0,255) : (dr.boxes[i].cls == 1) ? cv::Scalar(0,255,255) : cv::Scalar(255,0,255); @@ -168,7 +197,6 @@ static void sched_vision() g_fb_ptr->write(disp.data, g_fb_ptr->width(), g_fb_ptr->height()); } - // 截帧 if (read_config_flag(CFG_SAVE_IMG, false)) { static int save_cnt = 0; @@ -213,8 +241,8 @@ static void sched_1s() g_vision_fps = std::max(1, std::min(100, (int)vfps)); g_model_fps = std::max(1, std::min(50, (int)mfps)); - printf("[FPS] vision:%d model:%d vision_frames:%d bad_line:%d empty:%d det:%d\n", - g_vision_fps, g_model_fps, g_frame_cnt, g_bad_frame, g_empty_cnt, g_model_frame.exchange(0)); + printf("[FPS] vision:%d model:%d vision_frames:%d bad_line:%d empty:%d speed:%.0f%% yaw:%.3f\n", + g_vision_fps, g_model_fps, g_frame_cnt, g_bad_frame, g_empty_cnt, g_user_speed, g_vision_yaw); g_frame_cnt = 0; g_bad_frame = 0; g_empty_cnt = 0; double speed_val = read_config_double(CFG_SPEED, 11.0); @@ -243,7 +271,7 @@ static void sched_1s() // ========================================================= // 模型异步线程 // ========================================================= -static void model_thread_loop() +static void* model_thread_loop(void*) { printf("[MODEL] 线程启动, 目标 %d FPS\n", g_model_fps); @@ -257,17 +285,20 @@ static void model_thread_loop() cv::Mat bgr = camera_capture(); if (bgr.empty()) continue; - int wi = 1 - g_det_idx.load(); // 写到对侧缓冲区 + cv::Mat resized; + cv::resize(bgr, resized, cv::Size(160, 120)); + + int wi = 1 - g_det_idx.load(); DetResult& dr = g_det[wi]; - dr.count = model_detect(bgr.data, bgr.cols, bgr.rows, + dr.count = model_detect(resized.data, 160, 120, dr.boxes, MAX_DET, 0.7f); dr.frame_id++; g_det_idx.store(wi); - g_det_new.store(true); g_model_frame.fetch_add(1); } printf("[MODEL] 线程退出\n"); + return nullptr; } // ========================================================= @@ -315,28 +346,32 @@ static void scheduler_loop() bool scheduler_init(FrameBuffer* fb) { g_fb_ptr = fb; - + printf("[SCHED] motor...\n"); fflush(stdout); motor_init(); + printf("[SCHED] servo...\n"); fflush(stdout); servo_init(); servo_center(); motor_enable(true); + printf("[SCHED] encoder...\n"); fflush(stdout); g_enc_l = new Encoder(ENCODER_LEFT_CHANNEL, ENCODER_LEFT_DIR_GPIO, 1); g_enc_r = new Encoder(ENCODER_RIGHT_CHANNEL, ENCODER_RIGHT_DIR_GPIO, -1); + printf("[SCHED] imu...\n"); fflush(stdout); g_imu.begin(IMU_DEVICE, IMU_BAUDRATE); + printf("[SCHED] vision init...\n"); fflush(stdout); preprocess_init(); element_init(); speed_strategy_reset(); - // 模型初始化 + printf("[SCHED] model...\n"); fflush(stdout); if (model_init("./model/nanodet.bin")) printf("[SCHED] 模型已加载\n"); else printf("[SCHED] 模型加载失败, 继续无模型运行\n"); - // timerfd 5ms + printf("[SCHED] timerfd...\n"); fflush(stdout); g_timer_fd = timerfd_create(CLOCK_MONOTONIC, 0); if (g_timer_fd < 0) { printf("[SCHED] timerfd create failed\n"); return false; } @@ -352,7 +387,7 @@ bool scheduler_init(FrameBuffer* fb) ev.data.fd = g_timer_fd; epoll_ctl(g_epoll_fd, EPOLL_CTL_ADD, g_timer_fd, &ev); - printf("[SCHED] 调度器初始化完成 (5ms)\n"); + printf("[SCHED] 调度器初始化完成 (5ms)\n"); fflush(stdout); return true; } @@ -360,16 +395,24 @@ void scheduler_start() { g_sched_running.store(true); g_sched_thread = std::thread(scheduler_loop); - g_model_thread = std::thread(model_thread_loop); - printf("[SCHED] 调度器启动 (vision=%dFPS model=%dFPS)\n", g_vision_fps, g_model_fps); + + pthread_attr_t attr; + pthread_attr_init(&attr); + pthread_attr_setstacksize(&attr, 1024 * 1024); + // 模型线程暂禁用 — 待修复越界写入 bug + // pthread_create(&g_model_thread, &attr, model_thread_loop, nullptr); + g_model_thread = 0; + pthread_attr_destroy(&attr); + + printf("[SCHED] 调度器启动 (vision=%dFPS model=OFF)\n", g_vision_fps); } void scheduler_stop() { g_sched_running.store(false); - if (g_model_thread.joinable()) - g_model_thread.join(); + if (g_model_thread) + pthread_join(g_model_thread, nullptr); if (g_sched_thread.joinable()) g_sched_thread.join(); diff --git a/strategy/deviation.cpp b/strategy/deviation.cpp index 435a988..92a9b1a 100644 --- a/strategy/deviation.cpp +++ b/strategy/deviation.cpp @@ -1,8 +1,73 @@ #include "deviation.hpp" +#include "vision/preprocess.hpp" #include #include #include +// ============================================================ +// IPM 坐标矫正 — 相机透视 → 鸟瞰 +// 搜线在原始相机视角完成 (准确), 这里只对坐标做透视变换 +// ============================================================ +void ipm_correct_lines(EdgeLines& lines) +{ + if (g_ipm_matrix.empty() || g_ipm_matrix.type() != CV_64F) return; + + cv::Mat H = g_ipm_matrix.inv(); // 相机→鸟瞰 (原矩阵 map 是鸟瞰→相机) + double* m = (double*)H.ptr(); + + uint8 new_left[IPM_ROW_COUNT] = {0}; + uint8 new_right[IPM_ROW_COUNT] = {0}; + + for (int y = 0; y < IPM_ROW_COUNT; ++y) + { + if (lines.left[y] > 0) + { + double sx = lines.left[y] * 0.5; // 160→80 缩放 + double sy = y * 0.5; + double w = m[6]*sx + m[7]*sy + m[8]; + if (std::abs(w) > 1e-9) { + double bx = (m[0]*sx + m[1]*sy + m[2]) / w; + double by = (m[3]*sx + m[4]*sy + m[5]) / w; + int nx = (int)(bx * 2.0 + 0.5); // 80→160 缩放 + int ny = (int)(by * 2.0 + 0.5); + if (nx >= 0 && nx < 256 && ny >= 0 && ny < IPM_ROW_COUNT) + new_left[ny] = (uint8)nx; + } + } + if (lines.right[y] > 0) + { + double sx = lines.right[y] * 0.5; + double sy = y * 0.5; + double w = m[6]*sx + m[7]*sy + m[8]; + if (std::abs(w) > 1e-9) { + double bx = (m[0]*sx + m[1]*sy + m[2]) / w; + double by = (m[3]*sx + m[4]*sy + m[5]) / w; + int nx = (int)(bx * 2.0 + 0.5); + int ny = (int)(by * 2.0 + 0.5); + if (nx >= 0 && nx < 256 && ny >= 0 && ny < IPM_ROW_COUNT) + new_right[ny] = (uint8)nx; + } + } + } + + // 插值填充缺失行 + for (int y = 1; y < IPM_ROW_COUNT - 1; ++y) + { + if (new_left[y] == 0 && new_left[y-1] > 0 && new_left[y+1] > 0) + new_left[y] = (new_left[y-1] + new_left[y+1]) / 2; + if (new_right[y] == 0 && new_right[y-1] > 0 && new_right[y+1] > 0) + new_right[y] = (new_right[y-1] + new_right[y+1]) / 2; + } + + for (int y = 0; y < IPM_ROW_COUNT; ++y) + { + lines.left[y] = new_left[y]; + lines.right[y] = new_right[y]; + if (new_left[y] > 0 && new_right[y] > 0) + lines.mid[y] = (new_left[y] + new_right[y]) / 2; + } +} + void fit_midline(EdgeLines& lines) { static EdgeLines prev; @@ -44,8 +109,8 @@ float calc_deviation(const EdgeLines& lines) int mid_ref = IMAGE_WIDTH / 2; int total = IPM_ROW_COUNT; - int zone_far = total / 5; // 远: 上20% - int zone_mid = total * 3 / 5; // 中: 中间40% + int zone_far = total / 5; + int zone_mid = total * 3 / 5; for (int y = 0; y < total; ++y) { diff --git a/strategy/deviation.hpp b/strategy/deviation.hpp index 8fa3b5c..43bd0c0 100644 --- a/strategy/deviation.hpp +++ b/strategy/deviation.hpp @@ -4,3 +4,4 @@ float calc_deviation(const EdgeLines& lines); void fit_midline(EdgeLines& lines); +void ipm_correct_lines(EdgeLines& lines); // 相机→鸟瞰坐标矫正 diff --git a/vision/element.cpp b/vision/element.cpp index 3ed27e2..265b05e 100644 --- a/vision/element.cpp +++ b/vision/element.cpp @@ -96,7 +96,7 @@ namespace { void element_init() { - std::memset(&g_elm, 0, sizeof(g_elm)); + g_elm = ElementMachine{}; g_elm.state = TRACK_STRAIGHT; g_elm.last_state = TRACK_STRAIGHT; g_elm.normal_width = 50.0f; diff --git a/vision/preprocess.cpp b/vision/preprocess.cpp index 65fb719..fe8e23b 100644 --- a/vision/preprocess.cpp +++ b/vision/preprocess.cpp @@ -20,7 +20,7 @@ static constexpr float CAL_FAR_Y_MM = 1050.0f; // 矩形远边 Y (mm) static constexpr float CAL_HALF_W_MM = 200.0f; // 矩形半宽 (mm) static bool g_ipm_enabled = true; // 启用 IPM -static cv::Mat g_ipm_matrix; // 3×3 透视变换矩阵 +cv::Mat g_ipm_matrix; // 3×3 透视变换矩阵 (extern) void preprocess_init() {} @@ -95,21 +95,6 @@ void preprocess_run(const cv::Mat& bgr_frame) cv::resize(bgr_frame, small, cv::Size(PROC_W, PROC_H)); cv::Mat bin = track_binarize(small); - - // IPM 透视校正 (首次调用时计算矩阵) - if (g_ipm_enabled) - { - if (g_ipm_matrix.empty()) - init_ipm_calibration(); - - cv::Mat ipm_out; - cv::warpPerspective(bin, ipm_out, g_ipm_matrix, - cv::Size(PROC_W, PROC_H), - cv::INTER_LINEAR, - cv::BORDER_CONSTANT, cv::Scalar(0)); - bin = ipm_out; - } - cv::Mat track = flood_fill_track(bin); cv::Mat full; cv::resize(track, full, cv::Size(IMAGE_WIDTH, IMAGE_HEIGHT), 0, 0, cv::INTER_NEAREST); diff --git a/vision/preprocess.hpp b/vision/preprocess.hpp index 1c796ec..a62ded6 100644 --- a/vision/preprocess.hpp +++ b/vision/preprocess.hpp @@ -3,8 +3,9 @@ #include "types.hpp" void preprocess_init(); -void preprocess_run(const cv::Mat& bgr_frame); // 全彩 HSV 二值化 + IPM +void preprocess_run(const cv::Mat& bgr_frame); extern uint8 g_ipm_image[IMAGE_HEIGHT][IMAGE_WIDTH]; extern uint8 g_valid_l_bound[IMAGE_HEIGHT]; extern uint8 g_valid_r_bound[IMAGE_HEIGHT]; +extern cv::Mat g_ipm_matrix; // 80x60 鸟瞰→相机 透视矩阵 diff --git a/vision/search.cpp b/vision/search.cpp index 8c3242e..2add3be 100644 --- a/vision/search.cpp +++ b/vision/search.cpp @@ -5,7 +5,7 @@ void search_init(SearchResult& r) { - std::memset(&r, 0, sizeof(r)); + r = SearchResult{}; } void search_run(SearchResult& r)