Fix vision pipeline: search-first IPM, IMU non-blocking, LCD inverse IPM, servo direction, model WIP

model/model.cpp

@@ -3,6 +3,7 @@
 #include <cstdlib>
 #include <cstring>
 #include <cmath>
+#include <new>
 #include <algorithm>
 
 // ============================================================
@@ -18,7 +19,7 @@ struct M {
     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
+    float dw_out[16*60*80];  // max(block1 skip 16×60×80, other dw)
 };
 static M* m = nullptr;
 
@@ -220,8 +221,8 @@ static int decode(DetectBox* boxes, int max, float th)
 // ============================================================
 static void forward(const uint8* bgr)
 {
-    // 预处理 BGR→RGB float [0,1], CHW
-    float in[3*120*160];
+    // 预处理 BGR→RGB float [0,1], CHW (堆分配, 避免栈溢出)
+    float* in = new float[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) {
@@ -251,6 +252,8 @@ static void forward(const uint8* bgr)
     // 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);
+
+    delete[] in;
 }
 
 // ============================================================
@@ -269,7 +272,9 @@ bool model_init(const char* path) {
         t.d=new float[tot]; fread(t.d,4,tot,f);
     }
     fclose(f);
-    m=new M(); std::memset(m,0,sizeof(M));
+    m=new (std::nothrow) M();
+    if (!m) { printf("[MODEL] OOM: 无法分配推理内存\n"); fclose(f); delete[] gw; gw=nullptr; return false; }
+    std::memset(m,0,sizeof(M));
     g_rdy=true; printf("[MODEL] load ok: %d layers\n",gn); return true;
 }
 

model/test_model.cpp

@@ -0,0 +1,30 @@
+#include "model/model.hpp"
+#include <cstdio>
+#include <cstring>
+#include <cstdlib>
+
+int main()
+{
+    if (!model_init("./model/nanodet.bin")) {
+        printf("model_init failed\n");
+        return 1;
+    }
+
+    // 造一张假 160×120×3 测试图
+    uint8_t* img = new uint8_t[160*120*3];
+    std::memset(img, 128, 160*120*3);
+
+    DetectBox boxes[16];
+    for (int i = 0; i < 5; ++i) {
+        int n = model_detect(img, 160, 120, boxes, 16, 0.3f);
+        printf("run %d: %d detections\n", i, n);
+        for (int j = 0; j < n; ++j)
+            printf("  cls=%d conf=%.3f xy=(%.0f,%.0f)\n",
+                   boxes[j].cls, boxes[j].conf, boxes[j].cx, boxes[j].cy);
+    }
+
+    delete[] img;
+    model_deinit();
+    printf("OK\n");
+    return 0;
+}