diff --git a/.gitignore b/.gitignore index e2b639e..6f937ac 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,5 @@ smartcar_framework bench/bench .vscode/ *~ +**/__pycache__/ +*.pyc diff --git a/ctl.sh b/ctl.sh index 809d3b2..5ebf023 100644 --- a/ctl.sh +++ b/ctl.sh @@ -47,6 +47,8 @@ init_pins() { echo 0 > "$DIR/mortor_kd" 2>/dev/null echo 0 > "$DIR/start" 2>/dev/null echo 1 > "$DIR/showImg" 2>/dev/null + echo 60 > "$DIR/vision_fps" 2>/dev/null + echo 20 > "$DIR/model_fps" 2>/dev/null echo "[framework] 引脚初始化完成" } diff --git a/model/export_weights.py b/model/export_weights.py new file mode 100644 index 0000000..90d189b --- /dev/null +++ b/model/export_weights.py @@ -0,0 +1,51 @@ +""" +NanoDetHeatV4 权重导出: PyTorch. pth → 二进制 +""" +import sys, struct, os +sys.path.insert(0, os.path.dirname(__file__)) +import torch +import numpy as np +from model_heat_v4 import NanoDetHeatV4 + +def export(pth_path, out_path): + model = NanoDetHeatV4(num_classes=3) + ckpt = torch.load(pth_path, map_location='cpu', weights_only=False) + if 'model' in ckpt: + ckpt = ckpt['model'] + model.load_state_dict(ckpt, strict=False) + model.eval() + + layers = {} + for name, param in model.named_parameters(): + layers[name] = param.detach().cpu().numpy().astype(np.float32) + for name, buf in model.named_buffers(): + layers[name] = buf.detach().cpu().numpy().astype(np.float32) + + total = sum(v.size for v in layers.values()) + print(f"层数: {len(layers)}, 总参数: {total:,} ≈ {total/1000:.1f}K") + + for k, v in sorted(layers.items()): + print(f" {k:50s} {list(v.shape)} {v.size:,}") + + with open(out_path, 'wb') as f: + f.write(struct.pack('I', len(layers))) + for name in sorted(layers.keys()): + data = layers[name] + nb = name.encode('utf-8') + f.write(struct.pack('I', len(nb))) + f.write(nb) + f.write(struct.pack('I', data.ndim)) + f.write(struct.pack('i' * data.ndim, *data.shape)) + f.write(data.tobytes()) + + print(f"\n导出: {out_path}") + + # 测试推理 + x = torch.randn(1, 3, 120, 160) + with torch.no_grad(): + y = model(x) + print(f"输入: {x.shape} → 输出: {y.shape}") + +if __name__ == '__main__': + base = os.path.dirname(os.path.abspath(__file__)) + export(os.path.join(base, 'best.pth.1'), os.path.join(base, 'nanodet.bin')) diff --git a/model/model.cpp b/model/model.cpp index 601114f..6ca4c17 100644 --- a/model/model.cpp +++ b/model/model.cpp @@ -1,13 +1,287 @@ #include "model.hpp" +#include +#include +#include +#include +#include -bool model_init() -{ - // TODO: 加载 TFLite 模型,初始化解释器 - return false; +// ============================================================ +// 内存预分配 +// ============================================================ +struct M { + float stem[8*60*80]; + float b1[16*60*80]; float b1_tmp[16*60*80]; + float b2[28*30*40]; float b2_dw[16*30*40]; + float b3[40*30*40]; float b3_dw[28*30*40]; + float b4[56*15*20]; float b4_dw[40*15*20]; + float b5[64*15*20]; float b5_dw[56*15*20]; + 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 +}; +static M* m = nullptr; + +// ============================================================ +// 权重表 +// ============================================================ +struct WT { char n[64]; int nd; int s[4]; float* d; }; +static WT* gw = nullptr; static int gn = 0; + +static float* wf(const char* name) { + for (int i = 0; i < gn; ++i) if (!std::strcmp(gw[i].n, name)) return gw[i].d; + printf("[MODEL] MISS %s\n", name); return nullptr; } -void model_infer(const uint8* image, int w, int h, float* output, int num_classes) +// ============================================================ +// 基础算子 +// ============================================================ +static void relu_f(float* x, int N) { for (int i=0;i=H) continue; + for (int kx=0; kx=W) continue; + s += ww[ky*K+kx] * ii[iy*W+ix]; + } + } + } + oo[y*Wo+x] = s; + } + } + } } + +static void batchnorm_f(float* x, const float* w, const float* b, + const float* mean, const float* var, int C, int N) +{ + const float eps = 1e-5f; + for (int c=0; cdw_out, in, wf(na), nullptr, H, W, iC, iC, 3, str, iC); + std::snprintf(na,128,"%s.dw.1.weight",pfx); + batchnorm_f(m->dw_out, wf(na), wf((std::snprintf(na,128,"%s.dw.1.bias",pfx),na)), + wf((std::snprintf(na,128,"%s.dw.1.running_mean",pfx),na)), + wf((std::snprintf(na,128,"%s.dw.1.running_var",pfx),na)), iC, iC*No); + relu_f(m->dw_out, iC*No); + + // pointwise conv + BN + ReLU (to output) + std::snprintf(na,128,"%s.pw.0.weight",pfx); + conv2d(o, m->dw_out, wf(na), nullptr, Ho, Wo, iC, oC, 1, 1, 1); + std::snprintf(na,128,"%s.pw.1.weight",pfx); + batchnorm_f(o, wf(na), wf((std::snprintf(na,128,"%s.pw.1.bias",pfx),na)), + wf((std::snprintf(na,128,"%s.pw.1.running_mean",pfx),na)), + wf((std::snprintf(na,128,"%s.pw.1.running_var",pfx),na)), oC, oC*No); + relu_f(o, oC*No); + + // skip (no ReLU) + float* skip = m->dw_out; // reuse buffer + if (str==1 && iC==oC) { + std::memcpy(skip, in, iC*H*W*4); + } else { + std::snprintf(na,128,"%s.skip.0.weight",pfx); + conv2d(skip, in, wf(na), nullptr, H, W, iC, oC, 1, str, 1); + std::snprintf(na,128,"%s.skip.1.weight",pfx); + batchnorm_f(skip, wf(na), wf((std::snprintf(na,128,"%s.skip.1.bias",pfx),na)), + wf((std::snprintf(na,128,"%s.skip.1.running_mean",pfx),na)), + wf((std::snprintf(na,128,"%s.skip.1.running_var",pfx),na)), oC, oC*No); + } + + // add skip + for (int i=0; icls, 4, N); + int cnt=0; + + for (int gy=0; gycls[c*N+idx]; if (s>bs) {bs=s; bc=c;} } + if (bs=OH||nx<0||nx>=OW) continue; + if (m->cls[bc*N+ny*OW+nx] > bs) pk=false; + } + if (!pk) continue; + + float pw=m->sz[0*N+idx], ph=m->sz[1*N+idx]; + boxes[cnt].cls=bc; boxes[cnt].conf=bs; + boxes[cnt].cx=((float)gx+0.5f)*ST; boxes[cnt].cy=((float)gy+0.5f)*ST; + boxes[cnt].w=pw*RS[bc][0]; boxes[cnt].h=ph*RS[bc][1]; + cnt++; + } + return cnt; +} + +// ============================================================ +// 前向推理 +// ============================================================ +static void forward(const uint8* bgr) +{ + // 预处理 BGR→RGB float [0,1], CHW + float in[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) { + const uint8* row=bgr+y*160*3; + for (int x=0; x<160; ++x) ch[y*160+x]=(float)row[x*3+sc]/255.0f; + } + } + + // stem + conv_bn(m->stem, in, 120,160, wf("stem.0.weight"),nullptr, + wf("stem.1.weight"),wf("stem.1.bias"), + wf("stem.1.running_mean"),wf("stem.1.running_var"), + 3,8,3,2,1,true); + + se_block(m->b1, m->stem, 8,16, 60,80, 1,"block1"); + se_block(m->b2, m->b1, 16,28, 60,80, 2,"block2"); + se_block(m->b3, m->b2, 28,40, 30,40, 1,"block3"); + se_block(m->b4, m->b3, 40,56, 30,40, 2,"block4"); + se_block(m->b5, m->b4, 56,64, 15,20, 1,"block5"); + + // shared + conv_bn(m->sh, m->b5, 15,20, wf("shared.0.weight"),nullptr, + wf("shared.1.weight"),wf("shared.1.bias"), + wf("shared.1.running_mean"),wf("shared.1.running_var"), + 64,24,1,1,1,true); + + // 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); +} + +// ============================================================ +// 接口 +// ============================================================ +static bool g_rdy = false; + +bool model_init(const char* path) { + FILE* f=fopen(path,"rb"); + if(!f){ printf("[MODEL] open fail: %s\n",path); return false; } + fread(&gn,sizeof(int),1,f); + gw=new WT[gn]; + for(int i=0;i #include @@ -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 g_sched_running{false}; @@ -45,34 +45,58 @@ 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::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 g_det_idx{0}; // 当前写入索引 +static std::atomic g_det_new{false}; +static std::atomic 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 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(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"); diff --git a/types.hpp b/types.hpp index 8f5d947..1e228d8 100644 --- a/types.hpp +++ b/types.hpp @@ -107,6 +107,8 @@ constexpr const char* CFG_MOTOR_KI = "./mortor_ki";// 电机 PID 积分系数 constexpr const char* CFG_MOTOR_KD = "./mortor_kd";// 电机 PID 微分系数 constexpr const char* CFG_DEADBAND = "./deadband"; // 舵机死区 (归一化偏差阈值) constexpr const char* CFG_STEER_GAIN= "./steer_gain";// 舵机增益 (放大系数) +constexpr const char* CFG_VISION_FPS= "./vision_fps";// 视觉帧率 (默认 60) +constexpr const char* CFG_MODEL_FPS = "./model_fps"; // 模型帧率 (默认 20) // ========================================================= // 赛道场景枚举 — 按危险程度从低到高排列