From 5416c0e986d14dc32477a954bda117c84e0c5695 Mon Sep 17 00:00:00 2001 From: QUYVN Date: Thu, 25 Jun 2026 16:36:49 +0700 Subject: [PATCH] init --- example | Bin 0 -> 25288 bytes example.cpp | 45 ++++++ olei_lidar.cpp | 408 +++++++++++++++++++++++++++++++++++++++++++++++++ olei_lidar.hpp | 131 ++++++++++++++++ test_dual | Bin 0 -> 31656 bytes test_dual.cpp | 53 +++++++ 6 files changed, 637 insertions(+) create mode 100755 example create mode 100644 example.cpp create mode 100644 olei_lidar.cpp create mode 100644 olei_lidar.hpp create mode 100755 test_dual create mode 100644 test_dual.cpp diff --git a/example b/example new file mode 100755 index 0000000000000000000000000000000000000000..94d5a08450326c8fee644b04efabc7391f598375 GIT binary patch literal 25288 zcmeHve|%KcweOlUlSxQI2m!qlOzmteQp4b)&2qVUM-*wK}lQWad zy_Wml=l$_Ke8N8ayVqWO?X}ll`^VX{+l$L?lMDtUX^)X!AOpW&sRFVZP|01|3j&!I z&v3&bL5Pf%tH;MkhjzjUro;mNr~){ah6l zA*r=d{%2WxZdB8IZd5sna0Ls4r&<*d!}Q6@n~HOqC{KgGQDCGrnm&3csgWkYcw;%9MEhJZl$5d{%y9T$6VL|} z;5VSS;rJva;D2=jd@=B0szkRz_Tl)~CcxiFAmz@Phw1%L5RM*{lo3HYZc zkms2Me86Vda>s+uaCXS<$`UUm;`b)tPY%Z_Pji#Q=XZMj4hK0FRV{JUy1njI4L-ly zTeZaA*xclBN78u zT5+suYjd=?y}srqXJdnZonvhtD%W(+Wmb_D=JL9oes^(&=Q4RHYN~DUwKV(O#cNsB zm#raIF-w8=EsPpQ^D4A_)Gq2g&9!ypxV`BfsMJy6_O*IoWRAL8ccb6os$Y$FSDVuT zg+o8+UcO$juxS*n2;pet{)n zwv~igfTUn5&i|(%ZosTlMGQa;lnRCh{A+|y5`J=La0jMAsq`~o{BZhOU3&R7>(U5( zex*zQi7s8uZ_?#A(f76Va`1cOSF17oOraMvY~7!J1%AnNNQ?Vt{1j8bB>HzPzUJai zct9h)hd4hh`TbfN$M-Apn&~qQQ_Gj4<%{{LQJaTlj^|uNf6(Ty0`Jl0>qp}7z1OMq zy>WP^HorU_hfmOO(&o8>zeU3{x5VKmwQ{G&;i9~}IQ*hUzaS15a?5czY2}v0 z<668t4i|E+h{HvFs^f5>xB56-==aF2JVK&nwRQ=NQT3c6$KoRX2aVnUdbV5f9M|C) z0z}xW!>8!*lREq=9e!GeU#-J0>hNoHc%~+&0pfmFhab2_m0yCKn#n4uJvv;gvV#9X zhwIm6M|8M;U2eb<54aN9L9X^5q?K!Q((E+2+MID|VMe(BPf$KF=ho|Up zz5X+G_$VFy1Rc&kM?4lCuAd)G*WvnkdY%r~Oh;k0K!@`hN<6X-A16SBB|2Pl0ZMAQ z4%b|Ug0IlwSPey=Y8^f?iW1f9@JTwnMTd(uIw!a3@bBp813Fx9Zx8D5EFJwLI^3ee zcj@qK9p0(Kr|R&%I((WA->1X%{pf%Wzg9;-#bSJGV8d@i14as>>uej?@XFpEqvP0V zV2=EeGxyhay4ig<@Eehl%abn$l)KctI)h^ zI$|wdtEU5#KQ2|&4ld+ zxx3az!K6I-z%-J>A3u;1Hd5ed|6J<+6k+5eD!gbk1l@BC2RMJPaY9$NJUiHL9Mff- zzNzBGWGVD1?6?>4-I2&nPH#Yb%6G`yn@UyiCHu~wH9QBV!DIcig5xPr(R%y#zx5d> zb}_B=WuPL`PN7H|h5u^O@+DJM?j#w)uxztbciq@Jl+ zE(`c0>;xa5nS$HqP_X?t?7{28630*r- zSCn1B@7N322Kple-S$6F5;j1eHZq(tK(;+bs^a!RTWo=@a?Vy-(MC43ksWO$hX*$N zPVobr9}~TO+2Cml%nZ`Sg<%4ps}$W%>}o<8{gKG{7&;oHn*_SK$ooEM((vAABXYreu`OxK+V&no!Du9e`&vcIl`W4bD|v`fe@e9t!xzl(Zi;qMdh?t||7r%~`z zw7a5{lM5BQG9f()Wz58%f}dL$mZ9S!l*fKhl!dg@R82wH-05tDw-TM|!8Rz{VD{;S zVJGsOMx6+LlJmvWb9+tws{IXX`lc<}Us<=l0{RZ*wSBcPT%+WbqItQ@fepV=eDYb? z;Z~GcT_YF5KVJ{NN9gl~iXA9rnB?&9zK-lX4*A)Rs%xn5EbL=o!y%>pg#4_3@y=mC zbi(Zj9{a5+sy!Y4da%wBqv21RQI><~%)kaiXuwWE7x&RhDRh1^h0mj|os;A*o#4@z zOt7uMQ8|gKDrwwdv}O1l__yCeXHsBiTu!^0&NyNFdnCEgKvWoz96oJS?Lm)oy{o~~ ziE^A=4tRgAwX5DMH_6XUS1NO0zZJZuX5g963#YJ zu#DxmsrKM3RqbIRW2g)0_B-dPm<^cg8q^Qv(Lz8)!wY&h4!o%*dT=lHVd0GDs6K9E>j5Q z`@sO@MExwu7w4ZFL&58j{;_c^`uw<@`|)nW-}z+fjwp0;w?3`k5U2+a97+Tjip4N>(oj*2GPW3#wyKfroO#=T>)IArtCF$aPCv@bmF$4q1`vuDE z_`+s)Em}MjyWeX%T7cFMHvKsz*>2d`@`XO?1U>_?_t(vv@ibSAAAn%vhW_* z?0)!~DfI;l4A4az+H747t#iV5)=5&Z{cB^0GH=ORZo{7C{?d*%Fv_rL z*oXw*f%VJ2b}x0?9+@3%KWZr4)G<3)y`B!X+bDc1Xxh=Y9b;14T+36!{{kE7fv;nl zEzs2zqBZREddKgcBtIY(vfqpMd3}1Hcb6SwauM`V9Bt1x!;$@1ODevv7rsM+ZXO12 z)(QK+J&ONh`ybym3HUi+Y}Y+#ha9E7RR3qkJL|g_b079!JnpV%TL+NpO6p$`O( zI{t~)EJ6LwRwgO!eaBKFL#i-Lr*^;$qy?k(i#gf$p8`}01eJ@bAw;}~}y-PYOwrwt9ep3;riCbM$)jNZ7`c0D3U&9u)_qekf0V|a+H@&W#q)!+m8$NZg}weO z?12`{$oh!G=`*t4Ll{`Tw}K#Z_5w>*o@G|nGbvQ{Ti_R5$)Srb3bKtHx{ZRbWKl4@ zIwb@{35MZ^&nb3+`7_!WeXHURU%}Q*^sk-h|2*dduFOe0fcIjY>oL)O?q7Z2*NHy% zFmRTy`XQ+hj_L^3-uo_e;P}K$a1c!0_DGT&= z34N^>w2k1$dEmDdnON?ZG?@ZlW3J3TIYmC$nMwyK9bpC?+P z@?CH(L?0{(IiQO@9qilal;y8H=N{K}26bNxoadM9M~fxe_LQPCH4o;nhpaQU z_Y&|_``(1ER=jhc+R5@k2L6k@DskW1H?YAN8hDk0wXpHCm8OuroW6I#We)NDilDFg znew_0^soN$6lT5jo5!cMleBsZ#uD4T^47WK6udB+3O@Pu=88|@U#}%;%hkI&y`a5< z%XzI_E;M2cv{jo*Ier5D(bhJlayR@`5hay&?CY51GSM1qBvSQzjNuUUh-2D)I`!4v~QScEv;BVg=LD%PeN4~if>31Yk zL9dy+|^|JR@Pd++WiWa)0GM$o+IC;^QE1212eUnP}A7RJpJo{WM~xH38V^ zo_DhE?v$ow(S+pA!-H~&M$FEll-w-%K&8JD`1HfhrlP-2MSuNxn%q4N{Z%qixD)F+Uj_xo zVH}H0wXA*x>$=b9FY;I*`}w=L@0LJMrJf(7|DL>dp@;2^7R=1z`8`danT2(L(udiv z(Sq!(ylhJr#@woXz<-6hVSPHj1NMvg#;NM_Y`+*MFuuyL;Xe3~pCi5Bg>@&^kssil z?c|3DxvfMyv7WiDc3v8^S4SEAI?7>NXS-0w`=|}a&gIy4c`i4vKwhVe4e%exb98wI z70iV_^V-5Uimo@90~HQ3tp4~HyRk(F--Wp!c5id4k#@nA6uNXHgv#<<#Ur!^-Ky2zCCG;IHc+diG*`sK(f1*@CeLJbIuzwuceW>n7N425fJ}L`xQr6Mq~r zBdcp9RdM>iO`W@CoZ*_RO?3wo9^psQ_|?CJEnaXH zhu9Xwt1+(L$m41W`tBm=tkm-p*yLYsl?S4}K(X;bU$6;dNzD9I^9Ah~*ZZ&s(fi+a zPa14*V@~hsNCCGyuW{JM4`%B~G9M&V-IroF-5sG$S0lT;Z zy!tjyn9Tf3Pe-aYB3(b$9m&ql`Wfu^AAxfnxo&^N`zGP*&!~N6EquX`fU~bZj6I4M z@ZPbqBpj$aa5PYN@^1l*xkjQ#eugq#J+p&okE1-!1u({qLR+%#EL)_)Mx~r2j0a}L zj@kCu4u$>od>Ps4cvr6X*GuAH3-Us%nu-580`YErCFmHVcWs-F9g#zGH(&@~z@axViO53En zcUQRPl(xZMR%5KL>Ue8+1;1Nhe})ftV+=5??!-IG1wXn^*>8$lpIDI&Uo*HqnTEN$ zU0t6{Vc&r<1$z=XEF+JFGUlOwM!ly%2OjKw>C-+#+NVg&DEcv){^WY` z#})SUr@W^70u^MUWdj>hv8I%Q=kH3#+*8V7{axBX;Y;vGuWp?7_}R*|&^h=%)}QOE zx~%Wb9!Ezl4vnLYIBZ)V+`P%~Ij>8%>j+-zab8 z@;`!)`5(|#_546L+Wy&qWWb+LkLVPmIy;QQLKPUi8W zdhG7UE@6*kq7gJizwCfsrkUw^lzkNA{LxNn;+FYyDdKA{^#nV(Do(@ZN)lD?2BH)T6|!`@0C5ZezZ}a zaT4rwTvsaUsN{$5fL{*x0?(#NhcAXz`|ZbkkmtC(j(PzY)?@p>fNswI^}grW&OZaj z>)$anX3HDExt^{^Ha+Gt8MpY(ZG7VbFoHj!&b;7nL96Og{az!!)gYbt`#V2*xynCU zT>t9+rd1YC-%GnKuLXa=)HF(_3t$s^=bKi?ExGar*FYRowG}ZUL z_-vD<1!ptdeua!GUCt)Uw5w}xuw1>q6`UISUfS%LY4Q7RzPi=o@y)d0SjWwIZCB^b zS)pXGSndMh_btw*RgLbOug;xQcW9plCt&<|pIhg#;Jk^u$%oT9$j=#;+gKVR{UN3s zZs4Og%}qE)((3YCnpzthEltgS%UT>;sa46zdVAF}e$BU4UeDpe%KTaUl0T164kA4OhcOv{ zHv!X*2a^TyD*V+0D?v!s49gRyCylnP($Wti) z27V0d%gYq$k{@?oVW7z@V-au*{(7)k%V&#CWAOFeno<5l>XRcDX7oCLF z$&vcOhi_>)SQp%8Ux8LRe{9XjV^SNF7HyaQA@#!#6k2g7s?q6Tj)dx+UlYwQl3nP@h<67b;sPozJYR=hoBNAjbl$Bg!^soO^EH1A5XKQ7gw z*6+nK=s}iq$ae%bUxngLd{D@mal~MI;)*9n+qY-z7+LaoN>YhAX;D&AnUu7|VB0oo zYoCz8AeDAGe<}&jVbBTEo^a=)BGq;W6?2o4~Cyhg?i8CsE z+Eca6wzS2&(srg7KQ`jg)D6eqw!Zt$;+^Tc(jHBHY=l*bTHkonTxwoy<$|qDYGqQZ zDgxx^IzS)inb*!P1dwyMT0@z(!%3!+unW0`kqy(2sfEksib-7Vx2)R z5_*rexURjxbjdsK##XF^8)<0Mg7mg{Qf|uktp7;?cf5W4op;}hEkoDhkE6Y~YDz6* zYsmUr0e4gnpDg+p`@-h8g#ocA4m5gXwh>y=Dj)_L=CS#F1X33GWY@ z`287^MIq)4ndc0Uhu)XqgkCqI!5MqlgqQuM@AR1IL30^6fBSr^fp0bNtp>i;z_%Ls zRs-K^;QyTl#P0!mc=3%#oLk3l5?;lhU7Yh5@#y(}DnNRH-NK7A_jwMDN5uIz9Qru_WZV(ans+32pv&C9|oC-((*T|?-oLkpYCTIy_{w&VQ^ClNRnc87>fp=;J z3AtM|dBmA{QNB21E$FZvqSO!Vpg>=1@gA)r5iKt2jb)ue-=w97wYdHayDH)T-T&p$ zD*ylA!xpXI32lqtp@{f54aM(N1U-K6b%SO4?KQ1U{#HwVer`c--mLsqC7Qo+&Yax5 zg50??v;>hSQOLjCTAN6~4`zs2I2(fa~oadehh zdPIFU;+Ny^&lwIMgL@K$XC6$)dF<%#&IVEbozOTE{yi4IGJ0P^EIvN!?_zOyDS5bZ zC(?vrapAoOxo~b1orw%0>YM5ZhuRg79|qJw*qcSms1BzXO|)@UgYnBjen!*SIDM-4 z(01YRXE=ET|AILF7SK!hg_6L9|5wR|+LekAwG$REak*)2qw(pnsDQHPjMt|%{QhYwuI$<3Rpmd9qVrcZIf{#Ut_DYO zF~5z+#XL72x5SOh{|T?LKr|vxEK$l_ci>+ksDoJqW)c=G&VS&~HzG z|3bmhf3*p{@~sCY{WbypG2ofF|0RAL`2hGHw2QC@-n-`KoW{TQyK0=jY49@<;c)e; zPkOJe7*3vXz_U@V z7#GAkCO-lFO2(z3<70IK`ul*hJ&67;>|rbL;qdi7oZ|M3L)pBa~i&J)f8=k|)X|40HpW3jLuPXDVC;OiB9XuW=t zfc|~ptj~CPKGXQ@(-!WXnwc7)AMU^L{U}4zTYNuSt zegAm^{D8(kUjEmBvwsugl=2-Itny%}!|C(E1o)%C*$&0LTG$WHnGeh7bp;CXSo$G38VaVrit*gs*(cmpe4!_5N zn_`;WKJctGe9-IowTt?>egU zIz4VjZL7z__si&FIM3678)bB)&5iB`+;UUv^eQ4=)^$qxbM$AF7U6cD;^MVcj>-iN zzPYKc*?SKztZ_8p$|omk#Y)R<@OhnFpOs4sSb}*)-iEbqZ+>1~W2>*;;ln|z;n+}R$E!IL%S+S z*4?_JXdD_`t{lE{;Ov)sk*bvI%bRgRjIXS!%y$$Q7tO(~N$RCR`uysxLwY1msn;v_ z5b=#mgSQ{Wkh^Q0erL2P^7EH0D=IE?lvUu2SaB2HLOygzxyh%Vn41PTZTKBY8#pytv)yR?AQyLV&u@J zDc3OR6SWJX+_-OwZxs6vZY|SS`0|t&e}kExBdZXO!q@DmcQ)1X-D>(vtMn4b-hri0 z8N7r_kBY0U^jOgyxn6f&Zh{-f65r6J@=zO{6c+|Ln+MxE*`@*XtAZMx4+4Q7hmDFE5%iUKW?ehdVBOXuBceJH0!cV zKlUm#y)4{z+U;X;rRe>n;tEjmWnZA?icx@SFsw-RrZcwQ7{6T5+~S5qhBnw$s8sPZ z_BsDY`y`q)zJ&Z(-=jKn^_DfguGCA}^oVLCO5N1bmjTAKpYHO( zSUl$bO!eZx7y`agbJ$xmmHQ84NMpNnTxU{eQP`pKt@HSuH3~xJYu7b_pBnnTY6_-#zJ}(en1};uUU#FD zGiYxujeg4I*(&Aw-EH`vC%H&#RwgUC?s{!nTaU{TqcJdb;SR_)m%GJ}X(rz*s_;-} zyebdz&V0B~uv7%ZY|@4N%_vGO0{)P4G5PjjP93wnx$Fopt9Yr4Z71as-^)a(ExWW- zl{Ol|3ol2o^o-^g`#mDON>7Y;Y`erf^AOYL7vDofXo;a7n!gC?_`OE6kbjrLTwv`mTS1kFTVSUP<)REneu4c z3L;8(7KEJFj zwndmHDyA`r?n7wtZE^X<_fQe8i2JS}YxzX*SX_Sb-Bg6pb-I=z-u^qa{6b9eeN}|~ zy+Z#8{RwO@UYJDWkH5cNeBTiC@#X(IE`RiUt@gdd;Xn@R1U&n?=9>ihJSX%M)=Ceu%uIbos@3nm^nK5Raf!V}o&le})8>Nt7@4y^d)4 zJ4Hq<6!`>v0WZQgFebiVh + +int main() { + // ── pick a model ──────────────────────────────────────────────────────── + // olei::Driver drv(olei::MODEL_VF); // 2D 360° + // olei::Driver drv(olei::MODEL_LR1F); // 2D 360°, 50m + olei::Driver drv(olei::MODEL_VB); // 2D 270° + + if (!drv.open()) { + fprintf(stderr, "Không mở được socket\n"); + return 1; + } + + // ── option 1: blocking recv ───────────────────────────────────────────── + for (int i = 0; i < 10; ++i) { + olei::Scan scan; + if (!drv.recv_scan(scan, 2000)) { + fprintf(stderr, "Timeout hoặc lỗi nhận packet\n"); + break; + } + printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X\n", + i, scan.points.size(), scan.timestamp_ms, scan.error_status); + + // Print the first few points + for (size_t j = 0; j < 5 && j < scan.points.size(); ++j) { + const auto& p = scan.points[j]; + printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%u\n", + j, p.angle_deg, p.distance_m, p.intensity); + } + } + + // ── option 2: callback (your own loop) ────────────────────────────────── + // drv.set_scan_callback([](const olei::Scan& scan) { + // printf("Got scan: %zu pts\n", scan.points.size()); + // }); + // while (true) drv.spin_once(); + + drv.close(); + return 0; +} + +// Build: +// g++ -std=c++17 -O2 -o example example.cpp olei_lidar.cpp diff --git a/olei_lidar.cpp b/olei_lidar.cpp new file mode 100644 index 0000000..56906c3 --- /dev/null +++ b/olei_lidar.cpp @@ -0,0 +1,408 @@ +#include "olei_lidar.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace olei { + +// ── Little-endian helpers ──────────────────────────────────────────────────── +static inline uint16_t le16(const uint8_t* p) { + return static_cast(p[0]) | (static_cast(p[1]) << 8); +} +static inline uint32_t le32(const uint8_t* p) { + return static_cast(p[0]) + | (static_cast(p[1]) << 8) + | (static_cast(p[2]) << 16) + | (static_cast(p[3]) << 24); +} + +// Normalize any angle into the SIGNED system (-180, 180]: 0 = straight ahead, +// + = left, - = right. This lets a model's FOV (e.g. VB -135…135) correctly +// filter lidars that report angles in 0–360 too. +static inline float to_signed_deg(float deg) { + deg = std::fmod(deg, 360.f); + if (deg < 0.f) deg += 360.f; // → [0,360) + if (deg > 180.f) deg -= 360.f; // → (-180,180] + return deg; +} + +// ── CRC32 (poly 0x04C11DB7, MSB-first) ────────────────────────────────────── +static uint32_t crc32_olei(const uint8_t* data, size_t len) { + uint32_t crc = 0xFFFFFFFF; + for (size_t i = 0; i < len; ++i) { + crc ^= static_cast(data[i]) << 24; + for (int b = 0; b < 8; ++b) + crc = (crc & 0x80000000u) ? (crc << 1) ^ 0x04C11DB7u : (crc << 1); + } + return crc; +} + +// ── Frame IDs ──────────────────────────────────────────────────────────────── +static constexpr uint16_t FRAME_ID_A = 0xFAF0; // 2D Ethernet (VB, VF, LR-1F) +static constexpr uint16_t FRAME_ID_B = 0xFEF0; // LR-1BS5 / LR-1BS2 Ethernet variant +static constexpr uint16_t FRAME_ID_C = 0xFEAC; // Protocol V3 (GS1-5) + +// ─── Constructor / Destructor ──────────────────────────────────────────────── +Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port) + : cfg_(cfg), ip_(ip), port_(port) +{ + auto_detect_ = (std::strcmp(cfg.name, "AUTO") == 0); +} + +Driver::~Driver() { close(); } + +// ─── open() ───────────────────────────────────────────────────────────────── +bool Driver::open() { + sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0); + if (sock_fd_ < 0) return false; + + // Allow multiple sockets to bind the same port (run alongside another + // app / debugging). SO_REUSEPORT lets several listeners receive the same + // UDP stream — only works if EVERY socket on that port sets this flag. + int reuse = 1; + ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); +#ifdef SO_REUSEPORT + ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse)); +#endif + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port_); + addr.sin_addr.s_addr = inet_addr(ip_.c_str()); + + if (::bind(sock_fd_, reinterpret_cast(&addr), sizeof(addr)) < 0) { + ::close(sock_fd_); + sock_fd_ = -1; + return false; + } + pending_.reserve(2048); + return true; +} + +// ─── close() ──────────────────────────────────────────────────────────────── +void Driver::close() { + if (sock_fd_ >= 0) { + ::close(sock_fd_); + sock_fd_ = -1; + } +} + +// ─── recv_scan() — blocks until one full revolution is available ────────── +bool Driver::recv_scan(Scan& out, int timeout_ms) { + scan_ready_ = false; + + while (!scan_ready_) { + if (timeout_ms > 0) { + fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds); + timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; + int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); + if (r <= 0) return false; // timeout or error + } + if (!spin_once()) return false; + } + out = std::move(ready_scan_); + return true; +} + +// ─── spin_once() ──────────────────────────────────────────────────────────── +bool Driver::spin_once() { + // buf is the recv_buf_ member, NOT static → each Driver has its own + // memory, safe when 2 lidars receive concurrently on 2 threads. + uint8_t* buf = recv_buf_; + sockaddr_in from{}; + socklen_t fromlen = sizeof(from); + + ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0, + reinterpret_cast(&from), &fromlen); + if (n < 0) return false; + + // Distinguish protocol family by Frame ID (little-endian) + // Family A / C: Frame ID / magic sits right at bytes [0-1] + // Family B: has a 0x010F preamble at bytes [0-1], real Frame ID at bytes [2-3] + if (n < 4) return true; // too short, skip + uint16_t id_at_0 = le16(buf); // Family A (0xFAF0) or Family C (0xFEAC) + uint16_t frame_id_b = le16(buf + 2); // Family B: preamble 0x010F + real id at [2-3] + + if (id_at_0 == FRAME_ID_A) parse_family_a(buf, static_cast(n)); + else if (id_at_0 == FRAME_ID_C) parse_family_c(buf, static_cast(n)); + else if (frame_id_b == FRAME_ID_B) parse_family_b(buf, static_cast(n)); + // else: unknown family (3D LR-16F uses a different format, extend later) + + return true; +} + +// ─── flush_scan() — a revolution is complete ─────────────────────────────── +void Driver::flush_scan() { + if (pending_.empty()) return; + + ready_scan_.points = std::move(pending_); + ready_scan_.timestamp_ms = pending_ts_; + ready_scan_.error_status = pending_err_; + pending_.clear(); + scan_ready_ = true; + + if (cb_) cb_(ready_scan_); +} + +// ─── parse_family_a() ─────────────────────────────────────────────────────── +// 20-byte header: +// [0-1] Frame ID = 0xFAF0 +// [2-3] Protocol = 0x0200 +// [4] Distance scale (mm/count) +// [5] Error status +// [6] Start angle (deg, uint8) +// [7] End angle (deg, uint8, exclusive) +// [8-9] Num points (uint16 LE) +// [10-11] Rotation info +// [12-15] Timestamp (uint32 LE, ms) +// [16-19] CRC32 of the block data +// 3-byte block × N: +// [0-1] Distance readout (uint16 LE) +// [2] Intensity (uint8) +bool Driver::parse_family_a(const uint8_t* buf, int len) { + static constexpr int HEADER_LEN = 20; + static constexpr int BLOCK_LEN = 3; + + if (len < HEADER_LEN) return false; + + // ── read header ── + // uint16_t protocol = le16(buf + 2); // 0x0200 + uint8_t dist_scale = buf[4]; // mm per count + uint8_t err_status = buf[5]; + float ang_start = static_cast(buf[6]); + // float ang_end = static_cast(buf[7]); // exclusive + uint16_t num_pts = le16(buf + 8); + uint32_t timestamp = le32(buf + 12); + uint32_t crc_packet = le32(buf + 16); + + // ── verify CRC (optional but recommended) ── + int block_bytes = len - HEADER_LEN; + if (block_bytes < num_pts * BLOCK_LEN) return false; // truncated packet + + uint32_t crc_calc = crc32_olei(buf + HEADER_LEN, static_cast(num_pts * BLOCK_LEN)); + if (crc_calc != crc_packet) return false; // CRC mismatch + + // ── detect wrap-around → flush the previous revolution ── + if (last_angle_ >= 0.f && ang_start < last_angle_ - 90.f) { + flush_scan(); + } + + // ── decode points ── + pending_ts_ = timestamp; + pending_err_ = err_status; + + // scale=0 means the firmware didn't report it → default to 1 mm/count to avoid dist=0. + const float scale_mm = (dist_scale ? static_cast(dist_scale) : 1.f); + const float ang_end = static_cast(buf[7]); + + const uint8_t* blk = buf + HEADER_LEN; + for (uint16_t i = 0; i < num_pts; ++i, blk += BLOCK_LEN) { + uint16_t dist_raw = le16(blk); + uint8_t intensity = blk[2]; + + // Compute angle: linear interpolation within the packet's range (device-space) + float frac = (num_pts > 1) ? static_cast(i) / (num_pts - 1) : 0.f; + float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start)); + + // Filter out anything outside the model's FOV (already in the signed -180…180 system) + if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; + + pending_.push_back(Point{ + angle, + dist_raw * scale_mm * 0.001f, // mm → m + intensity + }); + } + + last_angle_ = ang_start; + return true; +} + +// ─── parse_family_b() ─────────────────────────────────────────────────────── +// 40-byte header: +// [0-1] 0x010F +// [2-3] 0xFEF0 (Frame ID) +// [4-5] 0x0200 (Protocol) +// [6] Distance scale +// [7-16] Model identifier string (e.g. "OLELR-1BS5") +// [17-39] Reserved +// 8-byte block × N: +// [0-1] Angle (uint16 LE, × 0.25° → deg, 0–360) +// [2-3] Distance mm (uint16 LE) +// [4-5] Signal strength (uint16 LE) +// [6-7] Unused (0x0000) +bool Driver::parse_family_b(const uint8_t* buf, int len) { + static constexpr int HEADER_LEN = 40; + static constexpr int BLOCK_LEN = 8; + + if (len < HEADER_LEN) return false; + + uint8_t dist_scale = buf[6]; + // scale=0 → default to 1 mm/count so distances don't collapse to zero. + const float scale_mm = (dist_scale ? static_cast(dist_scale) : 1.f); + if (auto_detect_ && !model_locked_) { + std::string raw(reinterpret_cast(buf + 7), 10); + size_t z = raw.find('\0'); + if (z != std::string::npos) raw.resize(z); + + if (!raw.empty()) { + detected_model_name_ = raw; + model_locked_ = true; + + static constexpr struct { const char* key; const ModelConfig* cfg; } kModelTable[] = { + { "1BS5", &MODEL_LR1BS5 }, + { "16F", &MODEL_LR16F }, + { "1F", &MODEL_LR1F }, + { "VF", &MODEL_VF }, + { "VB", &MODEL_VB }, + }; + for (const auto& entry : kModelTable) { + if (raw.find(entry.key) != std::string::npos) { + cfg_.scan_angle_min = entry.cfg->scan_angle_min; + cfg_.scan_angle_max = entry.cfg->scan_angle_max; + break; + } + } + } + } + + int num_pts = (len - HEADER_LEN) / BLOCK_LEN; + if (num_pts <= 0) return false; + + const uint8_t* blk = buf + HEADER_LEN; + // The device's angle counter runs continuously across revolutions + // (no per-revolution reset) → mod 360 is needed to get the real angle + // in device-space [0, 360). Wrap-around is detected on the [0,360) space + // (monotonically increasing, then resets), NOT on the signed space, since + // the signed space jumps by ±360 right in front of the device. + float first_angle = std::fmod(le16(blk) * 0.25f, 360.f); + + // ── detect wrap-around ── + if (last_angle_ >= 0.f && first_angle < last_angle_ - 90.f) { + flush_scan(); + } + + for (int i = 0; i < num_pts; ++i, blk += BLOCK_LEN) { + float angle = to_signed_deg(le16(blk) * 0.25f); // -180…180 + float dist_m = le16(blk + 2) * scale_mm * 0.001f; // mm → m + uint8_t intensity = static_cast(le16(blk + 4) >> 2); // 10-bit → 8-bit + + if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; + + pending_.push_back(Point{ angle, dist_m, intensity }); + } + + last_angle_ = first_angle; + return true; +} + +// ─── parse_family_c() ─────────────────────────────────────────────────────── +// Protocol V3 (Olei GS1-5, magic 0xFEAC) — ported from the existing C# +// production driver OleiGS15Driver.cs (RobotNet10.RobotApp); NOT independently +// sniffed/verified against real GS1-5 hardware (no device was available to +// test this while writing the code). +// 48-byte header: +// [0-1] Magic = 0xFEAC +// [2-3] Version +// [4-7] PacketSize (uint32 LE) +// [8-9] HeaderSize (uint16 LE, usually = 48) +// [10] Distance ratio — read by the original C# driver but NOT applied +// (distance is always raw mm / 1000); same behavior kept here. +// [11] Types: 0x00=2B/point (range only), 0x01=4B/point (range+intensity), +// 0x10=4B/point (first 2 bytes unused, range at [+2,+4)) +// [12-13] Scan number [14-15] Packet number +// [16-19] Timestamp decimal [20-23] Timestamp integer +// [24-25] Scan frequency raw [26-27] NumPointsScan (total points per revolution) +// [28-29] Input status [30-31] Output status +// [32-35] Field status +// [36-37] StartIndex [38-39] EndIndex +// [40-41] FirstIndex — index of this packet's first point within the full revolution +// [42-43] NumPointsPacket — number of points in this packet +// [44-47] Status flags +// Angle: angle = (FirstIndex + i) * (360 / NumPointsScan) - 180 → already in +// the signed system (-180..180); no fmod needed like Family B since the +// index always stays within [0, NumPointsScan). +bool Driver::parse_family_c(const uint8_t* buf, int len) { + static constexpr int HEADER_LEN = 48; + if (len < HEADER_LEN) return false; + + uint16_t header_size_field = le16(buf + 8); + uint8_t types = buf[11]; + uint16_t num_pts_scan = le16(buf + 26); + uint16_t first_index = le16(buf + 40); + uint16_t num_pts_packet = le16(buf + 42); + + if (num_pts_scan == 0) return false; // avoid divide-by-zero + + int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field; + if (header_size < HEADER_LEN || header_size > len) return false; + + int bytes_per_point = (types == 0x00) ? 2 : (types == 0x01 || types == 0x10) ? 4 : 0; + if (bytes_per_point == 0) return false; // unknown Types, layout unclear + + int payload_bytes = len - header_size; + int num_pts = num_pts_packet; + if (num_pts == 0 || num_pts * bytes_per_point > payload_bytes) { + num_pts = payload_bytes / bytes_per_point; + } + if (num_pts <= 0) return false; + + // Magic 0xFEAC corresponds to exactly one model (GS1-5) — no model name + // string in the header like Family B, but recognizing this family is + // already enough to know the model, so auto-detect resolves immediately + // without reading any extra field. + if (auto_detect_ && !model_locked_) { + cfg_.scan_angle_min = MODEL_GS15.scan_angle_min; + cfg_.scan_angle_max = MODEL_GS15.scan_angle_max; + detected_model_name_ = MODEL_GS15.name; + model_locked_ = true; + } + + const float angle_inc = 360.f / static_cast(num_pts_scan); + // raw_angle is used for wrap-around detection: it does NOT have the -180 + // offset that the externally-exposed angle gets, and stays in [0,360), + // monotonically increasing — matching the same convention used by + // Family A/B (last_angle_ >= 0 means "we already have a previous value"); + // subtracting 180 here could go negative and break that sentinel check. + float raw_first_angle = static_cast(first_index) * angle_inc; + + if (last_angle_ >= 0.f && raw_first_angle < last_angle_ - 90.f) { + flush_scan(); + } + + const uint8_t* blk = buf + header_size; + for (int i = 0; i < num_pts; ++i, blk += bytes_per_point) { + uint16_t range_mm; + uint16_t inten_raw = 0; + bool has_inten = false; + + if (types == 0x00) { + range_mm = le16(blk); + } else if (types == 0x01) { + range_mm = le16(blk); + inten_raw = le16(blk + 2); + has_inten = true; + } else { // 0x10 + range_mm = le16(blk + 2); + } + + float angle = to_signed_deg(static_cast(first_index + i) * angle_inc - 180.f); + if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; + + pending_.push_back(Point{ + angle, + range_mm * 0.001f, // mm → m + has_inten ? static_cast(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0} + }); + } + + last_angle_ = raw_first_angle; + return true; +} + +} // namespace olei diff --git a/olei_lidar.hpp b/olei_lidar.hpp new file mode 100644 index 0000000..ae1ee4f --- /dev/null +++ b/olei_lidar.hpp @@ -0,0 +1,131 @@ +#pragma once +#include +#include +#include +#include + +namespace olei { + +// ─── A single measured point ─────────────────────────────────────────────── +struct Point { + float angle_deg; // -180…180, signed; + = left, 0 = straight ahead + float distance_m; // meters + uint8_t intensity; // 0–255 +}; + +// ─── One complete revolution ─────────────────────────────────────────────── +struct Scan { + std::vector points; + uint32_t timestamp_ms; // ms since power-on + uint8_t error_status; // 0 = OK; BIT0=Monitor, BIT1=Voltage, BIT2=Temp +}; + +// ─── Per-model configuration ─────────────────────────────────────────────── +// scan_angle_* use the SIGNED system [-180,180]: 0 = straight ahead, + = left, - = right. +// 360° lidars keep the full circle [-180,180]; narrow-FOV lidars (VB 270°) shrink it. +struct ModelConfig { + const char* name; + float scan_angle_min; // deg — VB/LR-16F: -135, 360° models: -180 + float scan_angle_max; // deg — VB/LR-16F: 135, 360° models: 180 + // Remaining fields are read from the packet header (distance_scale, rotation_rate…) +}; + +// Table of known models — the driver auto-detects the packet family (A=0xFAF0 / +// B=0xFEF0 / C=0xFEAC) per packet, so this config mainly decides the angular +// window (FOV) that gets kept. +inline constexpr ModelConfig MODEL_VB { "VB", -135.f, 135.f }; // 2D 270° +inline constexpr ModelConfig MODEL_VF { "VF", -180.f, 180.f }; // 2D 360° +inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f }; // 2D 360° 50m +inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f }; // 2D 360° (Family B) +inline constexpr ModelConfig MODEL_LR16F { "LR-16F", -135.f, 135.f }; // 3D 16 line +inline constexpr ModelConfig MODEL_GS15 { "GS1-5", -180.f, 180.f }; // 2D 360° + +// Sentinel: model unknown ahead of time. Family B (0xFEF0) carries an ASCII +// model name string in its header (e.g. "OLELR-1BS5", verified via live UDP +// sniff) → the driver auto-detects it and narrows the FOV per the table +// above. Family C (0xFEAC, GS1-5) is identified by magic alone. Family A has +// no such string, so on a Family-A device MODEL_AUTO keeps the wide default +// FOV (-180..180, no points dropped) until the user specifies a concrete model. +inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f }; + +// ─── Driver ───────────────────────────────────────────────────────────────── +class Driver { +public: + // callback invoked whenever a complete scan is ready + using ScanCallback = std::function; + + // ip : receiving host's bind address, usually "0.0.0.0" + // port : UDP port the lidar sends to (default 2368) + // cfg : model config + explicit Driver(const ModelConfig& cfg, + const std::string& ip = "0.0.0.0", + uint16_t port = 2368); + ~Driver(); + + // Non-copyable + Driver(const Driver&) = delete; + Driver& operator=(const Driver&) = delete; + + // Open the socket and start receiving + bool open(); + + // Close the socket + void close(); + + // Blocks until a full revolution has been received; returns false on error/timeout + // timeout_ms = 0 → block indefinitely + bool recv_scan(Scan& out, int timeout_ms = 1000); + + // Or use the callback (drive it from your own non-blocking loop) + void set_scan_callback(ScanCallback cb) { cb_ = std::move(cb); } + + // Receive + dispatch callback (call from your own loop) + bool spin_once(); + + // The REAL model name read from the Family B/C header (only meaningful + // when the Driver was constructed with MODEL_AUTO). Always the actual + // string found in the packet (e.g. "OLELR-1BS2"), even when that model + // has no specific FOV entry in the table (FOV then stays at the 360° + // default). Returns "AUTO" if no Family B/C packet has been seen yet. + const char* detected_model() const { return detected_model_name_.c_str(); } + +private: + // ── parse Family A packet (ID=0xFAF0): 20B header, 3B block ── + bool parse_family_a(const uint8_t* buf, int len); + + // ── parse Family B packet (ID=0xFEF0): 40B header, 8B block ── + bool parse_family_b(const uint8_t* buf, int len); + + // ── parse Family C / protocol V3 packet (Magic=0xFEAC, GS1-5): 48B header ── + bool parse_family_c(const uint8_t* buf, int len); + + // Once a full revolution is ready → flush into ready_scan_ and fire the callback + void flush_scan(); + + ModelConfig cfg_; + std::string ip_; + uint16_t port_; + int sock_fd_ = -1; + ScanCallback cb_; + + // Buffer accumulating points for the scan currently in progress + std::vector pending_; + uint32_t pending_ts_ = 0; + uint8_t pending_err_ = 0; + float last_angle_ = -1.f; // wrap-around detection + + // recv_scan()'s output, gated by a simple ready flag + Scan ready_scan_; + bool scan_ready_ = false; + + // Per-instance receive buffer — NOT static, so that 2 lidars running on 2 + // threads don't overwrite each other's data (data race). + uint8_t recv_buf_[4096]; + + // Model auto-detection from the Family B/C header (see MODEL_AUTO) + bool auto_detect_ = false; + bool model_locked_ = false; + std::string detected_model_name_ = "AUTO"; +}; + +} // namespace olei diff --git a/test_dual b/test_dual new file mode 100755 index 0000000000000000000000000000000000000000..710a6765c6a0b7e892569200e1d49628fe082718 GIT binary patch literal 31656 zcmeHwdwf*Yz3-YmlgX110Sfw|?ul9((PzHx`yGk#sseQ!YJwp6PIJPL%<(f=~A-H*m~s z5sSzFH1-Wahqa9^R!;5x3jA|7Rr$1CB%tgJ+dr?^>4{)H>bM{MXUN>K>QmGq*?<)D&)a78`LX{@iF z(^xaRvA(IbeYVZ!vezxh$#geoX3OOs1^!gWOH1!!&z^YW&L6*jpn7u0(RTmRQOV}V z_di5zDG$VtXy_vPSbitt6eiMB;4kIqSt7eSaww^uL8;5aIlh2IZyHL<&PTzIMZp)K zo<~IBV~;{l1&fsK|BQma6@|~0DEK!Z;7Iw2M?NE^y99ZUgx5sDe-tI%X5f<|$RQ5- ziNybvD15S`;9H~M-BI#si&EZ{C~_VdC7(Zy!e>jA{B(hTB>CH-r2ASFKEH{Ar$*s( zItqP8l>D!Vg8wZFPHol{p?)i)q`M{xpZBBCZ-|1UxDnI+PtZq_^QLaWu@-kzPEJ-6ECuD2sWSAEl(Vta)rJKJ7ob6Gtu zTfN6!Y%i>^;a_2{b%oXDcGs_IDlA`Kk%OEOmjy_{VXMiWk5Da6v6`oua)LGvI;x_ne z8q3<8UoFqsS;&^l(b8zMI|`jnl$|qL{*RYFr?$SSrjSRkTvZCbBs^PneOoq(*ts}T?6jz?KUea z7ouE8K`m$no?0H}bU2;OZ3JSBK@Egjq|;X4L>yFfXgPLg3v)Ny8yp_yhDzH#%;m7R z)w-IU%-)ESGSHwEvT9_5*{Yjep!1+ryBHF&w{(Cb*xMT@U2Cnaz7cFZ#IU)=!|IzH z9;>aU#uY}jrq$-EVaOR6*>AaphCB<%>kQqku_M#+P>2ek;1P#POG}C^`PR9a*$dgy z{Cw-Y%q+&C)vIxzlbM~(@>j3UvlLsiGv}#pGjqep=4PwC>if*Ob7e2mn3p5Ww#4X9 zkDr0X@OvYC;-10`7>5xOhkK%kSLo#X1o(6qW65!K2-mSBg}Dy*ccKp!{lre+4h&0^ zSQ{|9D7;7&UUqXwA_K2Ys_-gRxE$Z8if?3dl<*etvt8YZ9wvc(U%_7gWdo)M@obmk z?|AK(;2+0+sQA}%`WQyzS<0hl#=l|6ZeYJt{4O4@XRpFf7nN?65=Q<*ygae&9R-u~ zCx!AS<`a4)e%UyR#n@_Pz9Dd?;B^{&kutAn(cs6GiBP)+?^STG1~*NV)7`AW56+eG zU0Ph3ZyeCzhZX#=2JcbuS2TE+f}hgh2Ne9Q2KOrXc?~Y|aaDsqq0lo$PC}ldJaJlF zp-<7^qCBY@T;$)R!9_WzYj9DnEDbKoZ`R;v75N`uNh2D}c@#bIj*;snPMMzwT=<1v z=n##nmUNe{B85NU>iOR*3V$8=i?&Jhr&PFl+|;YWrxKxZomJs81qk=N3cp2#Usd5W z1`}6`QZCBpBmu(RRVvFtLOwAx3te3*ToDluJD|eV>i`E;xR^sygu^Oa%*_e!QQ`3f zDAy}0JVAhPkE?JPl+blbg%1m%jP4Ph z+7AVnsqk;8=vS-oG!%Gp~Bl$xLR+$DtxkvezOXnqQak0;Ww-B zT`GK<3hz?k(^dEZ6+T0SA5`J$c6wNa&sNc=oAhrEtpA;VK+k*=b!^+f`eO%r^wyK- zfjxodJ!09?K#Jv7pGitTNwng}!%7iWEr)S`IsBa}|0?+RtNi!C|B}joKm7eFe>MEl zZ9FZ`uNMAED*w0Px2XJ0@K>w+F8DiD{xm3#d=Q+te5-)4>MoJnWce?6?#AH)bY#r8vL+h zPh8GtzAN?^|1Qw(#BUNa2l|JzfW*8ff_fJC6neZb7~F~Vk8a)MOy9>Z&Gv1U5`2!% zsXjkwU?z{>Jkjib#GK%ZpMLD`-=4z!efrVeb#W}vs~^){`;^X4ylS>d{?myp@F;lo zCYl3Y4Ec0OE5l$8nAqZzl+R!+3w#&hln$l$6U0N`d;AaH1m6{;J7tvFzYS@-k@hsC z^}s~tzdD)udiAN@9;8R<)@+m0tNo*#UQ?9xC~rF{3~{%U8*2_!N$lBPJ@Nw=`8&@b zum0iY0Oixdbmw%)57wC~8S|D8the|tR^_|Qb~oQB^; zl%p5$$Xjq~t8FgRA&gl{0dH2gjU4~o-|vVMg6;CVZ{ zG@wVA0l#eUxeq^%Us@V4L$>YUL-`TBx=`1w3UvuL1MXtR&h=m!opdDQ{L+98an9>? z-3)e#;%VtK7<+Zn^^LFRC2iW0aXoMAG0=A+uI2jDKsApmh2m101M7dwZQw=J`EQVB zRkb9^82f`8PqXRn;u_BJ^Wm{Y%_#iv0gsB^R0h3;tng zohQi7UaZie9wgL*-hb^D76{sBzZ%p_f$tzK&+Y7{PakH!AE7Q!>qpJRx^DMXya_QPxB%+%#6`V?IvUfRBvBoV>Gq*6?2ol;+z@E53;G*u=pY4qcpzl3?&*XSt$6ee1PWkQAPvB`tpLxrJ8<{@{ zxg^H0ZInhI;*wk>p3dl*ma14EyGr>*df?BH*JmIj(#<}|rV8?=`uH8NXN_z`x_)$n zZXk&CivpkREecQ>Ug7#QraKcZmpSS314UuFtczupBtyt+%f*U3$j|~A=0k=g|37f~ z_l`9Csr(> zl%MjMfv^mOEoLLn#eWvWlIEx_tj_adKiWf|etb8@w;9YwSA*Linj23O~u`ER);Y z(+KN_Tq&Gno57@4rC{(-FLYuJ!Y(U%w+ZQH^vs58u)zG!gJVg*1;%v{^+`M^d^X}y zTi3y+307C?szp+ifM@1=f}k=sN{*K<912=jzdeR{IjV^M$J6ZzmtcC8m>=|i8>uO9_{ z7}I@hAh`3A-t0dK*`LHcLtV4p1m8v6lYA(?T;7r0nczY4q&Cupb|Lx{wJnq7t$XcZ z)S;EJ3j^zQ{(*evv-AE?@?W070+(U?y2hJ7vw=?^@}ook%rUI8f{lI^X$DB98}DQn z;`C(y&4GYrq|6|M6#qll`-iYm3b4FRUTwz zLAvHZP%rlz6`uzWVSdHsLv+_sRJdY$M6c@K~W=$}tBnk7R+-O!DnZK%UIzK#he3e)uT!Z~Q#Qzi|Nzc=fVvIRIW& zF7uJ9-RwwJra54nYYtSO$jhyENx9T_n4ue0b6KFOAG%BJw^Pd1K}U7y`zW8>o{S_r z^6~*G$s9P3_C$S)8mE4y3OsE{$42RZ_a{nwQ}-W~f0koD;$>GC?%?J1-c+@K9bscT zPoNIARIMt4zUsKX+U_sn`r1zUx5(oR+cIj`>{T~mD2o?*Vp||%O~C9oqkJd_^aE{4 z*IkYBRI$Y;s~{s=C-b>2%t!otP-Yu2YSYvvsE@jTAUIaEadm%7<1g?#o(Ep2PmCAj zF_^4#dZnEk{77%+7=+hAN6w%PP=7vf9r;B0OtDuNUK+`K^AV1|i|T*oBe=KVhxUJD z8kJ`O+2g^mex0{G19&jV{I+Uy2bCei&~p#P=^x2D`o^-3`3T4OY9__)gT0U>@E^{O z*nyj3t}e7eMxJV&&x^R9BF)avx17L=-K*));my$BP9y2_V<)blue}@iIK;b*IwHIc zaV>1jtJKaK(N-21*t1>I*ewf?{uT5qt(h$F?0B=!dy@G{Pf-`gBaO3;AsyJYnd)(t z7-LbsM;~Aw7{AG2_J0TU{2}a1 z4C=YdxamL_bgL3$penTSyAc+YM*XxGcBm?zRl?>3Ixv3O3%fHNef3`0oP9>vWY`?& zW$pve*+Z~3WXEBzZfZxJbyTws8+4~bl6)IK*ZWz@ooVZgW?yiF&d)}7o~S~5*@!yO zo1xRt5%i-6F&{X%(Z!Bfjwkpwp3voPdMUwIwT>O#2-~n2G#k;jts@iK?W^Jfzkt2% zfvqE&=OL?f#@52-9q%2xcf9#~QZCs&tUIcHF z3E97W-2Rb{kL|t{IQj<2%N(HbKq;>;c98mq2HcapsXt5AQ~#dYJqGS+(Ebo@o8nX7 zQ4L+B_D%g#8{$x3{S@3_zMi+)f%OL1P95(bF5he9^?7YM3!I&n;QVA^lCui+)SJ$D zKWu%9t;Mt6%8L~-yuL5#rI{bkNpvQrWTu%?G3I3~Cy`A~`wi-#>bsWQD)_4&wd7*) zFlX;?+1e5wx6uFboaIjFT{+2;WOzDV?lBT+xD|>dTS4q$APYQMQ8c8 z4f>Ir5O3QUHlnKRC$1U56H;cTy^%a>%X84DB>1VFR!QcEh%N(ub}Hk37B@4^4*sd& z`wYexDW>UZv*4m%IhqCRk0{r^Wcj`Z_co*Xq25Gx^w^_jzX|bCe;q9DmNfRDRMDHy za#`H0wBF(DC~spJbCV94FqSf9F<<{k^N~q$>_`uE-8~88L21;vP1k~zG;X|DAEz2}J?PqbU@^`?m*kk=y?eg65r6tUF zERFdB4RL-biZ1{=dfq1sz;?<7!-QUaPZ3hIllG!WZ!2Noll+*q;@|H{*=a5h5s?kFR#ERSoLGi znIN}oXluQgJ3N7KibL~aqNlXaDRI1?2P=EYCdZkNb|tW*ED3HhJ4*G|i}71J8^?L| zVceF1e(P6=cg4OGZLr91g)H`Ul5Jz>On(C(w22ESdmC_?Z;~B_es9~)WhTcd$35vg z$&h45`rZhha@$Mo-iLc?Q~yEvLmucCd|6W3mOS3AfsS9SF!=M!*f(jMOXDjBc`Zya zcXXnC^<$h%g`Fy3G0Qp+c8<3j*xI~cu<{R(#Rn|*oDK4uoPx10 z(xE83_HcGutn=eJan8RkPH@ghHQzBm z#hklflsR|C+Zh`cC7F*r3fsO5_I5aI#17coH{;p-nG?--EJOI+u`H+8$iBHOcShPL z3*()`pu5yw6X8;Or8Y?IbQb(H_e_RM_&|lS~cAF#r2x zVX@PMywhCa<2gl;(=zAx(SFZPTk0e|V~b{`(R`mxpOuDn0p5nmuCYZKX;~SjH1xTZ z2Z8?*W&1VUPUtVj8|SJnlm4QgK>uon4%1r9j}hLFcK#IR!SCUo^yE8msjfslk({Zn z{{6E-eYKXrt|J}Nbx0&c;}_b{BY@M!&i-pRGLu z*Uk(xe+b=wewDcc>ruUC!HD}l-Sz;O2PU=gVFya`kAxS;+&@MS#tlnm<@b9 zCm#Jog7X0Shbr_vrY-1uz@rDUBYlX6TsJ{?lc9Sv#+lNnpZHV!jI{1ySd&2bZzj## zGFo?Y+NRpW<6pI?|V$O30eUF$kS$JQe%$X)*{`!dIqxm4sGxK4aF?Zo> zp;&jqT&wfMN0=MY{O2^%WYX2=bAYEOu?s(kUANt|tX;Z)Z@GQ$vUcc81N!R9&Nug# z)4d7$r+a@d`T$)+7w$=)VMim*Px25BTQfL6nT)Y}zC4#tC)41XWh+L`(T%8y_)*;D{nD> z36%?d@@(`SG`{g-J#{43La9EeZ0?6Gx$f)191qrzZ26e8W87#ayNbwHZ-|E3rO-GK#hQP0$C_4eD?i)?=1q zY+Zse%?jvnssGw~)TiFS`*y5lmeU+_o>CVi57ZULEAm=G9@afFu)c6_U~tWSVEqgJ zGgu3*!TK+)57PQ{puylDzuJtnbVx_X6n8X>6DY8Swb9EwI~xUf>yQ{HqgzlYaMOyhvkUnrHo=tHGV)2ZDz_1%B~w5B-|j zK{DuQems(m-10hbDyRL4O}p&|{TBD74XY8QIdi&anXql{j;!{nS#wvFa7}LPoCL#AyzE@;ep|38GdnAbehho} znmltTf7Wa&GqydtWba(^vb?)0SCXsVJbUY{v_Z1D37h>|?H*H8Yh$CSso7&{!{)*o zBE~hiOEWuL-Vkf8ceXUr2H8c{;-X+`#Iy2)O!guEu|krZ$Aq<@OkV% zgTZ#VKZA?5%AWcde2M?LU@!|E@#Js8A%KC~Z7w+>xS zdzx-SQaZ;{k<@*s!2>SKi%m;`L;I3;0m~u`7hz`n4gtFzE?p-4D)H+DRs@&jC7XU= zcv^4SDwV$f-s^lz55;T8FA1_k7WhTjWB9eBqojMfO!#%-=K&_?L*`YmRaz=|Q9kL? ztt%_xC5YYAI$p;nP&rzFoA9IjHA1$Aa}ZEoa@7x#o=#kve02~QgGcI)#J zE!^eu*oHJ2@=ROI4EuPRUY+F!2~Wr8Z;jg)YuRqx5tF~uuuK1>v|HDl@ZS4xzY|J} z_DE6rcOh;PbcXh<(M9t81i!f6VDLrayIL+w{tx1wj?Ld{+!m9+-LOM%*(vSPN#6v| zFpi8(r}&6>3i&#Vdz8wY{{!RGF$G%<+w|KdE26x~OPk0@)~S%!Q%L_snKp|^Eks(d zUAiyf{r5O+eqz43%|pgXKb}AuQ=sFAac_7VwL4hPctuo4+Tos9hJEdiBvoS%5Y)M?cD{*I1;qLe+6V{)4EAO4Rm+ws4mH1@B?)W_J&3pX~<1*v&JW4o^ zNb`s^PtF4JPh}{B=q|xl9Y9%-|AWYX%G1LOw`2_6WQi$` zjVX!)lbD!g#=Mx7x|mX_EM^5VXJ|t)N-QVe3u!D} zu2S-1A@@*?tLh6(SG@gBc)^OOl7?0-NSA0ysVd*i`@bY`_gkmle&^lrG*mUd2lYio zyl*;otp8q@^+yBik_@=tr7yw# zQwF+!#Q+LWChpQ14omD22|X6P86|)LZPv5HIs%THWR@vTIa)sn7ds({@AlBxT5F$@1sxuf@baM^IP|&V zFH-~?RQ#gc=N127m4e~13y(iePVWEP{}e?qH(Xid(c~b`qZ1V)&Jhs)FYAhP1_V8h zBe~Ty{jTcPCQqv=I~#|QWX;ZQ<=*TKbLVDePu)K0_kXy?x*F;S1< z^ut)BdegIHChS}|{cu*L*uQW*C3Ie3I6i{ClA}iCdp%AL=(;TuJ`(373d=nhj`!h0 z=a>wlbZ*0FRuz$cOz6DCaQd+!dl-(xij_r5cN|NNhzsjJ$c19#RkGk9BHzh&a;RRB z8PuT!Lf=eEMET5vVWD#{6dxYUu5-`D)*f1t=(Lc!a zL-k6=hw91fFHAR)#RAB^Q7$pQ5x9^~orb@luhMid7DOaD z2%MgK(zAOdA|Bos3p#c{BbiIY2 z4xng{qP5C=hT4bIoPH>tfQXdO(0G}c(1 zJY_z^Sv0+RLZd$gO8y0YR!QJD3hq?~Vtn5L!i2u{&Jj70U6Jl9q4B(o3wo9nPJc?_ zpE6Cx`SWSq8qhvSo^Q;MasGT4w=)0HOpNp8&_P_x2ei1D2WW9IAJF33d4WjxtVaK{ zkWU@@3xSs@jAZ%|?BD{yS$6lqiKPF6{wYoWg?D(se+1Lw#4QpZypVxwXutU#&gVi< zJHC2c!F!Z*RVX%Ha(xZ$-?2YD?0g=Xnz84APeOeOeGv1KGg0V2je^Hv9xy^MRk@kK zZvy|W3>g$^w1nd-Lgn^bz*AUis9g(+8sPg-UqXMBX$f1e@IN$Jrlh?dbaioDzE}T$ zF$$iDdDKm6LgoI4=$K8&k9Isj=h|i4%1irS=%PIkBiLZStW4Y3 zWZ;p~UCim_xa$A+0Vg?ZR`glO!4-wilN=u$*U?@lx}FA3^)C7&QSToBPk~)EX~sE! ziNb#bCZ>_{UkjZ0i++&yZqa4LL~?|b%CvTVB=AUlW&xihM-BVGBntg~QScVv8Aw<3 zcVb@p80QcDS0?Pjd`|-b)vxGJrz@!+10E@#9}_MO9sitq~QM(;8ZVSyd-c778IzRnz6~8uEVm!egggHxT4@fKC3vrLZou) zqTmk^E)5+wJ`9}nLGj#m_`y&O{>iLJk`FWO7{O zdeD6kIMt)JT#qVzm`SGXQrg8nPCwMn{6eAE+LJ`A!$i`P6yQ`}qiEobYn1G;`5ad~ zDrZR)d^PY$`L6*^@)Y9%zBdp2jz{4$9XnJb>1Qr*(tpuk3q34}Lf^>oVJvI5oO!zN@ZBILs=>@H7sW;2^ogJWxPQnJ-!Tf?TbGr;wL?bsV`eZXPEakou`H39QD+&B`t z)z+A4Z)w57z9qS?)+QXjTVtbRtN*#1;3-YFr?s{=)6NEu*S30`ILR3YY`c-^nr7>o z#^!2UqqPQG+TB)LD{|kA&H6a9x&|lYMx>y#gzK#~m&?{+bu@Wg9e9(#=5(M~PA47n ztMXxEd_4~KRguyjf4RJh1ir!s|LnQymk^2}xWdA=N^8X;ylpTytG3zo0L~4z*5myS z8%jmBl3DL|*{D4C$Zr`eC~(!cIb7LUwT-RrIxD>cQP`H*g42&95aj@OJ8)*Pv%ayz zYAY;TURVF+XzdzWP3qx{t2uFDc6I}Q1423f+`6PZZ$+WCu(W_qO1I+s4c5XU1uQBk zXV!v$Da~6^oX@OFOIBL)N~|lFEU750v{vR>N(!yYiOpu!;m7>YZb)zye~ZD2mpdHA zvQXxi;rMJ1orTU1E9XaX%V#&MB=}8StI)S{LpBA*mvlt>M*8` z8k@%!iiOugRu&YNSWC+BN(Mi9oWCt1>M@ce5jX6db#;b_?>~M*`ry&!YF?Q*6Iwp| zIxPQT$CZDbUWCz9M1D4Z%f|XodGQA4^{>P+-!=73)>gNJ%wqVN-(eC`h4GWV!vdA2 zlAR^jJH3mZHG# zQfWUrpyo)b#WhuYgWzlPKGg#H>Od<$Y~NbrZnol$w;K9fgZk9?u=)%;gkBvocnZ84 z6=%z|qMQdj;OGXZ-uCJNB*>k8j+1gDA#Db@*B62 zhpE{MsVfS554U(3AHWv2mExl*Ynod5SAft6nwnisOm)P>ptuZV74z_2o`RN@mCDzC zzEWEW|Nf4)!SIivgcVR}_QFV6$WRXbpbAs~s{AaZh|G zQLyA0#7cr4@tcv9oCHQmEE5YaPOPUySjqawAX?_9C09Jg;@2YOM!3pj03}qW4S8@w zkD2huAQWHh8xrmmH8JkRa~=WHV?}j*vEM_uCPApU+V};)aXv`1$p0?oM!4cR4j$L= z3y&Ydu2T4DOPz>+Sh*1{y>Cnx?Kh-HX(GOOzO_ohMSQWZNw{J^BFOkfvQ}UB!$W)h zM0~OTNx1hZ#3KKKPPh#Sr@e#1FZM|Zmsc)mDF4JmET8IBs#87;!Thk@>Rf+wK zB7G5Gl>ZUXQhW4cOu+ZG+UX>p_uZ`ZcTi#FHE?h^i-D@ z;86o_TSO!Fw~6oGYSR;b;l7VJcw`WYzgl_jv3fai8`L-1H|Q7fKSluMiP|<@;`wHk z692pmhyIIr0=R-3VH*e&`x44ZWD43>L6;Dth#%5JB(6?h>__&JVZ!Cr2u#EmZj2^= z=(&8E9ADdhL_9eZwh!?M6Xh4r=TepUT6#@(Sc;Nt6hzXbJ`*$ z{msfl8M5Ox{6v04x$yWlly@Fc`FEU%aV^$7)Dh{*q4F~yQFeimJKQb%#%UsGyzIXL D=spJB literal 0 HcmV?d00001 diff --git a/test_dual.cpp b/test_dual.cpp new file mode 100644 index 0000000..fd78a4a --- /dev/null +++ b/test_dual.cpp @@ -0,0 +1,53 @@ +// test_dual.cpp — test 2 Olei lidars (front + rear) concurrently, per appsettings.json +// Olei-front: scan_1, DeviceIp 192.168.100.11, LocalIp 192.168.100.100, DevicePort 2368 +// Olei-rear : scan_2, DeviceIp 192.168.100.12, LocalIp 192.168.100.100, DevicePort 2369 +#include "olei_lidar.hpp" +#include +#include + +static void run_lidar(const char* tag, const olei::ModelConfig& cfg, + const std::string& local_ip, uint16_t port, int n_scans) { + olei::Driver drv(cfg, local_ip, port); + if (!drv.open()) { + fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (interface khong ton tai?)\n", + tag, local_ip.c_str(), port); + return; + } + printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port); + + for (int i = 0; i < n_scans; ++i) { + olei::Scan scan; + if (!drv.recv_scan(scan, 2000)) { + fprintf(stderr, "[%s] Timeout/loi nhan packet (scan #%d)\n", tag, i); + continue; + } + printf("[%s] Scan #%d: %zu diem, ts=%u ms, err=0x%02X, model=%s\n", + tag, i, scan.points.size(), scan.timestamp_ms, scan.error_status, + drv.detected_model()); + for (size_t j = 0; j < 3 && j < scan.points.size(); ++j) { + const auto& p = scan.points[j]; + printf(" [%zu] angle=%.2f dist=%.3fm intensity=%u\n", + j, p.angle_deg, p.distance_m, p.intensity); + } + } + drv.close(); +} + +int main() { + // Both front and rear are Family B in practice — front's real header + // string is "OLELR-1BS2", rear's is "OLELR-1BS5" (verified via live UDP + // sniff), NOT the VB (Family A) model the config name suggested. With + // MODEL_AUTO, the driver reads the real model name from the header and + // narrows the FOV when it matches a known entry in kModelTable + // (olei_lidar.cpp); "1BS5" matches (→ full 360°), but "1BS2" doesn't, so + // front currently stays at the unfiltered 360° default. Call + // drv.detected_model() to see which name was actually read. + std::thread t_front(run_lidar, "front/scan_1", olei::MODEL_AUTO, + "192.168.100.100", 2368, 5); + std::thread t_rear(run_lidar, "rear/scan_2", olei::MODEL_AUTO, + "192.168.100.100", 2369, 5); + + t_front.join(); + t_rear.join(); + return 0; +}