From 235ec4c41ef0a747a3a3008373f47b7aa1121cd7 Mon Sep 17 00:00:00 2001 From: spdis Date: Mon, 25 May 2026 13:57:47 +0800 Subject: [PATCH] Fix vision pipeline: search-first IPM, IMU non-blocking, LCD inverse IPM, servo direction, model WIP --- .gitignore | Bin 80 -> 116 bytes control/imu.cpp | 35 ++++++++----- control/servo.cpp | 54 +++++++++++++++++--- ctl.sh | 2 +- main.cpp | 8 --- model/model.cpp | 13 +++-- model/test_model | Bin 0 -> 21872 bytes model/test_model.cpp | 30 +++++++++++ scheduler.cpp | 111 ++++++++++++++++++++++++++++------------- strategy/deviation.cpp | 69 ++++++++++++++++++++++++- strategy/deviation.hpp | 1 + vision/element.cpp | 2 +- vision/preprocess.cpp | 17 +------ vision/preprocess.hpp | 3 +- vision/search.cpp | 2 +- 15 files changed, 260 insertions(+), 87 deletions(-) create mode 100644 model/test_model create mode 100644 model/test_model.cpp diff --git a/.gitignore b/.gitignore index 6f937aca59d0720d8df3f67e2bbd1d9c4f81c3a7..0a05b66a1c250dbb9e7cbf44c65e0ee3e9b08065 100644 GIT binary patch delta 41 lcmWG2nGm3o%aG5I!jQ_4!=TSl0;G$9WIO{xl$U{v0RYU$2v7h3 delta 4 LcmXR3m=FK}1T+Dq diff --git a/control/imu.cpp b/control/imu.cpp index 443aa81..2378b0c 100644 --- a/control/imu.cpp +++ b/control/imu.cpp @@ -3,14 +3,19 @@ #include #include #include +#include #include YawTracker::YawTracker() : _fd(-1), _yaw(0), _unbounded_yaw(0), _gyro_z(0), _gyro_bias(0), _ready(false) {} bool YawTracker::begin(const char* device, int baud) { - _fd = open(device, O_RDWR | O_NOCTTY); - if (_fd < 0) return false; + _fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (_fd < 0) { + printf("[IMU] 无法打开 %s, 继续无IMU运行\n", device); + _ready = false; + return false; + } termios opt; tcgetattr(_fd, &opt); @@ -30,16 +35,18 @@ bool YawTracker::begin(const char* device, int baud) void YawTracker::calibrate(int samples) { float sum = 0; - for (int i = 0; i < samples; ++i) + int valid = 0, fail = 0; + for (int i = 0; i < samples && fail < 10; ++i) { int ret = _parseJY62(); - if (ret > 0) sum += _gyro_z; - usleep(5000); + if (ret > 0) { sum += _gyro_z; ++valid; fail = 0; } + else { ++fail; } + usleep(2000); } - _gyro_bias = sum / samples; - _yaw = 0; - _unbounded_yaw = 0; - _ready = true; + _gyro_bias = valid > 0 ? sum / valid : 0; + _yaw = 0; _unbounded_yaw = 0; + _ready = (valid > 10); + if (!_ready) printf("[IMU] 校准失败 (仅 %d/%d 样本), 继续无IMU运行\n", valid, samples); } void YawTracker::update(float dt) @@ -60,18 +67,20 @@ int YawTracker::_parseJY62() { uint8 buf[11]; int total = 0; - while (total < 11) + for (int tries = 0; total < 11 && tries < 20; ++tries) { int n = read(_fd, buf + total, 11 - total); - if (n <= 0) return -1; + if (n < 0) { usleep(2000); continue; } // EAGAIN, 等 2ms + if (n == 0) break; total += n; } + if (total < 11) return -1; - while (buf[0] != 0x55 && total > 10) + for (int tries = 0; buf[0] != 0x55 && total > 10 && tries < 10; ++tries) { memmove(buf, buf + 1, 10); int n = read(_fd, buf + 10, 1); - if (n <= 0) return -1; + if (n <= 0) { usleep(1000); continue; } } if (buf[0] != 0x55) return -1; diff --git a/control/servo.cpp b/control/servo.cpp index 43f156f..5a89b4b 100644 --- a/control/servo.cpp +++ b/control/servo.cpp @@ -4,13 +4,31 @@ #include #include -static PWM* servo_pwm = nullptr; -static PID* servo_pid = nullptr; +// ── 全局对象 ────────────────────────────────────────── +static PWM* servo_pwm = nullptr; // pwmchip1/pwm0, 3ms 周期 +static PID* servo_pid = nullptr; // 位置式 PID, 偏差→脉宽偏移量 (ns) static bool serv_initialized = false; -static float g_deadband = 0.03f; -static float g_steer_gain = 1.5f; +// ── 运行时热调参数 ────────────────────────────────── +// 这些值会被 scheduler.cpp 每 1 秒从文件热读覆盖 +static float g_deadband = 0.03f; // 死区 (归一化偏差, 默认 ±0.03) +static float g_steer_gain = 1.5f; // 转向增益 (偏差放大系数) +// ============================================================ +// servo_init — 初始化舵机 PWM + PID +// +// 调用时机: scheduler_init() 启动时调用一次 +// +// 硬件: pwmchip1/pwm0 +// 周期 = 3ms (333Hz) — 舵机标准周期 +// 中位 = 1.52ms — 前轮摆正 +// 行程 = 1.52 ± 0.21ms — 约 ±45° 物理摆角 +// +// PID: 位置式 +// Kp=400000, Ki=200000, Kd=0 +// 输出限幅 = ±SERVO_MAX_DELTA_NS (±210000ns = ±0.21ms) +// 这组参数等效于线性增益 (因为 Ki 很大, 积分瞬间到位) +// ============================================================ void servo_init() { if (serv_initialized) return; @@ -26,10 +44,33 @@ void servo_init() serv_initialized = true; } -void servo_set_deadband(float v) { g_deadband = std::max(0.0f, v); } +// ── 热调接口 ───────────────────────────────────────── +void servo_set_deadband(float v) { g_deadband = std::max(0.0f, v); } void servo_set_steer_gain(float v) { g_steer_gain = std::max(0.1f, v); } void servo_set_pid(double kp, double ki, double kd) { if (servo_pid) servo_pid->setPID(kp, ki, kd); } +// ============================================================ +// servo_set_angle — 偏差 → 舵机脉宽 (核心控制回路) +// +// 调用频率: scheduler.cpp sched_control() 每 5ms 一次 +// +// 输入: deviation — 归一化中线偏差 (-1=偏左极限, 0=正中, +1=偏右极限) +// 由 strategy/deviation.cpp 的 calc_deviation() 计算 +// +// 处理流程: +// 1. 钳位偏差至 [-1, +1] +// 2. 死区: |deviation| < g_deadband → 直接回中, 跳过 PID +// 典型值 g_deadband=0.03, 即偏差 < 3% 时不转向 +// 3. 增益缩放: deviation *= g_steer_gain +// 典型值 g_steer_gain=1.5, 把微小偏差放大供 PID 处理 +// 4. PID 计算: out = PID(deviation) +// 位置式 PID, 输出为脉宽偏移量 (ns) +// 5. 叠加中位: duty_ns = 1520000 + out +// 正偏差(偏右) → out>0 → 脉宽>中位 → 舵机右转 +// 负偏差(偏左) → out<0 → 脉宽<中位 → 舵机左转 +// 6. 限幅: 1.52 ± 0.21ms (1310000ns ~ 1730000ns) +// 7. 写入 PWM sysfs +// ============================================================ void servo_set_angle(float deviation) { if (!serv_initialized) return; @@ -42,7 +83,7 @@ void servo_set_angle(float deviation) } float scaled = deviation * g_steer_gain; - float out = servo_pid->update(scaled); + float out = -servo_pid->update(scaled); // 负号: 偏差正(右偏)→输出负→左转回正 float duty_ns = SERVO_CENTER_NS + out; duty_ns = std::clamp(duty_ns, static_cast(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS), @@ -50,6 +91,7 @@ void servo_set_angle(float deviation) servo_pwm->setDutyCycle(static_cast(duty_ns)); } +// servo_center — 紧急回中 (停止时调用) void servo_center() { if (serv_initialized) diff --git a/ctl.sh b/ctl.sh index 5ebf023..cbf7509 100644 --- a/ctl.sh +++ b/ctl.sh @@ -36,7 +36,7 @@ init_pins() { echo 1 > /sys/class/pwm/pwmchip1/pwm0/enable 2>/dev/null echo 1520000 > /sys/class/pwm/pwmchip1/pwm0/duty_cycle 2>/dev/null - echo 11 > "$DIR/speed" 2>/dev/null + echo 12 > "$DIR/speed" 2>/dev/null echo 0.03 > "$DIR/deadband" 2>/dev/null echo 1.5 > "$DIR/steer_gain" 2>/dev/null echo 400000 > "$DIR/kp" 2>/dev/null diff --git a/main.cpp b/main.cpp index 55c76b1..3d9aa8a 100644 --- a/main.cpp +++ b/main.cpp @@ -71,14 +71,6 @@ int main() { std::this_thread::sleep_for(std::chrono::milliseconds(500)); - // 热调参数 (读取文件) - bool running = read_config_flag(CFG_START, true); - if (!running) - { - motor_stop_all(); - servo_center(); - } - // 打印状态 static int last_print = 0; if (g_print_flag.exchange(0)) diff --git a/model/model.cpp b/model/model.cpp index 6ca4c17..d9cec6d 100644 --- a/model/model.cpp +++ b/model/model.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include // ============================================================ @@ -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; } diff --git a/model/test_model b/model/test_model new file mode 100644 index 0000000000000000000000000000000000000000..faa2b6cc1a5e2cdadcdd48eb5e91449b9aeb976f GIT binary patch literal 21872 zcmeHvdw3L8n)m7I5JG6613?)KPUE%8j**}ujN@8?Ag~0Dz-%+9)7^CECf!L)8WIRf z1%t9{(BOresO_%o>^B=`n9&{48OQ1nqB8@CI-fTjX-2>s2#6!dt-jxTs!pf6i8K3s z&$G|>59i70x6XUs+j-CZROKw4Ty~4(a4-=hW~SUO<{_+VWo*9Hd~kBycs2kz7qO8n z8|f^LGxJrJ;^X`$EYSkym}HFP5Z`#oL;!Mn0f$(00tyQW_OtRV!1?gZ;fiZatbn2b z#7DNWe=zgeeI~ttNy`py7?G$Z9ifTOZSlD+J^_zdh6yOjM`fcp)shRy?Gw<0JPIEE zYtqtV`Tkg#`0Bl81|a7XFlM5efJ95-cJT4?naRKV>$WInTKc}rFHHEwf>v?aHIbTm z13OzzdSr&fH;)rm_JmH_WWF=hau)83j0XL07MFd z5Qp=p-XE9sIItJRpQs*$=^~5#(;t!Ao(O}j2T?A{BRb+GTE_05JTW&^@?e%a@e((? zg7QcgmFMKl&j!0&HY|~^9ei-imSuycI2H`?yfNU+l7||HoG6_0yJY->sJii**Ix3@ zAO4foj}=^Z$nWl0ux!wXrK^|x@{5a~f{5=79mpXanc)l&b{~rT|NR|!@}ox(*pRCEY7Y8i)eUt`{&~T$ zKiJR^T;#8TW9jdfSVP&!Ag>C zQPZr(dm3VutfINTl8{xQkiXF!2#^YyB$ajb6}7B>L9CGl=hZdDShOPASiwQ4ei5q- zMd}(WSY<;+Fw7cb4WVc~^ZSY2-xv!v#Qf1>I8{R&i={7u#og(bA;qCLM+rkr$#x@Z~*zopo$YaCX$6VQl z7h^KX8e_xL_$|VC8{R&isW!Yh{+b+9ZFn(eQ%<=JPjiO|b8L7qCsSU)h8Ob+;j3+U zF((ne-iD|7NrYw_K3^aKH5*>E1m~J4Br)`($8k3TQkzZjC4sxdUQtGk&)i}Ub?eYk&4@X|02KM*?>j(yz=q zCjA(2!ehszf_zP4Yw{i0$yTYKwoJ-VHK_`8VJB*?eAw)@czytJGSbf>8aErh5p!60 zgz>86a_(Vk`<1W}&%0S9pi3jJy_z+F_Ag_Zckd<4n;go#`z1}Dc-i+HqY)Y6)Ghgr zD-p91kI&Pk%Sh%IXMHDj8veQcHR;YK!%w=62haLgVB~D>kB*~~G3Bf;3$o!C8GezQ zlRfexy+>Xg?~z-y9(hTiN4{6>k()gacth%Il83P}&Z{1ljDg_I(j?9 z?qMp|anDX!8=kI%?99{?(qONuD@~9S48U$pmySc%@n7wb4od7Q*jZ{^WB6RC3#5rg z2E7b=IRx5Fd+p^Hy3qbobr@1lN=@1}^pgkVXmYu{*t1QE+~r}(Ty|%Fb`^WbF`Sj# z(zQqzUhkpx9?u^s4QFm!y4KUB+zOhvt}bAA1I}k#6_PRQ%&Ss!V4Kpywkh}E`6A$& zr9G^fbt~fm>-6PvlfG41lI&KF_nXE}Q@+}*lwDcCX5Z~*L+{LEbD%2+F&y8jRKYjR zdO~S)>(W|(9{U7vVSJl%uimZP7w=YVFN$XOY zlL@8D)2#&5gc9%|!sh`77*8lUm*ufnp?@6W?I=e?-KrE$1k|=Ft|CCaOL5&*z*aWm zInbpXgpI#*>|t%=h!$z|JwM`l@Nb$>z`~xb%Dte8#=De=r%Tq}bf^(es~lzBa;>LZ z-tRgUO@EFnicl$W_UN?E0Z29ARCuivcDz&T}{N!pK_(o@jEbJYVaQJE1ok-zrad5xN82 zrd|ztU;}$D-=Pg+D(sNaAH2yYnHuwGa-*urIq0Kql&=ouJBs?i2CTd|Taymo<7V!; zZg%BQ*qlP(wL?be$rcu_UTf?AJKH#<5ay9hUwD$nmpE4?PmX2 z<7S078~&);CD#VF$`LIgkGO%#4c~$vX=T#{o`$~@@+aUqjyBf05Fgr0c9CRkzAf)zXJH#bv3GC4 zw_376rxBgSrzVuBzE$r0^BF(dvW$L-dfg_Y{ZhY_Ti7-k{X<3@m(j*$v~fA4Z%ga* zA?=LRgf{3yA3KhI<6@ zzmRQ!@2O86M7#)XpxM)2V0TlVBvxs?dVeP!2^b$`u*{*+-wBu|I`Y@ z4;y8aA9Glj47+8NT}HWO*e^E+T48%DY;X0puvYJqq~;B2UEF_nqh6~uU8xM{N(Jhv z%A+er|KmR3W#DDtW#DDtWy5b9KQUhO@jZB4Y6j0*hX%do*xdv<>)@9WVRNoO1fOqu z^BX?rzv$3qG4GGKu!J34mXK%-z+9~SIG#_8Wpge^AKQn1WZe9X@0o+f6dLRI%}bzs zI?AV$p46C6w3k7CoBJ)v*z=8#Xzg?~@Aox73{o&bS!4On(I*RS=@9!$+5z0dh}#jH z;yaXPZHKZ2W7=YMhtlHNp;TzPvKz8#y>tci_tfu_)*yWnF$~{UfiIZcp;X7wUv$i8 z`VM7LV284V0V3~C#Cwz5l~HvC>^^Y?thzU@w3-Z_Tl zztj4Hv;BQHmyK~zG44O>qwx{)W!NGQa%s|h_~LHF&k&AcjS~Sy7 z`=an|EqqM={df45?0pT8d^-)1+x+}V-!I_T!|>-}`182PaW{7 z$L7;V;8SX|yWltXGSo@J&GO)jd7zNkZcz&xeUyl*spW%}ir zQ$EqY-H;*LcD<@A^_IWM&(yBFVK=pP!~0FIpR{c$`NpM|%;~3k^8;o+?RR)vG>)2j zue8y)O_~jYX17J-w9zEHxt=|+;b}qlyhXSBq>ub8eES+`9zd)>+=pmP|JH}HV-2d) znkVJAcK9tRF>fn;d>p;{U`QBzZ%8vB0SbL zuOROg%xjU3ZfPa-JpETU_UPS`@{jKkgfzwzF_C1dI`SE3i z3^L@$pzA2?n+uy7A9b@1$Z60L5?_bqFqOdv5!ih-+8eErguR zXS`w296#ZsvOJHuc&YZf^d`#Fg7Q#Vs7zlWonD^Aca~>;W_i9uy`elOA^Wc5ZuTu? z-H3Uj<=U;12WtkZ2U-KN{Co%56Etx$Yh#YlTDD{Kv8CRZb{qb^z_(+4L4DS&?<@zc zCEAY}{tX|y`MUk(A#BcE$THqFW>H^P-RX4_h2WqfqNa}JO#xSfSm7XyL ztY?MFjNg3eqcte4Es-Z1$4>J-TSz@CU5;{Z>^LDc1+XTIC$L9MD9vg@S>#D5i&;Wx zNn(!Eb>-f;uG|N|EDh)i&B?}vU-~pAsSo0r)+N~(OZvGrsT=az0)LW>2fy@vi02L0 z{z>Zm+0XLZfAoZNrBjpfd_ntE&7)+@65~BKNCo<@Z>)6XU&cp)rffW&G4!BVI;jdE}t)h9BU33G=`E1LMh_gTCeP>vrJVkM58LK^|Y*sYj)i@PT0*^r7Bm z)Vqu}AfxVO)V++lmr?gJ>Rv|O%cy%9buVXuKcpvk``eE)a9Nvirt{+er5xBNCk19LAXm?Ed5nA207SU--}#WDVn%;WTx`(Ki0u_S*CqAAjLvSht86VrL|-d)4oy zMc9vEZ*f&H*@bmwm(mo+{zU6i7OP!KiwAoa)}`E=Oepv131w*Q`Jej=sXm`Q!p{|H{OQJ-V@FP#^SZOf@V3+UJ7;0^ zD&Oj6rH{E;1KQ2w(r=xO(q`wNx7_SD)EoJr6Y_r9`kM45AlkxJ?Sph_C)(|^8}l8b zF{ja3m%w?DzxP%W&4Q!|->UF{GJ0-7M?ZhM)KGx4X@{bAKkSr*?ZI)Ikz+-%_<7p4*PkrU9M4NM=Zw*F!8#cTP7;Xjb(HlJz#hbni2vw#$TVigdI&mxGS$ zXx%ThfuHM%Yf?@#^Z;gq&v?w_`QS>@v$c+IdIV1rJa{(zCeKg2EdL|tmR>!h z_nBoWhmIdY2WyqG!P9mEW2*NnAFrFuA4^YB{oa9fSkkKB_{Wkl{b+CfQToQCeC^G7 zDD4HkZqe&VUv$*>!8I(uGO5dRoy(jq`0(W~us=fGigiD=pN*%`eo-$21E^=%GazLf z>Uhl;zFM@AsIJSrp3ydcib&(?e(>aOZj%Op&$#)puaov0j1L~xZBO{gKzHw&9_*?bOLgJ&^jjZ zmc+sry1o}?gSLNV_(!5|{8r?HcEeWW?~@$HiwAt#q^Ni9{mk(p*3Kk@uY-sm=Pg=E z)Vunq;eQ2dpVb&s{`{ojzi{8lDvVqCH@m1l9&&!;T;-%SjIs6KoHT9@Kdiu*)?5~j__dABvR|2N^?t~Ui#+%;JDKO&^OP}t81yyXQouSP^LLOF zMf(~Bz0a4q+0Pdgus^}BA;{Y~1Lu&iHD|mg{R`UPpdhP${U$fdK|K|KFXs@>wPvt6 z1!c(Z?+7^H=l;-{Ji(?82LGU$Y|bW>bI&GYN~&x)8B&0Ap)VHx^4mM+PD)5?K0O;U ze){dK!)_KhjJ(HjPHH@2%o;S<wvWL+9c`r@x}W`ze8+>(N$>GC z?R}Td9p#NkUqWO2|H^leA9&uI??^Q6KY+Z6k7KQi=%RUM^t;m4Ur04@uCFWaLFb6j7t$cK-5}aWlcwX`T~`)+bR~W}Q{PAagttGJ z3bC$nwV}1bhaEP$1*zs&#qO<7sU_z!d4mx)>{`j$vWEFw_;4+mx{Js&2&tRnt z>xg$D>-bxrN&6&Sz8veSjoJ%Nqvd0)iEvKA%l?^^lyrsGi?9RpHO|UZjO`wrm+9s^ zlINhusQTDvJfB`3lmAnx4Rfmp^LmIGQj?Cdpgb><-k~2#FHu>BeJD-XlZUe{yoZRR zoB>@CXG;@?Oka!qZp7BE&cucj9I8RJ-g zO>IrgQyHv@RD?N}v4#b;p3B2OK=gpTA{48st8L6>%;O0~8gBwNR99Ph)8$3iR(hHj z-6UULG^VnUeiYD9xNSOP>({OOhdXB6Ryw)tF3*fwlpE4(iaz{J3+|l#S6Eyd6Bx%{90VCFkHROVA7a)%f&CI6RdW@@f`Wq!F0zXN@VmNjP)M>WS;u zHBbCTo-*Cu2(|f&FI0630c(8_`{8tY>5P z7{)HZc+xOBD81J(HUkF!Vi+C=(gzHq9x(n1=l}zs8ipIgf%}MIR1-XA7$*SZCkR*XV~NAz8f0b^08deT@7re4vHnnqKzTnz#N13K zu`EP;z+lDhM2uVCv4u@G#!3h8azvNmeWMG#C}w|2ZG7A(}?jrQ(kqieQ^imm9Nh&d(w61Z$|ufWP~i;Ft}CP zGW4bVmoJdkW>|Wn^xM*trKd_y+wCRa+zFZPVW#{SkmjLuu%~QocKNy-@_|&Io0w{r zn#wxcasgHzvq6k^bZnQ!-zh$N3Jliz&t8+gHm7{O>&d~*t6eOPUW z>R})Ci7QO`-IiXon=z$pv+tx@pmr!lauc&f`?IyX0?=QEwwAQ`X?J1|B)=5-N38tE zMP}(>bt;LjZk5(%o9$du)0_BU(2hWC1AoVrrhS?HNQgTR&wJp!2hMxoya&#E;JgRU zd*HkW&U@gz2hMxoya&#E;D5pco2_$}fCa_zNG7EJjiVs&zcQ1lv~H!pXF>6Q)fZXm z91D&VY3u)}Y2QsjUf@^sH`AgVCoF#PKW0MzS;H((jzw=+ zX<-MB_IPC#)kQ+p`1evzL#ViDoJSrzX6$vv#l_bvif25|;KvB&?|}55 zp5mSYI|==d=~S!TrsJJVwC8kuR;pd6gd)$Y^r{aIh_#fe@e+F3gN04CaN zIv%~pYS-!bflRcAbi6Bd?wgJu#NyUDS2}($-D|-6bvk|s`?OCyPT|Bo4K@ZR{xd$O zPyA5USNn0&tpZm6wuxa*oQik!i6741>l1$gOZJIRjS4C8CFuWI?5L4KSPwKT^o6JM z#=h|QW}r{}RlrNk#Wq{#@<{Sfh+o3v1%t;Lf;gUDJ`i4v^BjqvA0aw$GlM9X;0bg5 zxqf~Kc&;}?{u)kyZhg^s+gG`MZOO?z=YQM6XP(!8ZQ(P|+3~*x-2Q`By<{Oxp&trD zF#klMOp6t&&Z@3q#%0Lw z9hXEq@mciA4EYfYpLx&2eH`CA?uqwZ_mg~lcak}7bXanP9RmKVz-Qe5@I2uq$hZ2n zu=5Tq5Qd`NX1=HUqa`QvzKgxU=Sk<*;~|UQlc5)bA@}o`41B(YS2OVU@p`;~vDWl< zd!h&9F*R8}IV_c9QFEb7NC8nqRTxH%(S=dX&?%?n2S;aFWmqd&Ny znT6`2^$~pi5iS~c{kZG;Ch}L};}PqEkc!$^18(MM2u3UX;RVs?BCyy}{4;e1PX#y=7iiHH({&|f*H`%)!Y~gKX3k<2k;<#-6Bu~M@4sbc$&AVV$+wpJ{YYWG z=bt>)LQXB6$^4~1zO`h=v`NS#mQ-=V=?H!Y3G+`WyKQ1gng6z1ZkaWCwtsfX#Inhd zN|&doD!-D%x^2Wiw|K1iDNeY8#4fb1G_k16i%a|!;b1HX_TDR1(xsSpqNMTgTU638 z=UkGKo;&R}+@BJzsr4^t#K%D5{**KtetAn8hTpYuMXCRakmV=vL%g%ZgNRO z_`?xDZ?3^u1>M<#=IgJo!L>Bztum@=yQpOXyoZy7uAB zU95=9!vl(8N<$5@B8+5Fj83+J`v38f&Lc2&VGU>ov5y1f!G8A(A&Ax7c@aL&|=! zZw2Im_AeB~IgijI;2h-98Hz}Y{VgC5nhCi5WT&v-LbED7kS_(l*arjh&}@Q~e!(xw ze-HABf1*W~+Amv#npI%3AG^+!e!;&GI3h`%TUqN)?5}z7WH^lWg%kvA!Lyw|ZmlB# z%`nP>77%;_+lmL`7v&fGbU+?(1koA5&D^Ct)EpO5jsqe`W5^F{t-EL{tk=3Lp52ZrZ~+9!6)D@pzZu( zzw(}%PH(1q{U_4z;(=rd`;(Ubq-s)lOpy8&dIjP`Jdpk|0%1YHKQ04Lv=r=6v|TgD z{2ftZ!g@;~okNo>pSn@rlf(R7G0lV%Ga9VWD`;tt+86&HW|$QF1)&8q`Tq~y CXtaX> literal 0 HcmV?d00001 diff --git a/model/test_model.cpp b/model/test_model.cpp new file mode 100644 index 0000000..fa2e258 --- /dev/null +++ b/model/test_model.cpp @@ -0,0 +1,30 @@ +#include "model/model.hpp" +#include +#include +#include + +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; +} diff --git a/scheduler.cpp b/scheduler.cpp index 6b36bf2..0c93943 100644 --- a/scheduler.cpp +++ b/scheduler.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -45,7 +46,7 @@ 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::thread g_model_thread; +static pthread_t g_model_thread; static FrameBuffer* g_fb_ptr = nullptr; static int g_tick = 0; @@ -105,6 +106,7 @@ static void sched_vision() 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); @@ -117,7 +119,7 @@ static void sched_vision() bool line_ok = (sr.lines.left_valid || sr.lines.right_valid); if (!line_ok) ++g_bad_frame; - // LCD 预览 — 原始摄像头画面 + 车道线 + // LCD 预览 — 摄像头画面 + 逆IPM车道线 if (read_config_flag(CFG_SHOW_IMG, false) && g_fb_ptr) { cv::Mat rgb; @@ -125,40 +127,67 @@ static void sched_vision() cv::Mat disp; cv::resize(rgb, disp, cv::Size(g_fb_ptr->width(), g_fb_ptr->height())); - int fw = g_fb_ptr->width(); - int fh = g_fb_ptr->height(); - float sx = (float)fw / IMAGE_WIDTH; - float sy = (float)fh / IMAGE_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 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) { - int dy = (int)(y * sy); if (sr.lines.left[y] > 0) - cv::line(disp, cv::Point((int)(sr.lines.left[y] * sx), dy), - cv::Point((int)(sr.lines.left[y] * sx), dy), cv::Scalar(255,0,0), 5); + { + 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) - cv::line(disp, cv::Point((int)(sr.lines.right[y] * sx), dy), - cv::Point((int)(sr.lines.right[y] * sx), dy), cv::Scalar(0,255,0), 5); + { + 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) - cv::line(disp, cv::Point((int)(sr.lines.mid[y] * sx), dy), - cv::Point((int)(sr.lines.mid[y] * sx), dy), cv::Scalar(255,255,0), 5); + { + 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, fh - 10), 5, + 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) * 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); + 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); @@ -168,7 +197,6 @@ static void sched_vision() 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; @@ -213,8 +241,8 @@ static void sched_1s() 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)); + 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); @@ -243,7 +271,7 @@ static void sched_1s() // ========================================================= // 模型异步线程 // ========================================================= -static void model_thread_loop() +static void* model_thread_loop(void*) { printf("[MODEL] 线程启动, 目标 %d FPS\n", g_model_fps); @@ -257,17 +285,20 @@ static void model_thread_loop() cv::Mat bgr = camera_capture(); if (bgr.empty()) continue; - int wi = 1 - g_det_idx.load(); // 写到对侧缓冲区 + 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(bgr.data, bgr.cols, bgr.rows, + dr.count = model_detect(resized.data, 160, 120, 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"); + return nullptr; } // ========================================================= @@ -315,28 +346,32 @@ static void scheduler_loop() 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"); - // timerfd 5ms + 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; } @@ -352,7 +387,7 @@ bool scheduler_init(FrameBuffer* fb) ev.data.fd = g_timer_fd; epoll_ctl(g_epoll_fd, EPOLL_CTL_ADD, g_timer_fd, &ev); - printf("[SCHED] 调度器初始化完成 (5ms)\n"); + printf("[SCHED] 调度器初始化完成 (5ms)\n"); fflush(stdout); return true; } @@ -360,16 +395,24 @@ void scheduler_start() { g_sched_running.store(true); g_sched_thread = std::thread(scheduler_loop); - g_model_thread = std::thread(model_thread_loop); - printf("[SCHED] 调度器启动 (vision=%dFPS model=%dFPS)\n", g_vision_fps, g_model_fps); + + 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.joinable()) - g_model_thread.join(); + if (g_model_thread) + pthread_join(g_model_thread, nullptr); if (g_sched_thread.joinable()) g_sched_thread.join(); diff --git a/strategy/deviation.cpp b/strategy/deviation.cpp index 435a988..92a9b1a 100644 --- a/strategy/deviation.cpp +++ b/strategy/deviation.cpp @@ -1,8 +1,73 @@ #include "deviation.hpp" +#include "vision/preprocess.hpp" #include #include #include +// ============================================================ +// IPM 坐标矫正 — 相机透视 → 鸟瞰 +// 搜线在原始相机视角完成 (准确), 这里只对坐标做透视变换 +// ============================================================ +void ipm_correct_lines(EdgeLines& lines) +{ + if (g_ipm_matrix.empty() || g_ipm_matrix.type() != CV_64F) return; + + cv::Mat H = g_ipm_matrix.inv(); // 相机→鸟瞰 (原矩阵 map 是鸟瞰→相机) + double* m = (double*)H.ptr(); + + uint8 new_left[IPM_ROW_COUNT] = {0}; + uint8 new_right[IPM_ROW_COUNT] = {0}; + + for (int y = 0; y < IPM_ROW_COUNT; ++y) + { + if (lines.left[y] > 0) + { + double sx = lines.left[y] * 0.5; // 160→80 缩放 + double sy = y * 0.5; + double w = m[6]*sx + m[7]*sy + m[8]; + if (std::abs(w) > 1e-9) { + double bx = (m[0]*sx + m[1]*sy + m[2]) / w; + double by = (m[3]*sx + m[4]*sy + m[5]) / w; + int nx = (int)(bx * 2.0 + 0.5); // 80→160 缩放 + int ny = (int)(by * 2.0 + 0.5); + if (nx >= 0 && nx < 256 && ny >= 0 && ny < IPM_ROW_COUNT) + new_left[ny] = (uint8)nx; + } + } + if (lines.right[y] > 0) + { + double sx = lines.right[y] * 0.5; + double sy = y * 0.5; + double w = m[6]*sx + m[7]*sy + m[8]; + if (std::abs(w) > 1e-9) { + double bx = (m[0]*sx + m[1]*sy + m[2]) / w; + double by = (m[3]*sx + m[4]*sy + m[5]) / w; + int nx = (int)(bx * 2.0 + 0.5); + int ny = (int)(by * 2.0 + 0.5); + if (nx >= 0 && nx < 256 && ny >= 0 && ny < IPM_ROW_COUNT) + new_right[ny] = (uint8)nx; + } + } + } + + // 插值填充缺失行 + for (int y = 1; y < IPM_ROW_COUNT - 1; ++y) + { + if (new_left[y] == 0 && new_left[y-1] > 0 && new_left[y+1] > 0) + new_left[y] = (new_left[y-1] + new_left[y+1]) / 2; + if (new_right[y] == 0 && new_right[y-1] > 0 && new_right[y+1] > 0) + new_right[y] = (new_right[y-1] + new_right[y+1]) / 2; + } + + for (int y = 0; y < IPM_ROW_COUNT; ++y) + { + lines.left[y] = new_left[y]; + lines.right[y] = new_right[y]; + if (new_left[y] > 0 && new_right[y] > 0) + lines.mid[y] = (new_left[y] + new_right[y]) / 2; + } +} + void fit_midline(EdgeLines& lines) { static EdgeLines prev; @@ -44,8 +109,8 @@ float calc_deviation(const EdgeLines& lines) int mid_ref = IMAGE_WIDTH / 2; int total = IPM_ROW_COUNT; - int zone_far = total / 5; // 远: 上20% - int zone_mid = total * 3 / 5; // 中: 中间40% + int zone_far = total / 5; + int zone_mid = total * 3 / 5; for (int y = 0; y < total; ++y) { diff --git a/strategy/deviation.hpp b/strategy/deviation.hpp index 8fa3b5c..43bd0c0 100644 --- a/strategy/deviation.hpp +++ b/strategy/deviation.hpp @@ -4,3 +4,4 @@ float calc_deviation(const EdgeLines& lines); void fit_midline(EdgeLines& lines); +void ipm_correct_lines(EdgeLines& lines); // 相机→鸟瞰坐标矫正 diff --git a/vision/element.cpp b/vision/element.cpp index 3ed27e2..265b05e 100644 --- a/vision/element.cpp +++ b/vision/element.cpp @@ -96,7 +96,7 @@ namespace { void element_init() { - std::memset(&g_elm, 0, sizeof(g_elm)); + g_elm = ElementMachine{}; g_elm.state = TRACK_STRAIGHT; g_elm.last_state = TRACK_STRAIGHT; g_elm.normal_width = 50.0f; diff --git a/vision/preprocess.cpp b/vision/preprocess.cpp index 65fb719..fe8e23b 100644 --- a/vision/preprocess.cpp +++ b/vision/preprocess.cpp @@ -20,7 +20,7 @@ static constexpr float CAL_FAR_Y_MM = 1050.0f; // 矩形远边 Y (mm) static constexpr float CAL_HALF_W_MM = 200.0f; // 矩形半宽 (mm) static bool g_ipm_enabled = true; // 启用 IPM -static cv::Mat g_ipm_matrix; // 3×3 透视变换矩阵 +cv::Mat g_ipm_matrix; // 3×3 透视变换矩阵 (extern) void preprocess_init() {} @@ -95,21 +95,6 @@ void preprocess_run(const cv::Mat& bgr_frame) cv::resize(bgr_frame, small, cv::Size(PROC_W, PROC_H)); cv::Mat bin = track_binarize(small); - - // IPM 透视校正 (首次调用时计算矩阵) - if (g_ipm_enabled) - { - if (g_ipm_matrix.empty()) - init_ipm_calibration(); - - cv::Mat ipm_out; - cv::warpPerspective(bin, ipm_out, g_ipm_matrix, - cv::Size(PROC_W, PROC_H), - cv::INTER_LINEAR, - cv::BORDER_CONSTANT, cv::Scalar(0)); - bin = ipm_out; - } - cv::Mat track = flood_fill_track(bin); cv::Mat full; cv::resize(track, full, cv::Size(IMAGE_WIDTH, IMAGE_HEIGHT), 0, 0, cv::INTER_NEAREST); diff --git a/vision/preprocess.hpp b/vision/preprocess.hpp index 1c796ec..a62ded6 100644 --- a/vision/preprocess.hpp +++ b/vision/preprocess.hpp @@ -3,8 +3,9 @@ #include "types.hpp" void preprocess_init(); -void preprocess_run(const cv::Mat& bgr_frame); // 全彩 HSV 二值化 + IPM +void preprocess_run(const cv::Mat& bgr_frame); extern uint8 g_ipm_image[IMAGE_HEIGHT][IMAGE_WIDTH]; extern uint8 g_valid_l_bound[IMAGE_HEIGHT]; extern uint8 g_valid_r_bound[IMAGE_HEIGHT]; +extern cv::Mat g_ipm_matrix; // 80x60 鸟瞰→相机 透视矩阵 diff --git a/vision/search.cpp b/vision/search.cpp index 8c3242e..2add3be 100644 --- a/vision/search.cpp +++ b/vision/search.cpp @@ -5,7 +5,7 @@ void search_init(SearchResult& r) { - std::memset(&r, 0, sizeof(r)); + r = SearchResult{}; } void search_run(SearchResult& r)