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

Add async model thread, vision FPS throttle, configurable FPS, .gitignore __pycache__

Browse Source
This commit is contained in:
spdis
2026-05-25 11:04:05 +08:00
parent 28d9c6da58
commit 610f0a7549
8 changed files with 503 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

179
scheduler.cpp
View File

@@ -15,6 +15,7 @@
#include "strategy/speed.hpp"
#include "debug/draw.hpp"
#include "debug/config.hpp"
#include "model/model.hpp"
#include <atomic>
#include <chrono>
@@ -37,7 +38,6 @@ static PID g_pid_speed_r(0.6f, 0.2f, 0.0f, 0.0f, PID_INCREMENTAL, 100.0
static float g_vision_yaw = 0.0f;
static float g_speed_l = 0.0f;
static float g_speed_r = 0.0f;
static float g_base_speed = 20.0f;
static float g_user_speed = 11.0f;
static std::atomic<bool> g_sched_running{false};
@@ -45,34 +45,58 @@ 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::mutex g_vision_mutex;
static std::thread g_model_thread;
static FrameBuffer* g_fb_ptr = nullptr;
static int g_tick = 0;
static int g_frame_cnt = 0;
static int g_bad_frame = 0;
static int g_empty_cnt = 0;
static FrameBuffer* g_fb_ptr = nullptr;
// =========================================================
// 5ms 回调
// 帧率控制
// =========================================================
static int g_vision_fps = 60;
static int g_model_fps = 20;
static float g_vision_acc = 0; // 累计 ms
// =========================================================
// 模型异步 — 双缓冲检测结果
// =========================================================
static constexpr int MAX_DET = 16;
struct DetResult {
int count = 0;
DetectBox boxes[MAX_DET];
int frame_id = 0;
};
static DetResult g_det[2];
static std::atomic<int> g_det_idx{0}; // 当前写入索引
static std::atomic<bool> g_det_new{false};
static std::atomic<int> g_model_frame{0};
static int model_interval_ms() { return g_model_fps > 0 ? 1000 / g_model_fps : 50; }
static int vision_interval_ms() { return g_vision_fps > 0 ? 1000 / g_vision_fps : 17; }
// =========================================================
// 5ms 回调 — 传感器
// =========================================================
static void sched_5ms(float dt)
{
if (g_enc_l) g_enc_l->update();
if (g_enc_r) g_enc_r->update();
g_imu.update(dt);
g_speed_l = g_enc_l ? g_enc_l->getRPS() : 0;
g_speed_r = g_enc_r ? g_enc_r->getRPS() : 0;
}
static void sched_10ms(float dt)
// =========================================================
// 视觉回调 — 按 vision_fps 节流
// =========================================================
static void sched_vision()
{
// 1. 视觉处理
cv::Mat bgr = camera_capture();
if (!bgr.empty())
{
std::lock_guard<std::mutex> lock(g_vision_mutex);
preprocess_run(bgr);
SearchResult sr;
@@ -86,14 +110,10 @@ static void sched_10ms(float dt)
info.base_speed = calc_base_speed(info);
info.curvature = info.deviation;
// 十字路口锁定直行
if (element_is_cross())
info.deviation = 0.0f;
g_vision_yaw = info.deviation;
g_base_speed = info.base_speed;
info.line_valid = (sr.lines.left_valid + sr.lines.right_valid) >= 1;
bool line_ok = (sr.lines.left_valid || sr.lines.right_valid);
if (!line_ok) ++g_bad_frame;
@@ -124,23 +144,37 @@ static void sched_10ms(float dt)
cv::Point((int)(sr.lines.mid[y] * sx), dy), cv::Scalar(255,255,0), 5);
}
// 帧计数闪烁点 — 验证屏幕在刷新
// 帧计数闪烁点
static int blink = 0;
blink = (blink + 1) % 20;
cv::circle(disp, cv::Point(10, fh - 10), 5,
blink < 10 ? cv::Scalar(255,255,255) : cv::Scalar(0,0,0), -1);
// 绘制模型检测框
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);
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);
cv::rectangle(disp, cv::Point(x1,y1), cv::Point(x2,y2), color, 2);
}
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;
char fname[64];
snprintf(fname, sizeof(fname), "./capture_%04d.jpg", save_cnt++);
cv::imwrite(fname, bgr);
printf("[SAVE] %s (%dx%d)\n", fname, bgr.cols, bgr.rows);
char fn[64];
snprintf(fn, sizeof(fn), "./capture_%04d.jpg", save_cnt++);
cv::imwrite(fn, bgr);
}
++g_frame_cnt;
@@ -149,30 +183,39 @@ static void sched_10ms(float dt)
{
++g_empty_cnt;
}
// 2. 舵机转向
servo_set_angle(g_vision_yaw);
// 3. 差速辅助
float steer = g_vision_yaw;
float curve_factor = (std::abs(g_vision_yaw) < 0.1f) ? 1.0f : 0.8f;
float base_spd = g_user_speed * curve_factor;
float diff = steer * 0.15f * g_user_speed;
float pwm_l = base_spd + diff;
float pwm_r = base_spd - diff;
motor_set_pwm(MOTOR_LEFT, pwm_l);
motor_set_pwm(MOTOR_RIGHT, pwm_r);
}
// =========================================================
// 持续控制 — 每 tick 都跑 (舵机 + 差速)
// =========================================================
static void sched_control()
{
servo_set_angle(g_vision_yaw);
float steer = g_vision_yaw;
float curve_factor = (std::abs(steer) < 0.1f) ? 1.0f : 0.8f;
float base_spd = g_user_speed * curve_factor;
float diff = steer * 0.15f * g_user_speed;
motor_set_pwm(MOTOR_LEFT, base_spd + diff);
motor_set_pwm(MOTOR_RIGHT, base_spd - diff);
}
// =========================================================
// 1 秒回调 — 热读参数 + 统计
// =========================================================
static void sched_1s()
{
g_print_flag.store(1);
printf("[FPS] 视觉: %d 丢线: %d 空帧: %d\n",
g_frame_cnt, g_bad_frame, g_empty_cnt);
g_frame_cnt = 0;
g_bad_frame = 0;
g_empty_cnt = 0;
// 帧率配置
double vfps = read_config_double(CFG_VISION_FPS, 60.0);
double mfps = read_config_double(CFG_MODEL_FPS, 20.0);
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));
g_frame_cnt = 0; g_bad_frame = 0; g_empty_cnt = 0;
double speed_val = read_config_double(CFG_SPEED, 11.0);
g_user_speed = static_cast<float>(speed_val);
@@ -197,6 +240,39 @@ static void sched_1s()
}
}
// =========================================================
// 模型异步线程
// =========================================================
static void model_thread_loop()
{
printf("[MODEL] 线程启动, 目标 %d FPS\n", g_model_fps);
while (g_sched_running.load())
{
int interval = model_interval_ms();
std::this_thread::sleep_for(std::chrono::milliseconds(interval));
if (!model_ready()) continue;
cv::Mat bgr = camera_capture();
if (bgr.empty()) continue;
int wi = 1 - g_det_idx.load(); // 写到对侧缓冲区
DetResult& dr = g_det[wi];
dr.count = model_detect(bgr.data, bgr.cols, bgr.rows,
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");
}
// =========================================================
// 主调度循环
// =========================================================
static void scheduler_loop()
{
epoll_event ev;
@@ -214,7 +290,17 @@ static void scheduler_loop()
sched_5ms(dt);
if (g_tick % 2 == 0) sched_10ms(dt);
// 视觉按 FPS 节流
g_vision_acc += 5.0f;
float vis_interval = (float)vision_interval_ms();
if (g_vision_acc >= vis_interval)
{
sched_vision();
g_vision_acc -= vis_interval;
}
// 持续控制 (每 tick)
sched_control();
if (g_tick % 200 == 0) sched_1s();
@@ -226,7 +312,6 @@ static void scheduler_loop()
// =========================================================
// 初始化
// =========================================================
bool scheduler_init(FrameBuffer* fb)
{
g_fb_ptr = fb;
@@ -245,12 +330,19 @@ bool scheduler_init(FrameBuffer* fb)
element_init();
speed_strategy_reset();
// 模型初始化
if (model_init("./model/nanodet.bin"))
printf("[SCHED] 模型已加载\n");
else
printf("[SCHED] 模型加载失败, 继续无模型运行\n");
// timerfd 5ms
g_timer_fd = timerfd_create(CLOCK_MONOTONIC, 0);
if (g_timer_fd < 0) { printf("[SCHED] timerfd create failed\n"); return false; }
itimerspec its;
its.it_interval.tv_sec = 0;
its.it_interval.tv_nsec = 5 * 1000 * 1000; // 5ms
its.it_interval.tv_nsec = 5 * 1000 * 1000;
its.it_value = its.it_interval;
timerfd_settime(g_timer_fd, 0, &its, nullptr);
@@ -268,12 +360,16 @@ void scheduler_start()
{
g_sched_running.store(true);
g_sched_thread = std::thread(scheduler_loop);
printf("[SCHED] 调度器启动\n");
g_model_thread = std::thread(model_thread_loop);
printf("[SCHED] 调度器启动 (vision=%dFPS model=%dFPS)\n", g_vision_fps, g_model_fps);
}
void scheduler_stop()
{
g_sched_running.store(false);
if (g_model_thread.joinable())
g_model_thread.join();
if (g_sched_thread.joinable())
g_sched_thread.join();
@@ -283,6 +379,7 @@ void scheduler_stop()
if (g_enc_l) { delete g_enc_l; g_enc_l = nullptr; }
if (g_enc_r) { delete g_enc_r; g_enc_r = nullptr; }
model_deinit();
close(g_timer_fd);
close(g_epoll_fd);
printf("[SCHED] 调度器停止\n");