spdis/Loongson_2k0300_SmartCar_Framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix vision pipeline: search-first IPM, IMU non-blocking, LCD inverse IPM, servo direction, model WIP

Browse Source
This commit is contained in:
spdis
2026-05-25 13:57:47 +08:00
parent 610f0a7549
commit 235ec4c41e
15 changed files with 260 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
.gitignore vendored
View File

Binary file not shown.

35
control/imu.cpp
View File

@@ -3,14 +3,19 @@
#include <unistd.h>
#include <cmath>
#include <cstring>
#include <cstdio>
#include <termios.h>
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;

52
control/servo.cpp
View File

@@ -4,13 +4,31 @@
#include <cstdio>
#include <cmath>
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_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<float>(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS),
@@ -50,6 +91,7 @@ void servo_set_angle(float deviation)
servo_pwm->setDutyCycle(static_cast<unsigned int>(duty_ns));
}
// servo_center — 紧急回中 (停止时调用)
void servo_center()
{
if (serv_initialized)

2
ctl.sh
View File

@@ -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

8
main.cpp
View File

@@ -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))

13
model/model.cpp
View File

@@ -3,6 +3,7 @@
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <new>
#include <algorithm>
// ============================================================
@@ -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;
}

BIN
model/test_model Normal file
View File

Binary file not shown.

30
model/test_model.cpp Normal file
View File

@@ -0,0 +1,30 @@
#include "model/model.hpp"
#include <cstdio>
#include <cstring>
#include <cstdlib>
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;
}

111
scheduler.cpp
View File

@@ -21,6 +21,7 @@
#include <chrono>
#include <thread>
#include <mutex>
#include <pthread.h>
#include <cstdio>
#include <sys/timerfd.h>
#include <sys/epoll.h>
@@ -45,7 +46,7 @@ std::atomic<int> 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>();
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();

69
strategy/deviation.cpp
View File

@@ -1,8 +1,73 @@
#include "deviation.hpp"
#include "vision/preprocess.hpp"
#include <algorithm>
#include <cmath>
#include <cstring>
// ============================================================
// 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<double>();
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)
{

1
strategy/deviation.hpp
View File

@@ -4,3 +4,4 @@
float calc_deviation(const EdgeLines& lines);
void fit_midline(EdgeLines& lines);
void ipm_correct_lines(EdgeLines& lines); // 相机→鸟瞰坐标矫正

2
vision/element.cpp
View File

@@ -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;

17
vision/preprocess.cpp
View File

@@ -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);

3
vision/preprocess.hpp
View File

@@ -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 鸟瞰→相机 透视矩阵

2
vision/search.cpp
View File

@@ -5,7 +5,7 @@
void search_init(SearchResult& r)
{
std::memset(&r, 0, sizeof(r));
r = SearchResult{};
}
void search_run(SearchResult& r)