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

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