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

.gitignore

@@ -5,3 +5,5 @@ smartcar_framework
 bench/bench
 .vscode/
 *~
+**/__pycache__/
+*.pyc

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] 引脚初始化完成"
 }
 

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

model/model.cpp

@@ -1,13 +1,287 @@
 #include "model.hpp"
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <cmath>
+#include <algorithm>
 
-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<N;++i) if (x[i]<0) x[i]=0; }
+static void sigmoid_f(float* x, int N) { for (int i=0;i<N;++i) x[i]=1.0f/(1.0f+std::exp(-x[i])); }
+
+static void conv2d(float* o, const float* in, const float* w, const float* bias,
+                   int H, int W, int iC, int oC, int K, int str, int grp)
 {
-    // TODO: 运行推理，填充 output[0..num_classes-1]
+    int Ho=H/str, Wo=W/str, pad=K/2;
-    for (int i = 0; i < num_classes; ++i) output[i] = 0.0f;
+    int icpg = iC/grp, ocpg = oC/grp;
+    for (int g=0; g<grp; ++g) {
+        for (int oc=0; oc<ocpg; ++oc) {
+            int occ = g*ocpg + oc;
+            float* oo = o + occ*Ho*Wo;
+            for (int y=0; y<Ho; ++y) for (int x=0; x<Wo; ++x) {
+                float s = bias ? bias[occ] : 0;
+                for (int ic=0; ic<icpg; ++ic) {
+                    int icc = g*icpg + ic;
+                    const float* ww = w + ((occ*iC + icc)*K*K);
+                    const float* ii = in + icc*H*W;
+                    for (int ky=0; ky<K; ++ky) {
+                        int iy = y*str + ky - pad;
+                        if (iy<0||iy>=H) continue;
+                        for (int kx=0; kx<K; ++kx) {
+                            int ix = x*str + kx - pad;
+                            if (ix<0||ix>=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; c<C; ++c) {
+        float W = w?w[c]:1, B = b?b[c]:0;
+        float iv = 1.0f/std::sqrt(var[c]+eps);
+        float* xc = x + c*N;
+        for (int i=0; i<N; ++i) xc[i] = (xc[i]-mean[c])*iv*W + B;
+    }
+}
+
+// conv + BN + optional ReLU
+static void conv_bn(float* o, const float* in, int H, int W,
+                   const float* cw, const float* cb,
+                   const float* bn_w, const float* bn_b,
+                   const float* bn_m, const float* bn_v,
+                   int iC, int oC, int K, int str, int grp, bool relu_flag)
+{
+    conv2d(o, in, cw, nullptr, H, W, iC, oC, K, str, grp);
+    int N = (H/str)*(W/str);
+    batchnorm_f(o, bn_w, bn_b, bn_m, bn_v, oC, N);
+    if (relu_flag) relu_f(o, oC*N);
+}
+
+// global avg pool
+static void gap(float* o, const float* in, int C, int H, int W) {
+    int N=H*W;
+    for (int c=0; c<C; ++c) {
+        float s=0; const float* ic=in+c*N;
+        for (int i=0; i<N; ++i) s+=ic[i];
+        o[c]=s/(float)N;
+    }
+}
+
+static void softmax_c(float* x, int C, int N) {
+    for (int i=0; i<N; ++i) {
+        float mx=-1e9f;
+        for (int c=0; c<C; ++c) mx=std::max(mx, x[c*N+i]);
+        float sum=0;
+        for (int c=0; c<C; ++c) { x[c*N+i]=std::exp(x[c*N+i]-mx); sum+=x[c*N+i]; }
+        for (int c=0; c<C; ++c) x[c*N+i]/=sum;
+    }
+}
+
+// ============================================================
+// SE-ResDW Block
+// ============================================================
+static void se_block(float* o, const float* in, int iC, int oC, int H, int W, int str, const char* pfx)
+{
+    int Ho=H/str, Wo=W/str, No=Ho*Wo;
+    char na[128];
+
+    // depthwise conv + BN + ReLU
+    std::snprintf(na,128,"%s.dw.0.weight",pfx);
+    conv2d(m->dw_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; i<oC*No; ++i) o[i] += skip[i];
+
+    // SE (输入是 o, 即 pw+skip 的和)
+    float se_avg[64]; gap(se_avg, o, oC, Ho, Wo);
+
+    int rc = oC/4;
+    float se1[16], se2[64];
+    std::snprintf(na,128,"%s.se.1.weight",pfx);
+    float* se1w=wf(na); std::snprintf(na,128,"%s.se.1.bias",pfx); float* se1b=wf(na);
+    for (int i=0; i<rc; ++i) { float s=se1b?se1b[i]:0; for (int j=0;j<oC;++j) s+=se1w[i*oC+j]*se_avg[j]; se1[i]=s; }
+    relu_f(se1, rc);
+
+    std::snprintf(na,128,"%s.se.3.weight",pfx);
+    float* se2w=wf(na); std::snprintf(na,128,"%s.se.3.bias",pfx); float* se2b=wf(na);
+    for (int i=0; i<oC; ++i) { float s=se2b?se2b[i]:0; for (int j=0;j<rc;++j) s+=se2w[i*rc+j]*se1[j]; se2[i]=s; }
+    sigmoid_f(se2, oC);
+
+    for (int c=0; c<oC; ++c) { float sc=se2[c]; float* oc=o+c*No; for (int i=0;i<No;++i) oc[i]*=sc; }
+    relu_f(o, oC*No);
+}
+
+// ============================================================
+// 检测后处理
+// ============================================================
+static constexpr int OH=15, OW=20, ST=8, NC=3;
+static constexpr float RS[3][2] = {{75.36f,62.15f},{49.79f,38.63f},{67.59f,34.94f}};
+
+static int decode(DetectBox* boxes, int max, float th)
+{
+    int N=OH*OW;
+    softmax_c(m->cls, 4, N);
+    int cnt=0;
+
+    for (int gy=0; gy<OH && cnt<max; ++gy)
+    for (int gx=0; gx<OW && cnt<max; ++gx)
+    {
+        int idx = gy*OW + gx;
+        int bc=-1; float bs=0;
+        for (int c=0; c<NC; ++c) { float s=m->cls[c*N+idx]; if (s>bs) {bs=s; bc=c;} }
+        if (bs<th) continue;
+
+        bool pk=true;
+        for (int dy=-1; dy<=1&&pk; ++dy)
+        for (int dx=-1; dx<=1&&pk; ++dx)
+        {
+            int ny=gy+dy, nx=gx+dx;
+            if (ny<0||ny>=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<gn;++i){ WT& t=gw[i]; int nl; fread(&nl,4,1,f); fread(t.n,1,nl,f); t.n[nl]=0;
+        fread(&t.nd,4,1,f); int tot=1; for(int d=0;d<t.nd;++d){ fread(&t.s[d],4,1,f); tot*=t.s[d]; }
+        for(int d=t.nd;d<4;++d) t.s[d]=1;
+        t.d=new float[tot]; fread(t.d,4,tot,f);
+    }
+    fclose(f);
+    m=new M(); std::memset(m,0,sizeof(M));
+    g_rdy=true; printf("[MODEL] load ok: %d layers\n",gn); return true;
+}
+
+int model_detect(const uint8* bgr, int w, int h, DetectBox* boxes, int max, float th) {
+    if(!g_rdy||w!=160||h!=120) return 0;
+    forward(bgr);
+    return decode(boxes, max, th);
+}
+
+void model_deinit() {
+    if(gw){ for(int i=0;i<gn;++i) delete[] gw[i].d; delete[] gw; gw=nullptr; }
+    delete m; m=nullptr; g_rdy=false;
+}
+
+bool model_ready() { return g_rdy; }

model/model.hpp

@@ -1,11 +1,31 @@
 #pragma once
 #include "types.hpp"
 
-// TFLM 模型推理接口 — 占位实现，后续接入模型
+// NanoDetHeatV4 检测结果
-// 用法:
+struct DetectBox {
-//   1. 将 .tflite 模型转为 C header 数组放入 model/ 目录
+    int   cls;           // 0=红绿灯, 1=锥桶, 2=人行道
-//   2. 实现 model_init() / model_infer()
+    float conf;          // 置信度 [0,1]
-//   3. 在 element.cpp 中调用
+    float cx, cy;        // 中心像素坐标 (160×120)
+    float w, h;          // 宽高像素
+};
 
-bool model_init();
+// 初始化模型, 加载权重
-void model_infer(const uint8* image, int w, int h, float* output, int num_classes);
+// weight_path: nanodet.bin 路径
+// 返回 true 成功
+bool model_init(const char* weight_path);
+
+// 运行推理, 返回检测框数量
+// bgr:    RGB 图像数据 (H×W×3, uint8, [0,255])
+// w, h:   图像尺寸 (必须 160×120)
+// boxes:  输出检测框数组
+// max:    最大检测框数
+// th:     置信度阈值 (推荐 0.6~0.8)
+int model_detect(const uint8* bgr, int w, int h,
+                 DetectBox* boxes, int max,
+                 float th);
+
+// 释放模型内存
+void model_deinit();
+
+// 模型就绪标志
+bool model_ready();

scheduler.cpp

@@ -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];
+            char fn[64];
-            snprintf(fname, sizeof(fname), "./capture_%04d.jpg", save_cnt++);
+            snprintf(fn, sizeof(fn), "./capture_%04d.jpg", save_cnt++);
-            cv::imwrite(fname, bgr);
+            cv::imwrite(fn, bgr);
-            printf("[SAVE] %s (%dx%d)\n", fname, bgr.cols, bgr.rows);
         }
 
         ++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;
+    double vfps = read_config_double(CFG_VISION_FPS, 60.0);
-    g_bad_frame = 0;
+    double mfps = read_config_double(CFG_MODEL_FPS,  20.0);
-    g_empty_cnt = 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");

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)
 
 // =========================================================
 // 赛道场景枚举 — 按危险程度从低到高排列