From c4965756e6388ce9ce516bcae2659072490036aa Mon Sep 17 00:00:00 2001 From: lyf <169361657@qq.com> Date: Sat, 28 Mar 2026 09:45:59 +0800 Subject: [PATCH] =?UTF-8?q?=E6=95=B0=E6=8D=AE=E6=8E=A5=E6=94=B6=E5=9B=9E?= =?UTF-8?q?=E9=80=80=EF=BC=8C=E6=B7=BB=E5=8A=A0=E4=B8=B2=E5=8F=A3=E6=B5=8B?= =?UTF-8?q?=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../armor_finder/send_target/send_target.cpp | 100 +------ serial_debug | Bin 0 -> 86808 bytes serial_debug.cpp | 269 ++++++++++++++++++ 3 files changed, 270 insertions(+), 99 deletions(-) create mode 100755 serial_debug create mode 100644 serial_debug.cpp diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index d4fc98b..8d08005 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -34,58 +34,10 @@ struct ControlData { uint8_t right_switch = 3; }; -// 接收的传感器数据 -struct SensorData { - float gyro_x = 0.0f; // 陀螺仪x - float accel_x = 0.0f; // 加速度计x - float accel_y = 0.0f; // 加速度计y - float accel_z = 0.0f; // 加速度计z - bool valid = false; -}; - static std::mutex control_mutex; static ControlData shared_control; -static std::mutex sensor_mutex; -static SensorData shared_sensor; static std::atomic sender_running{false}; -static std::atomic monitor_running{false}; static std::thread sender_thread; -static std::thread monitor_thread; - -// 解析接收到的数据，从杆量位置解算出4个float -// 协议格式：| 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE | -// 映射关系：x_move -> gyro_x, y_move -> accel_x, yaw -> accel_y, pitch -> accel_z -bool parseSensorData(const uint8_t* data, SensorData& sensor) { - // 检查帧头帧尾 - if (data[0] != FRAME_HEADER_1 || data[1] != FRAME_HEADER_2 || data[14] != FRAME_TAIL) { - return false; - } - - // 解析4个杆量值（小端序 int16） - int16_t x_move = data[2] | (data[3] << 8); - int16_t y_move = data[4] | (data[5] << 8); - int16_t yaw_val = data[6] | (data[7] << 8); - int16_t pitch_val = data[8] | (data[9] << 8); - - // 转换为float（假设原始数据是放大了100倍的） - sensor.gyro_x = x_move / 100.0f; - sensor.accel_x = y_move / 100.0f; - sensor.accel_y = yaw_val / 100.0f; - sensor.accel_z = pitch_val / 100.0f; - sensor.valid = true; - - return true; -} - -// 获取传感器数据接口 -bool getSensorData(SensorData& sensor) { - std::lock_guard lock(sensor_mutex); - if (shared_sensor.valid) { - sensor = shared_sensor; - return true; - } - return false; -} // 更新控制数据 void updateControl(int16_t yaw, int16_t pitch, int16_t feed, @@ -98,47 +50,9 @@ void updateControl(int16_t yaw, int16_t pitch, int16_t feed, shared_control.right_switch = right_sw; } -// 传感器监控线程 - 独立打印传感器数据 -void sensorMonitorLoop() { - LOGM(STR_CTR(WORD_GREEN, "Sensor monitor thread started")); - - while (monitor_running) { - SensorData sensor; - if (getSensorData(sensor)) { - // 每秒打印一次传感器数据 - LOGM(STR_CTR(WORD_LIGHT_PURPLE, "[Monitor] gyro_x=%.3f, accel_x=%.3f, accel_y=%.3f, accel_z=%.3f"), - sensor.gyro_x, sensor.accel_x, sensor.accel_y, sensor.accel_z); - } - - // 每秒打印一次 - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - LOGM(STR_CTR(WORD_YELLOW, "Sensor monitor thread stopped")); -} - -// 启动传感器监控线程 -void startSensorMonitor() { - if (!monitor_running) { - monitor_running = true; - monitor_thread = std::thread(sensorMonitorLoop); - } -} - -// 停止传感器监控线程 -void stopSensorMonitor() { - if (monitor_running) { - monitor_running = false; - if (monitor_thread.joinable()) { - monitor_thread.join(); - } - } -} - // 发送线程函数 void senderLoop(Serial &serial) { uint8_t tx_frame[FRAME_LENGTH]; - uint8_t rx_frame[FRAME_LENGTH]; while (sender_running) { auto start = std::chrono::steady_clock::now(); @@ -185,15 +99,6 @@ void senderLoop(Serial &serial) { // 发送数据 serial.WriteData(tx_frame, FRAME_LENGTH); - // 尝试接收数据（非阻塞） - if (serial.ReadData(rx_frame, FRAME_LENGTH)) { - SensorData sensor; - if (parseSensorData(rx_frame, sensor)) { - std::lock_guard lock(sensor_mutex); - shared_sensor = sensor; - } - } - // 精确控制发送频率 auto elapsed = std::chrono::duration_cast( std::chrono::steady_clock::now() - start).count(); @@ -210,9 +115,6 @@ void startSender(Serial &serial) { sender_running = true; sender_thread = std::thread(senderLoop, std::ref(serial)); LOGM(STR_CTR(WORD_GREEN, "Sender thread started (50Hz)")); - - // 同时启动传感器监控线程 - startSensorMonitor(); } } @@ -332,7 +234,7 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { } // 索敌状态：yaw=150旋转，pitch大范围扫描，小陀螺开启 - updateControl(300, pitch_scan, 0, 3, 2); + updateControl(150, pitch_scan, 0, 3, 2); return true; } } \ No newline at end of file diff --git a/serial_debug b/serial_debug new file mode 100755 index 0000000000000000000000000000000000000000..546a3d7f73cc7ce4639709ceb6eb7f2165c2169b GIT binary patch literal 86808 zcmeHveSDPFmH(Y22nu2ZR8$HBVxk!T%Y-CABw(6EOD)|elgU7)PBQ7t0Ey5w zzHCdYZAnd)U0+I-{fN|7wsl*mw2hT*-Jk9@Ds9=W+6f?~)pk>*t!bh8eb2r3%yZ|- z^8niZ`8*$nbMN<_d+xdCo_p@gGn3n@YnSB(0)n4>@f|_A?uiNs>4G&YZJv-=B+e5d zF-=Sr=b*d*ALet?;ye`MVLC=-MLsCi>(%8>db#Ff%B>R}rOVvNJ0DRfQz3h!{wH0x zo>JvoPbrB^`$!=?&uPl_NIj~zL)Y7(>oFBtf2Q0&>KlDJbh(qxL>WCyiI*Ot^f{#a z;iN&7(G!&aD_%<0y`FBNdMB$@fs^jn{bI`PeHitqe`os?YQ}oqo|ip4b-$VN_))u) z@F8`Uuf9>-HYfIPJ8!CbFbMFL%JGl`k$8vjRMjs$f18oM7;9@>5`Q!>@)yZumYA{vUbZ zuc09~ImbS~z!h?Ud2md_~P3#IW?O{5`RQW9wmC#`fc97_YJuC51G+8j>BB32@mPDaCRHH}tdu~pr$z-ma( zw;H19O{m`zuiMxLy1F3|PgS>MQ|q#+ST+^Urcy{Xq|2i1E%A5VcveW=vigQZgHse2`*)W42oG!je2675Rc2+SrMPcMkTn(c{F>!x`n^Dr3U zmM$xT7H=tSPi(4I?anKSB~lg)7`3pdB*V!rE7l%MqtII4*=cn|lc_{|I37!PSsRO4 z)3Y0gNFzpJzNH4ReG{1<#*8F3q*>0pAJ9eA;dQZiyr!X4np6!a**Lw3d(wd67uJpK zn_}%PR(&+Jp)D%f!|jPwJR0o~ZPB*2#Kx#dN7ltRq*?`hrbQ$=qA(wsAfi3FEgWkX zk#(tPI+5y#M&UA6q%&--i;*>MjjE!UDk8@tUnSY7AYkMaHLyOK4yV&ek&3NHEdVH# z0Qe#uT@ZC?qhMN<~w15_=bW7x#YiLPdK-u*XsHGNoQc>Z{D=V$iqGD0G zW=+M?8mpwJR4lKpS&A&+vZB&?IoXnOgR!XGEK_M~UQuaTHd(R|ae$sY>_qbMzfD!T zKqL9sZBjXZ&XK%%Vsy6N7+D$+=K=}n5A9C^q8Txf<6PbQyT%Ie+zO1I0OU64@+Rb8 ze)1M;O826v4Njw z;EN30v}ee`#~J)94E%fpuQTxR2EN9?FEH>X12?zctp;wcyB!8@uJ@e=u8*d&m2Lyq zt|{@Y27ZyD&o%?U*ub|N_$3Cu!@wsS_$~v#%)s{;IG71hmpS+`A3ZW zWyt@~$WKN7u#umEd|tqA|NY3%F!EO;ztqSVA>VA|=OKTGkzauPBS!vOfY~;U${2eN9`rg^3t(o~heV^!lX(~Ri=ZoC~`Jti1SBRct@aqixRvcVDl#j9k zd~P2ezW)F|5GGmEqw_$miLgofN!-A~>pN0CYO&5+UB`6QU9M{^>`T4}~FP zFh8(cU|x_MCqH7z^D|D~_HxCz+wY!!vf|!nz<(HZm%h1ts9Dy{%hpwM^80d5zCBh9 zy^S`TW>lAl0%9TL_KR1R4|SuCY-4+9=!59XA$)?OyS{10sPebRhK5dU{$X|=Q(wqF z|BF5jSBe)i7l?hbZl<7Tnh@nw4!no-9=;Rhqs7obz8K1k4h;zrDxdad-Y3HW(J1o; zqDNw{1U`vAC~T3&_U3=G37CK$AI9~R3>Y!?WLavDpFg&dPpoA+mZA#(}F<|?sIz$XIT=PBRL zEFU`b*Wvqzmxz7Xv(fUsC$MLBw(KF49mbrH_~6w&nf$!n+kw3eEQEIEj}pBY@2fNU z6L~8V{RS&)k|I=*@2Kt~E+V~dgWR`^Xi2`vU z#&_QU(wVD5`!IKIdE?K+@Du1+XzQ8MZAiTP8Y!b4*Z|G3L(7N0hBW!A^6vw?$X`!w z{$7r4P)pRakT?SD0R6Tq`=kFndnl5pMjhme+S zH`#5^(ekM6`wV&40e80jyHjd>sO?W1ymizD+kBke_Bn*>wx=PjY;OC*c%%Fhq-}qC zR{K-#T%$3?fl9_a8N(>=6~k^r%%E~JhEd)thHcjTW(*^KuNbyT^P4e@_`PCSO7qWh z=s(*d<|Qi9x4}G3K}vm>-0D=A@dWYsogTo_q-^VcHUu+@lzN#9*6HC zt;WGFe*#!`tSMF@UZhM+$EkK;9H+(_Sx5KL$>U}K=6Fo-ra`m z6Z!Vs|7TN%Uhm**h>v-@Nj~@MGf1OfIWcUht}AgJ!#=3^1_L3q3*Quo*KuIfuO0B~ zpBio73Vh(UP*)RC?Z<6gzuT_A)~NpGtUpDGrOZ5_=uAotW4${(py6yT? zfRX=XUbfdDOJkC^54;$!3C#Bso9{+oY}<>Gmi!r8_9$D9T1O%0g_DNAYTx`ZF!s$B zqYvkzzFbq_n|M!{U0bJVxtZr}UFHE}UEYQ~=#t}4-!R+N{`9!wgCAdz9gjmMpPo-& zF)^+4zW`&M7aF>~qH&r}Uo`NyvzfISbxYqqvVBFvDAZ^EA zUeEXCtmm}XsR-U)Pwj64eE@pq)B#_SM4Q#tk?cWZQq*q zHYk7HDE}Ro@36~XrSe~fhW^*!JIZ_+o9|^{fzbB)>0(Ec`pa$p5Ay~mWZQhsko6*$ z8#?zH<^O4^S#0(O}QKaK}5mm-0l)p??q=s=+RrEelV z{FA`G;pYSUsQt_}VyJ-lo(=Tm1-oC8{s1|jrLv!6EPgJ}UcdjZ%SGex5i#_0*m(fF znIm*=5_gkaysww?gNR`mm*4-I&aJ@Vqc7oIY%hFGo@b|Pd^+%{jN?ojhQ2|JHEx~Z zYTfl1J2|JtMM}; zo~tSSG}6zG>M5f1%i2e}?eSGMs{iRL?ey4cXGob*EMpg2qNGmt+6Ye}u1H-Q~?y%|QmYmk<5LzA*{ zF9W9SvrV3Pq1(Wp4NR|luK?q9uPb1$$x~6EaDjF2MFTGb{>=Nj>#ql8dyYj~w%={H z|7SyP#Hc?;%Vm52leRZ}TAs5>KFj?BWI*mVeHNDX+GF-Xubp25#&wP(O|l<=Tr50t zZbguuws@p(;@(Kk0piX47(RK$Iq;QpdUjAAG6tRv4Lwe2%p-yQZU%E}0BM?UOK{Ki z#?j%%OQ;P!|2~KH?U2+Nv2V8cq~P_gPum6S-BTzhyZ*Q4dk~c4``0P0ee~N%(|pT( zC9rQG1-&ZIk$obH?|AgWfBFUPsRZV&xyBB>ChrgW1LC5rFWhg)-GQ_mt3JE@i$?i& zE*BSP>wkvIG3Wlv;QIpeg>1f0Q8|45S%dE`=IgfkQdCa!1~O?~T&3s8G|@aYIQBU1 zD%U^!*RlUNRh2hS{pIDiO{KUq?WmZWsce#W2;YT_?ELS8Y?}W@yX&}JBUayNl>Yju-%^npIS3tm20NFPvrZ;{LoPL z9+B=DHBNiH931zL17q>IAha(tHncB@J;xc!uXdJasBV5Ri=W!8?n`mEIW%wy;?@Q5 zW$Y`k7ufd()+9OafCYpqqx(^Hf8<^7r>Cu_?wxYln+%<>Z;)*_VV{V$e?jXZ_6Vmo z^Zi0@y~7hVKi8cY+V^9lT>&{ir#x`%f%olkl>Kmod`Ev!=Ua|C&mvD_;%0+@*ji8e zKSy#wPsj86X?T{N+g&gBX^=tfdC4Na0M;fd-*+0mJA&Qyx08Q@?@0$Ad_&!7!zbMQ zBnLBmH6Re;6mfE?oOC@xYf{eVguVz2xxSB>4#GO?S6+j+skw8unLX zzTo)6}4d4bk3r^HI2Kb4ew*j)r zT%=)(6Bs}0FZLHN-GsYdxt2j^d>2&ieVg(`Ip%tHk5A*l_M!XbtZ(Y}(Z?RyC-fDs z=_irhDgSNQhImyUf0VN>(wvh0(lLbmmFES|A?UB;5ac1Afah-JIkj2)EzN0q_e1lU z^r0~$Kg|#H{EYhseI!n{xgyZh4;+1bUi$J8=#D-24}fJbc4`f0|ILj(BtzO1^Jox# z<2mn)2PWT}+6FeS5sx4zgSb;bdratVuZ4EI*bm4!H3~e^hfs&(va3uTK2Y>z(e3v3D9E62^TE^z-l$%eGgh^Thp z3%wL$Sx=`-@AVq~v6F|~cq){pFqdkw=E?D3_t|HC*83_bi(_!Ilz`fB>+w{D+& z^sT^_iU+2jTz;=OdE33@_vZRDmuv_fc^uBl&(VhL8+@DQ9<3MB&w)2N{QN8A7bE;! z%_SPU&r`XcKkVxd0yEDT;R3{3^zHk^tL@0V2MN>j?B7WbvOny6t?ZW?i$R=esm~M} z2JVEf;Ir;eh${u6WB{?|Gf0!aO)C_h_YM_*9d49o0?4q}n_S!f4DE#P`_fC#{9@-@ zflod2)-zWg*0#Ud)%N|QmtlKz{Bcf^u?RXUs^d5N1oKj#7~`?y_;i@|JCF}qzwf3q z59YkI6)GrVaVh9QK_fibJ6!K zD29OV@M!ST8bG|LLvc^0p}SmH2}f+=wbc2&0Fp!eGeH6y^MwSE!{RS(j)URV3ejc{Ua(( z<0RK_;4+5Jg)TT-W-0?comAGK_y!>Bewtr5Ab$huuLK|J(EK`r_v(jHJ^-A~0dE7R zbf!VAo74v0tA!iAFLPM7WlXJKdb*_Wlg}|31T?)SIAWQETuE*V! zj5P~GL*r2geJXD{&3=LIr`dkg?+-&ax#xmz10R9>kA;RdQlHS5%u2Eg;sK?B^FB`c zEAlc{QfxwhKSOnNAEjSV8uiI9WWQmH>8^XN7jpI?=<`qpK4i}$_kD%jtB~y|UQ>L# zN{wp}cRZvA@0BlwUebV}SvRkBlKFDu zfE@$ny7{Q+`3u^l_MG#QzC*>aiOPvj?vbI_J4lniVr-vRy776;kWck5Kp)7jF+X7& z>}_Fxv`I1jziEsh%WhZiZ_ReWM|0*k#4G1tXz1TaU$WZ~$az-u{DO2b?o?$w()IBD zH06)b|55NAMtjo!*vr34a)8rZgPmWbwDb>9#s@Ih(;(Q21EZXCY7ZU+rIc zufwtBXJ~_CtXurV@9m%iQ4<2S`Vig#q&U!p$gVz*(h)p*7C zC22G8u}_lj{C{GLF#R3ch>{GGrXMJT!C! zX&GC{u1Ft&&D7abov8>j&pGaXYs@>NJ!-24dk5JT(u0U6VcOH6-6c2+WvhAK*{mKg#h6=>eq4J_E1;oe$U!@bSB){lG)-i{%)}`5kxzeQP8+v!H{Ce^%o& zfYZ81@xYwJrZ31jg72n~51dPW4*l$LSM$j^7m1JO;yFmW%|$mmG6r3TD6YMi*x}UW z82ZS%96_3Nk$YzSem6Hh7~d0ejN=`K&)kNzTxV%b!d&g9GWh(d&Ai6iafa;oK0SBD zimfkY0z2LcZV?ZdvE~N6H^_}O_I#rHB;TGxS~krI_%rQGWUPeW9y~F8KaY)ROPWW> ztM7sucetjF$u8uR&Ey-fz4VLG=Gvlc!!cLx2Wigexk~#X>C^lUZA$1%(`et~c#j@} z{3P_G{_X@-@j=;*bm((lYiTj!Z>cx$|zMPn&J!=d?A_A6$I_?};-%*YTBoQO7+Q-^q936PLh0 zaE_iN{Ree!2anvBqKwY$ZoXsMj@CQAi-ixBlT7UC%9UTM_YrPBqrMNWexu?1eFptL zjh?4B{Z7#Dh32+IH_lC`yDC?dmK9UM577di`tYV;RX82KHYl?~6Y;^bd{iwVg2DA& z$%NIJl@pFcqVW;=E=T#ToL`m_Ap1PLgt-uc}qseQ7#huiKCcrx_`u*j# zL8WOh*yYTJH=z&@RRe!VEFEbjMCaGx*O-z)=Tn@Tso3DTI(V(oy{mK4%Gx+P?e?I=?lQyn@#Oc6UMo@`7 zwsqU~RjNF+hZ&c4sxl+F)gw z!GEa_1Z_^_8}Lii4XJB`H&xWuR2li2)e;HLQo?9BgYw6%`o~DsogL9gnnpm4>+(ce zSy|_gKl$|QU%bz@nW3NEzFSHw8VvsG>8~9B##fL2;H$@OfAU(`4(@>xC6feZV>_fAfjHLe;xf^bR%@T>%WGF`;k5hdK7dL4hQdm)`5=0Md41+GSDYL zw;^b}0J;mbsegF54|L0ms7LtA!^2|`Y-;~!cz8DGb9nJkfFE|AJUl#Hi~5s}3=h*E zDwz$s1+*5l7c>s~1n6C$_@21f4|){z1ZV+%C^;L!btdRNpjDuKpe>+BLGJ<`gO_?c zL5o2TfUW^O3fc`?0KIxar-1GPEd%Wbtpz;<+5|cX+6g)t7qfNHy9%@ev;}k<=v|I-ZK@Wk>M!!yg)`E`v74!iOf^Gp_1NtE7PS8G3aeR3AIB4;&hldOCgUv}eP?do$ z0<8sI3)%wO2f7vXIOs0WNq7nT9Oxp@z z7lUpGT?;zpBHnb(PSvV08pk%&8b3e5l~z{H6Eit$yv*m&XGEJPDsI__TtL+?pT4 zd-2)z8uTZyVtip0>`V9)9{2%`2SJH{ndW~5**%cs#7_X<>ySSM*tmeg7aGjrI>$TadTS z;Hd-8{uhRayRl|~hvnq24Jam(lLpV^moN{NoQ{XaKYaeud&lKHpyiOSZ3j;ue2L~9 zw^d_|6|J`~pggF`rLQbA$`75PoYrKjzu1t!O4ld*O~#r@zHHiCrcoBSwXRl1i_s6% zd@CR>rUYXy&Dk*OHppa0Oc50@{!cTxU-Dg#j8(vZCqvMCK7se852 zHm&139Bq?Mw5}6QY1EVcMKWlee-OBympSt;i1NMoJcaUF(1M`UOSbu8-R4Z-uh0iP zYl%llp1Oz5d-&X?_m1fu-GG5PI9~R#7~@(9-z?7clipEzm`TKhCy37)@J-HO??6Ds z_@;-(Jv?^lz326wyFsy0AGUz!5O}D4>d$gLUwVNbb>Km;JOO+Vc#Gm^{chB5BV$xQ zc!~$S=lCGX7olAHm9&HDSB2QSRH0n!ByDNNm|~Q#K)GB4^w?c*_+1^!*Py&n=_u>N ze9|t|=MI$LW4B*egAy9Ut-yBxcZ*F_kM@YW>~cBJsa_xOy};e{38FlM^5;;#25H!s z=VLwFjAR$UUr&NZ`Zvab)*F&NMdOr4-5ft9Y%$8;K|Qt3sdx^-vadmuuSNMR>{m}W zUftkn1qt)>Y$P*ZNc$|+E}5>O<$(=nt*RZJvaTSyc^|xC@)iTrN8i+ zM{;+d{2_JovcjJ0a9xguVMLrAK!CSp$3paM^}o{|?}E zCZIMTGiQDU0d3Ru78~VQV_*{OobZs%o&rx3WSBf0yMrh{i1IBcKY%pa;Mha`&dPWP zJX3I{AbH>!;oFn3x1Wr1w>GHGB9xb*T=!enH}?{2P`()DjY_`sGcyi!qPzp;rhS6o z-3B}ke66a-^R;)Bj={wL6nN-71Xe>CaDFxIN4nhud@*nxn`AvRHa&szG|J8PNX7x+ zy$*Rn;I9DR2D}Mr^qXzhI|@Q+sh0M@+3yALG%6lxJN8p5529TB#y##)c}SPfHtO?w z8U)sa@^PpiGRiT=I2PDGc^7zQfyYe-wXqZBWhlQ($&mf9jPX5y^6e-uR^@Uo@|sNc zIg0XKC^zj*ti!{B3tKY?kb`;K#`caEJI5DDkP74nJV4~0oWdymomG0Ubre`e{KvO| zZvo!|z6E>>_!jUj;9J1AfNufc0=@-&3-}iBE#O>_!jUj;9J1AfNufc0=@-&3-}iBE#O>_!jUj;9J1AfNufc z0=@-&3-}iBE#O>_!jUj;9J1AfNufc0=@-&3-}iBE#O>_!jUj;9J1AfNufc0=@-&3-}iBE#Ogzc!wG`+JMPa`d6J_qZJ+2dDd%C=l`rbHl*`x2kyt;i|)|laA5l1OJc5^jP?_s zD!X{ufvr{?P#&q>iD9w4vhv#Ctg2{pEZiO}DJz;+R6M7o`~y0fWphpBi-720M>ja~ zMV{z&#q+cC&tMn%qCoJx%f&|to}an+IfDH-7auLy|8wy%g6CB(ey-s8my4e#I1c6F zV+Gqe7au3OCz^;#=Zo_N$1k%qUyK)>OHG9NFAyiI-S9%Ox5f>>P(1Y!H++IPCJSjw7v8r_<*tho6wVJTXo% z&g+EIf21EOe57B^Ic>Y=iRW~@1e<(VPOHNY%TRhc{WA{zmw=!2cj6TqFXRr%Rs}fe zxm0jG(y(Ymg5>Z%g7Hrv5fHXNaQsqoM*8hW$v+Z*NXzH_3d>h=M$U`JrJRwt;x81L z8kV4_!fidd^f@F>XU}(LF{h$xT*isQ54hvAJn$7Bc&i8A?Sb$0zz03>Sy#L3xzz*T z?}2AL@PcdH<Y94^N#)bw;I3Qfd{}q z{C7BTXnWImH8S^uTv`;3qxs*>jCXk>qv@i{0@u4}6UW zzTX2s>VX%Qc(m(*w|L-tJ@BI*__%oDCzj!&kZ|5_ z%p^7NtO7nkC3F6_G(X3UNxF2i#(AI6sPXS>ocC|s?p$n8sa@U|9o78x8s~L?jmCEZ zzYyogMS8s0-=5I?9A{Q&{yzgR1i$k>Auvj{%jX?lmZoT&_jPNvocS8(dB@wzu*Uhk zz{j~5E_j1z*LlCO1N?6G{4wwgje8-eEuWuh`5cE?{zbU3c9U~0aIz=ww}V>F%^K(Z zHsdJ|IiL2x@6&QPZrr8izo~IvmuG5xEH1W5AKnia>jAqO_ypj5KU}6Sz~@R_S2oB- zEr<7Ab-L^?zzcCdP^bNYtK{P#<7S^Z9{3XA7v_z;m#76!`ty02{baqy`P?x@H#`~> zZw4!2(CHE%C-^+h{$H+fUXNOIov_AvAIS5oSL4p{eG2$==pWK{fO_)j2kvHvjF#i{ zhanIC^Us5vypi`#6A9OCndAc=_%h&RPhOYVo@+JE=Ym;U*q1%<|JJy3p8iVXd|$!x z&mZgVCzk>zeR!P>>UKY&ao*2VY5XaT^EsE}z+sK^{+8#(SS$=~_6Y)~cKKYu&ij1s*Q3DQ^xUWUc|Xs7d(1;l@p#oP z?@Rl&-98GO=6fgNB0Y6jhv;cTo^ZZDFH^AC%yL2sXwvv0DMx1vG6fg4Zu%DiCp$ai zXPd@(pT_oo+Cxsi=I8TQP`CYijq|yK?R-O_yFPIb{7#9F6VGWydUc$91UTtmrq9vE zy50ZNIPb?;|LGTM`&TH&kmlb2d_vyH`{d12xH0Z#mUkIMFG(m0<(p@w|!(>R~g89%6T zKL0jq{*xN#^D@hygg8KP%;{G%fRjFa&f<8!NaK8e!u?t=<&4~Sr8Pg_@3EXmB>%|$ z!(*DC&p|xzPHCL)vDrT-PgZ*J`F*YK?=p=$dT*Bdp?_c={k&+lrS?_pSomVp{!Wea z`5oPn&&w?T8kJ{#CS2}rpQ}9Z1`qr(5Bwx>>Hl+7yS0jlDF`@(^ZOdsbEd}m{+9ir zLgRc7iL8A7SL0>#RG$56&ij;nzQudabm?Z1#Hpl>;oQh zzOLo){Vm7&V;=nHOjUaFJq(XavBvqHndPj~INv`o-UZyPUtiGtd>`AQ`*kk*L-yx; zyu})?(m3CTGyl`TX%R=LMB~=X&=+jq^P@kIT)#-SqF! z{7!p5u5o@J#CH27aQqwb`~@KC&+l1~mCyL;?(JR)ob2P2|1pj8Jv_I&P2>FDkmc{z zINu*K|9@zl@6UO?uEJr5+U0xl=d_;NHO}u}c)q*<+|6!>H9x;cWBzGZDEWL}U8d!) z2F^NbNRp2Ocaxvea`?R}%ip4Lem}@@Y@3Jt-I{-ENVVFi8(#p&AUn)<#H*DW=l4e( zPj1mT-#;JFa`pgsTlasY`T2b%>-nO_o$*{WM3U)}qJ)^)DQ30^%L+HgtaNz202~7x z3CH8srfB#UYh62G?Sv)Msq}_*>xv?xC7O(`kEPPlq?K;7BJo6fGzB3o32S{k(HxFj zE$KuuWra6%ib$fZBOXmhTZ)R^IjnWD_Lvn;Cc|A;v^|~d66=!Twy4#zp{=b8Rg9bk z&U6lIDw>Q@bJ6Av>x&{C9m2Y~wxn!XeZ{J3YejWMRdu~p;)2bSSh3=3tf;A#`FSc| zTfKUD;|htCS5+^oxUsg;YN)QSsi?K;Zm4ghf<;R!Zmg=WXskwNbwhd)x*ko3WAV~r z3p%Ew>&Y+)E7BT<6=ErCLwhQ=zCGGf^NH$rU8U{aSLu>f8qy{6piat4hSRZxwXvol zJzpkkYU3r<_!RR~eRlk3$p)!s!O3kGlO|`!pXIdFKvl1PD-vVrgwPYTUefWkY&Fq&1mnPb_NLAdS*q6L+x> z6q_GOY-mqcZ&X9QU}H1_->zwG2-mb$SI0W5+eBNmEz;2yT3%bTw9=YaRI(7P%J}es za7#-vno5DW4sKK;TSw!$V&+Ga;Z(GGb-87&Z{J`=Iy*}iT2^}^*#_s1VVZ@h&AOGX z5TM+_?m$U#I)>@ek$~gXtZuMMt()eR%){Iaw{+3ejNDQU^z_|SgMrRgSC=f1;!?5p zNYsjUBqFUIT`%w0kZQG>!;oLUvQ=A0*)*Q^FlM}A9`tXnUzwv32BsJ=p`}($V~N#B zTq3!ly*<{xUK+EkEwM3Lb0+(=jxcO>b;AOy8scI5MOMQ?m_OPQPKTw?@alRD6Piq< ztYvNKb@A}}RD=5fRn3!D0DGBbMG_rd7Aa(fOKNJHORDQy8_)q#yx2ky?NL4(7t7kd zvZN-yk<8GjbV$Tg)pg6D;-VN7Y=(nSE6UB*tg%Xq%7j&OgOy6R#M-S5sVKtu&1bR@ zJjm&L)s-b^6KYGFkl&>-F*c#W77MFDBC;{JNeXxfEH}w{Al2f@0734vq>`+cj;xEr zW6Wui=!mwPc^OPgN(>82^OA`h((@K1!|g4JHY*&7U_xc51ua|Eh`?%8YFaTX>o5*9 z+ZI?2Wzv>3TXJXIf=Dw~uyzlH7h=jrlN+Nnxx=w`<$}@nmUvABoiCxdZl#mqSUOb` z!HAI;VSHi{E0Ibkqv5t1<QrBPv4-nF8@Z1yY3CFl|7>@z|}=oF2kDkpvBF zPIy3P$^GXeI7c_B=^;aNYxN>ISHfxdOx;R74cw^v(xH`CQ}JgJKbe%EEaIOmR$9rro-js^d$eJG0tJ89hmGHg`9P&h$D;@ zk2OaSWWvd`MROrru@vD4Mk;NKwb!)Cs70Yqh7oG}Y(gKrxpH1^&^!Bzj%exZZB;fH zWy5C~ym%cV$%rwvnh{UzSwY(-Y&5Zlqo7-Z=VnOxkP)oJ&}q>qaE3il^YljH#cOz zmuYS9`Y`s6`AxwMt^S>6HqBG*geCJiDp)cgq-x^TWw%C?37&NC##t$p>iu(dwOp=O zV_CMN>&>#^+{Fe1yUM~8rPb65cVN|Pskt7TCUkgXRmTkoRjU_R%Qu9REtu$at!?#? zn4OQQ-1(^Xt`7MnkWbqht0RrADJ_6{<@?)|p`A@unUQNKylxfFIMK*07M2f+dl6_| zGQT5(@@JS(ZnIEr?$owR@9q$h;whYT&SsI0 z!0=ckoy(ndbNi+?r_t>>>^4hjXFeiEU@VP&6=F)89aG**XAyMd2KMHbc-_XfQ01C6 z6-#TZlA_Y0a&nfDNC=qEW{ZRZ`r6^Ko<;9rt0udWV~w$wuxrN4Pd;rQ8Hb zpHoMO`qt_jD6}m|M&%A3E2`=dZ5Y%h;RdycfhoHVY)XbZ-rMN|CmA_xSPX=`zM!GX zMkWjdO#Zi>*yw;maT6Y&XlqNf%b}^cp7%KR#>dJzDMBl3i{y@|(yD2wvM`itxsn}s z+1|r5G)jw%j5lh>Z#Ax}Lu4i-sG5d<`C$mn5@Z%XnWO@h5AU2 z^TR6MSJ4Jup2bVr6B`hBlh_$Y%@bA5XuP`}DwZ-@vCKt`@)(J~;|k`{#q9A*F8Ate ziiWd0odvjX^4ROpmaQckaT>u64sHR!J-(HA%r$aGmmPBU484$ai^Xx6!e$I{TjwYm zV`y>OpqZJw8!4G57nS<(CR$OT4((SP(&2VAmAgZi2l$3`8E$R#k%f1%#=Lt^hp(Ky zi$0p@!@In~!VwIYSvZCnN3fg&47rEvc`TW1PgvL+&^?{K!RC7;+&CnovJ~4ktAtLt zBb*FJIk4gU(;Q20ieZ1JYtR`9k!>R%h3cTU4s*~%{j0}S7VTAY=EBIBRN9`{RGo7d zP*g$-59aQ=SZB1w;ZY6gh3gW@P1w`a&`t-NoO<$$k#X17h!j|P21j&RXUk~zXSZ@A zc18_2w&-OL%dy;8k=Ip4aTH2)!G)_g>X;&r0qUefOB$VR!ksQ>8;?m@N}DjX$)jT+ zwoY{6O}jEFQ<>MJZq9^OQ5Bm+UB%@>r1(r1B5217r|8z%!sWcY71@YgA>E9~u9bU* zLltvvhoj>UZnflXC#@+K?&vUN^*BH)_0D?pj04*lH_vbp$C;&!EQry?O=c=5D9CL^ z!ev`=Mjvw9I2cZ3dm!zTTpaE_Y?Bb>mXf{xrv11Mpl8|_xoE7mqC8?}<%4=r!LHJS z-Rl;^o<-yzwlQ7T6*pbwb)=!Io~iUUNgfocA+yDvy>j}Nz8h!MK|A;IAsxY$OgNp+ zy%|9em{-EruXGiX3@5wfJ}x&BmeLy~dGvVqTOWG`#ME-!@4i==SQN3Wb|j;bSSq`w zm@N1*?VD&7G&fCpA2ni0d(U@z#yXG1&;6|qDR9;= zBHRyGk#I*ig6lWLrf_p2nR`DD(SoSS~%T=LqZdCExZL3yN>|DrP0@wUVJQ_~Qg%?30_r1tGulFM92sVEsrbEcao?E~5UN6tyg<-nIc zYdDta`R3~sx<-yU&T{q$+hgQD&~ztkyRb2vPg4;wCFk{7O(X5a_)7{npVC(p8fiC- z4wsawIkrN+zewUJabx?Y7+#9hM^hWxu*s;b#M?D|UE%E8ey_LK_BCV!-tfXdTn;m2 zx!h4LrK?+h)u!UYdpgljC`KUU`%D>$-^Gh0GEYfKx)sL`yhOqqh;n(4Y+>K;F^6&A zNpGV@1~WZd$>g-=oH5ox@tpVaUdc$_JB7N=F|86#gE z(wV3=+UYpzksG)M5aVS*t^+T^QR(cy@{s+;10Ct{=E@OOfWnZQ=r+J*g~FEeEbrZ= zrlKg-)s_x7gQk;;wsNXHk&YJOyBS5z8)EU6Ik6T|L|^ntb_j{KhEuJgsHLkN5)@4* zRmn!YsKX8{Cu5-uC#*14(5a4ink2^Xe>&QU|K&TmqGW=OKcXnws=o)-+Jd^AgV0FC zigre#9cjD>6~K&l&Vi$x$Ufrjy4k<>=Dj8|i# zh;A0~-7Nhjw7-Gi>$CE2oaJd5{P&@l;_GTTPeD#jh$7?z_;7vxJ5o$ZzWETZ`7A)@ zkNDeWTz`*FFy-&za=T;9%1DRs!B1+k?f2{YOzU(-`Z{SoK3t!_Pq;$k+&=&PDyIDR zB;^)dKnNYcb`&he!3vX3qNEz{n=}IiY;`?{G2Y@zcdpS56b%jE`AA zQ>aRrc5ufOjPI)FKA%R`tk1toV)_a>sPTl1G$rrB$E?qP|BEU630Gq~a{f!YKC8)p zCyePG()aSoD#`Gk9_hmlL_P^<<&wrnc>2$NQ z<~5-M$k0zgS%3aJW=w-7Gty4|Ujl}oh{?H0to--Rww|J#?Qbe)=eZxRp#VSW%GT$< zi?;17>;DFI$^L9+{`+a$&rly#O-2o9QS=eg1ov$94S>2{#_D$8@}-KL7rC7X2Y3JVE)t{U6t3WD?j2h+x|#@m_nZ4};`6Up^=Gi6y3mmo{~rU7fy4j+ literal 0 HcmV?d00001 diff --git a/serial_debug.cpp b/serial_debug.cpp new file mode 100644 index 0000000..9cdd828 --- /dev/null +++ b/serial_debug.cpp @@ -0,0 +1,269 @@ +/** + * @file serial_debug.cpp + * @brief 串口调试程序：接收数据并打印解算结果和原始数据 + * + * 用法: ./serial_debug [串口设备] + * 示例: ./serial_debug /dev/ttyCH340 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// 协议定义 +constexpr uint8_t FRAME_HEADER_1 = 0xBB; +constexpr uint8_t FRAME_HEADER_2 = 0x77; +constexpr uint8_t FRAME_TAIL = 0xEE; +constexpr int FRAME_LENGTH = 15; +constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyCH340"; +constexpr int BAUDRATE = 115200; + +// 传感器数据结构 +struct SensorData { + float gyro_x = 0.0f; // 陀螺仪x + float accel_x = 0.0f; // 加速度计x + float accel_y = 0.0f; // 加速度计y + float accel_z = 0.0f; // 加速度计z + bool valid = false; +}; + +// 全局标志用于退出 +volatile bool running = true; + +void signalHandler(int) { + running = false; +} + +// 解析接收到的数据 +bool parseFrame(const uint8_t* data, SensorData& sensor) { + // 检查帧头帧尾 + if (data[0] != FRAME_HEADER_1 || data[1] != FRAME_HEADER_2 || data[14] != FRAME_TAIL) { + return false; + } + + // 解析4个杆量值（小端序 int16） + int16_t x_move = data[2] | (data[3] << 8); + int16_t y_move = data[4] | (data[5] << 8); + int16_t yaw_val = data[6] | (data[7] << 8); + int16_t pitch_val = data[8] | (data[9] << 8); + + // 转换为float（假设原始数据是放大了100倍的） + sensor.gyro_x = x_move / 100.0f; + sensor.accel_x = y_move / 100.0f; + sensor.accel_y = yaw_val / 100.0f; + sensor.accel_z = pitch_val / 100.0f; + sensor.valid = true; + + return true; +} + +// 打印原始数据帧（十六进制） +void printRawData(const uint8_t* data, int len) { + std::cout << "Raw Data: "; + for (int i = 0; i < len; i++) { + std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(data[i]) << " "; + } + std::cout << std::dec << std::nouppercase << std::endl; +} + +// 打印解算的传感器数据 +void printSensorData(const SensorData& sensor) { + std::cout << "========================================" << std::endl; + std::cout << " gyro_x: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.gyro_x << std::endl; + std::cout << " accel_x: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_x << std::endl; + std::cout << " accel_y: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_y << std::endl; + std::cout << " accel_z: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_z << std::endl; + std::cout << "========================================" << std::endl; +} + +// 解析并打印帧中其他字段 +void printFrameDetails(const uint8_t* data) { + int16_t x_move = data[2] | (data[3] << 8); + int16_t y_move = data[4] | (data[5] << 8); + int16_t yaw = data[6] | (data[7] << 8); + int16_t pitch = data[8] | (data[9] << 8); + int16_t feed = data[10] | (data[11] << 8); + uint8_t switches = data[12]; + uint8_t left_switch = (switches >> 4) & 0x0F; + uint8_t right_switch = switches & 0x0F; + uint8_t crc = data[13]; + + std::cout << "Frame Details:" << std::endl; + std::cout << " Header: 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(data[0]) << " 0x" << std::setw(2) << static_cast(data[1]) << std::endl; + std::cout << std::dec << std::nouppercase; + std::cout << " x_move: " << std::setw(5) << x_move << std::endl; + std::cout << " y_move: " << std::setw(5) << y_move << std::endl; + std::cout << " yaw: " << std::setw(5) << yaw << std::endl; + std::cout << " pitch: " << std::setw(5) << pitch << std::endl; + std::cout << " feed: " << std::setw(5) << feed << std::endl; + std::cout << " switch: L=" << static_cast(left_switch) << " R=" << static_cast(right_switch) << std::endl; + std::cout << " CRC: 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(crc) << std::endl; + std::cout << std::dec << std::nouppercase; + std::cout << " Tail: 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(data[14]) << std::endl; + std::cout << std::dec << std::nouppercase << std::endl; +} + +// 初始化串口 +int initSerial(const char* port, int baudrate) { + int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd < 0) { + std::cerr << "无法打开串口 " << port << ": " << strerror(errno) << std::endl; + return -1; + } + + struct termios tty; + memset(&tty, 0, sizeof(tty)); + + if (tcgetattr(fd, &tty) != 0) { + std::cerr << "tcgetattr 错误: " << strerror(errno) << std::endl; + close(fd); + return -1; + } + + // 设置波特率 + cfsetospeed(&tty, B115200); + cfsetispeed(&tty, B115200); + + // 8N1 + tty.c_cflag &= ~PARENB; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; + tty.c_cflag |= CREAD | CLOCAL; + tty.c_cflag &= ~CRTSCTS; + + // 原始模式 + tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + tty.c_iflag &= ~(IXON | IXOFF | IXANY); + tty.c_oflag &= ~OPOST; + + // 设置超时 + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 1; + + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + std::cerr << "tcsetattr 错误: " << strerror(errno) << std::endl; + close(fd); + return -1; + } + + tcflush(fd, TCIOFLUSH); + std::cout << "串口 " << port << " 打开成功" << std::endl; + return fd; +} + +int main(int argc, char* argv[]) { + // 设置信号处理 + signal(SIGINT, signalHandler); + signal(SIGTERM, signalHandler); + + // 获取串口设备 + const char* port = (argc > 1) ? argv[1] : DEFAULT_SERIAL_PORT; + + std::cout << "========================================" << std::endl; + std::cout << " 串口调试程序 - Serial Debugger" << std::endl; + std::cout << " 设备: " << port << std::endl; + std::cout << " 波特率: " << BAUDRATE << std::endl; + std::cout << " 按 Ctrl+C 停止" << std::endl; + std::cout << "========================================" << std::endl << std::endl; + + // 初始化串口 + int fd = initSerial(port, BAUDRATE); + if (fd < 0) { + return -1; + } + + uint8_t buffer[256]; + std::vector frame_buffer; + frame_buffer.reserve(FRAME_LENGTH); + + int frame_count = 0; + int valid_count = 0; + auto start_time = std::chrono::steady_clock::now(); + + while (running) { + // 读取数据 + ssize_t n = read(fd, buffer, sizeof(buffer)); + + if (n > 0) { + for (ssize_t i = 0; i < n; i++) { + frame_buffer.push_back(buffer[i]); + + // 查找帧头 + if (frame_buffer.size() >= 2 && + frame_buffer[0] == FRAME_HEADER_1 && + frame_buffer[1] == FRAME_HEADER_2) { + + // 等待完整帧 + if (frame_buffer.size() >= FRAME_LENGTH) { + frame_count++; + + // 打印帧号和时间 + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - start_time).count(); + std::cout << "\n========== Frame #" << frame_count + << " (Time: " << elapsed << "s) ==========" << std::endl; + + // 打印原始数据 + printRawData(frame_buffer.data(), FRAME_LENGTH); + + // 检查帧尾 + if (frame_buffer[FRAME_LENGTH - 1] == FRAME_TAIL) { + // 解析并打印数据 + SensorData sensor; + if (parseFrame(frame_buffer.data(), sensor)) { + valid_count++; + std::cout << "Status: VALID" << std::endl; + printSensorData(sensor); + } + + // 打印帧详情 + printFrameDetails(frame_buffer.data()); + } else { + std::cout << "Status: INVALID (Frame tail error)" << std::endl; + std::cout << "Expected: 0x" << std::hex << std::uppercase + << std::setw(2) << std::setfill('0') + << static_cast(FRAME_TAIL) << std::endl; + std::cout << "Got: 0x" << std::setw(2) + << static_cast(frame_buffer[FRAME_LENGTH - 1]) << std::endl; + std::cout << std::dec << std::nouppercase << std::endl; + } + + // 清空缓冲区 + frame_buffer.clear(); + } + } else if (frame_buffer.size() > FRAME_LENGTH) { + // 缓冲区溢出，丢弃数据 + frame_buffer.erase(frame_buffer.begin()); + } + } + } else if (n < 0 && errno != EAGAIN) { + std::cerr << "读取错误: " << strerror(errno) << std::endl; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + // 打印统计信息 + std::cout << "\n========================================" << std::endl; + std::cout << " 统计信息:" << std::endl; + std::cout << " 总帧数: " << frame_count << std::endl; + std::cout << " 有效帧: " << valid_count << std::endl; + std::cout << " 有效率: " << (frame_count > 0 ? (100.0 * valid_count / frame_count) : 0) << "%" << std::endl; + std::cout << "========================================" << std::endl; + + close(fd); + std::cout << "串口已关闭" << std::endl; + return 0; +} \ No newline at end of file