430 lines
14 KiB
C++
430 lines
14 KiB
C++
#include "scheduler.hpp"
|
||
#include "types.hpp"
|
||
#include "hal/gpio.hpp"
|
||
#include "hal/framebuffer.hpp"
|
||
#include "hal/encoder.hpp"
|
||
#include "control/pid.hpp"
|
||
#include "control/imu.hpp"
|
||
#include "control/motor.hpp"
|
||
#include "control/servo.hpp"
|
||
#include "vision/camera.hpp"
|
||
#include "vision/preprocess.hpp"
|
||
#include "vision/search.hpp"
|
||
#include "vision/element.hpp"
|
||
#include "strategy/deviation.hpp"
|
||
#include "strategy/speed.hpp"
|
||
#include "debug/draw.hpp"
|
||
#include "debug/config.hpp"
|
||
#include "model/model.hpp"
|
||
|
||
#include <atomic>
|
||
#include <chrono>
|
||
#include <thread>
|
||
#include <mutex>
|
||
#include <pthread.h>
|
||
#include <cstdio>
|
||
#include <sys/timerfd.h>
|
||
#include <sys/epoll.h>
|
||
#include <unistd.h>
|
||
|
||
// =========================================================
|
||
// 全局控制对象
|
||
// =========================================================
|
||
static YawTracker g_imu;
|
||
static Encoder* g_enc_l = nullptr;
|
||
static Encoder* g_enc_r = nullptr;
|
||
static PID g_pid_speed_l(0.6f, 0.2f, 0.0f, 0.0f, PID_INCREMENTAL, 100.0f, 50.0f);
|
||
static PID g_pid_speed_r(0.6f, 0.2f, 0.0f, 0.0f, PID_INCREMENTAL, 100.0f, 50.0f);
|
||
|
||
static float g_vision_yaw = 0.0f;
|
||
static float g_speed_l = 0.0f;
|
||
static float g_speed_r = 0.0f;
|
||
static float g_user_speed = 11.0f;
|
||
|
||
static std::atomic<bool> g_sched_running{false};
|
||
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 pthread_t 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 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;
|
||
}
|
||
|
||
// =========================================================
|
||
// 视觉回调 — 按 vision_fps 节流
|
||
// =========================================================
|
||
static void sched_vision()
|
||
{
|
||
cv::Mat bgr = camera_capture();
|
||
if (!bgr.empty())
|
||
{
|
||
preprocess_run(bgr);
|
||
|
||
SearchResult sr;
|
||
search_run(sr);
|
||
|
||
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);
|
||
info.curvature = info.deviation;
|
||
|
||
if (element_is_cross())
|
||
info.deviation = 0.0f;
|
||
|
||
g_vision_yaw = info.deviation;
|
||
bool line_ok = (sr.lines.left_valid || sr.lines.right_valid);
|
||
if (!line_ok) ++g_bad_frame;
|
||
|
||
// LCD 预览 — 摄像头画面 + 逆IPM车道线
|
||
if (read_config_flag(CFG_SHOW_IMG, false) && g_fb_ptr)
|
||
{
|
||
cv::Mat rgb;
|
||
cv::cvtColor(bgr, rgb, cv::COLOR_BGR2RGB);
|
||
cv::Mat disp;
|
||
cv::resize(rgb, disp, cv::Size(g_fb_ptr->width(), g_fb_ptr->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)
|
||
{
|
||
if (sr.lines.left[y] > 0)
|
||
{
|
||
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)
|
||
{
|
||
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)
|
||
{
|
||
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, 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) * 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);
|
||
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 fn[64];
|
||
snprintf(fn, sizeof(fn), "./capture_%04d.jpg", save_cnt++);
|
||
cv::imwrite(fn, bgr);
|
||
}
|
||
|
||
++g_frame_cnt;
|
||
}
|
||
else
|
||
{
|
||
++g_empty_cnt;
|
||
}
|
||
}
|
||
|
||
// =========================================================
|
||
// 持续控制 — 每 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);
|
||
|
||
// 帧率配置
|
||
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 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);
|
||
g_user_speed = static_cast<float>(speed_val);
|
||
|
||
if (read_config_flag(CFG_START, false))
|
||
{
|
||
double kp = read_config_double(CFG_SERVO_KP, 3.5);
|
||
double ki = read_config_double(CFG_SERVO_KI, 0.3);
|
||
double kd = read_config_double(CFG_SERVO_KD, 0.2);
|
||
servo_set_pid(kp, ki, kd);
|
||
|
||
double deadband = read_config_double(CFG_DEADBAND, 0.03);
|
||
double gain = read_config_double(CFG_STEER_GAIN, 1.5);
|
||
servo_set_deadband(static_cast<float>(deadband));
|
||
servo_set_steer_gain(static_cast<float>(gain));
|
||
|
||
double mkp = read_config_double(CFG_MOTOR_KP, 0.6);
|
||
double mki = read_config_double(CFG_MOTOR_KI, 0.2);
|
||
double mkd = read_config_double(CFG_MOTOR_KD, 0.0);
|
||
g_pid_speed_l.setPID(mkp, mki, mkd);
|
||
g_pid_speed_r.setPID(mkp, mki, mkd);
|
||
}
|
||
}
|
||
|
||
// =========================================================
|
||
// 模型异步线程
|
||
// =========================================================
|
||
static void* model_thread_loop(void*)
|
||
{
|
||
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;
|
||
|
||
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(resized.data, 160, 120,
|
||
dr.boxes, MAX_DET, 0.7f);
|
||
dr.frame_id++;
|
||
g_det_idx.store(wi);
|
||
g_model_frame.fetch_add(1);
|
||
}
|
||
printf("[MODEL] 线程退出\n");
|
||
return nullptr;
|
||
}
|
||
|
||
// =========================================================
|
||
// 主调度循环
|
||
// =========================================================
|
||
static void scheduler_loop()
|
||
{
|
||
epoll_event ev;
|
||
while (g_sched_running.load())
|
||
{
|
||
int n = epoll_wait(g_epoll_fd, &ev, 1, 10);
|
||
if (n <= 0) continue;
|
||
|
||
uint64 expirations;
|
||
read(g_timer_fd, &expirations, sizeof(expirations));
|
||
|
||
for (uint64 i = 0; i < expirations; ++i)
|
||
{
|
||
float dt = 0.005f;
|
||
|
||
sched_5ms(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();
|
||
|
||
++g_tick;
|
||
}
|
||
}
|
||
}
|
||
|
||
// =========================================================
|
||
// 初始化
|
||
// =========================================================
|
||
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");
|
||
|
||
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; }
|
||
|
||
itimerspec its;
|
||
its.it_interval.tv_sec = 0;
|
||
its.it_interval.tv_nsec = 5 * 1000 * 1000;
|
||
its.it_value = its.it_interval;
|
||
timerfd_settime(g_timer_fd, 0, &its, nullptr);
|
||
|
||
g_epoll_fd = epoll_create1(0);
|
||
epoll_event ev;
|
||
ev.events = EPOLLIN;
|
||
ev.data.fd = g_timer_fd;
|
||
epoll_ctl(g_epoll_fd, EPOLL_CTL_ADD, g_timer_fd, &ev);
|
||
|
||
printf("[SCHED] 调度器初始化完成 (5ms)\n"); fflush(stdout);
|
||
return true;
|
||
}
|
||
|
||
void scheduler_start()
|
||
{
|
||
g_sched_running.store(true);
|
||
g_sched_thread = std::thread(scheduler_loop);
|
||
|
||
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)
|
||
pthread_join(g_model_thread, nullptr);
|
||
if (g_sched_thread.joinable())
|
||
g_sched_thread.join();
|
||
|
||
motor_stop_all();
|
||
servo_center();
|
||
|
||
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");
|
||
}
|