From 0e9f494c50be6552ae5c3c448b235cd0b5825823 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Sun, 1 Feb 2026 16:02:18 -0800 Subject: [PATCH] message type updates --- aerpaw/MESSAGE_TYPE.m | 9 + aerpaw/build/controller_app | Bin 25624 -> 41096 bytes aerpaw/client/uav.py | 282 -------------------------------- aerpaw/client/uav_runner.py | 140 +++++++++------- aerpaw/compile.sh | 5 +- aerpaw/controller.coderprj | 7 +- aerpaw/controller.m | 66 +++++--- aerpaw/impl/controller_impl.cpp | 163 ++++++------------ aerpaw/impl/controller_impl.h | 26 ++- geometries/REGION_TYPE.m | 2 +- 10 files changed, 202 insertions(+), 498 deletions(-) create mode 100644 aerpaw/MESSAGE_TYPE.m delete mode 100755 aerpaw/client/uav.py diff --git a/aerpaw/MESSAGE_TYPE.m b/aerpaw/MESSAGE_TYPE.m new file mode 100644 index 0000000..338c5ee --- /dev/null +++ b/aerpaw/MESSAGE_TYPE.m @@ -0,0 +1,9 @@ +classdef MESSAGE_TYPE < uint8 + enumeration + TARGET (1) % Server->Client: target coordinates follow (3 doubles) + ACK (2) % Client->Server: command received + READY (3) % Both: ready for next command / mission complete + RTL (4) % Server->Client: return to launch + LAND (5) % Server->Client: land now + end +end diff --git a/aerpaw/build/controller_app b/aerpaw/build/controller_app index 56ed54ee51c12a01914b961fb70b95c9b0f1581a..48b5e4e2495d545c8b1059db0c00aca54adc4c18 100755 GIT binary patch literal 41096 zcmb<-^>JfjWMqH=W(GS35Klk}BH{p{7*5Q9G8h;b92hJZ_!t}*xvV>jT>t0Fh@vqdSBk;xHOnA1G{Q3wqVY&gTZv#{xj1~YXU|?W?(Xj9YxeHp~nXZ|MUU9yj5m=uZ0|Ph@L;Nkkq{tw`01^XPslmX&P&AF5Ni~

>QivIa{&(ZoH)$i zgG2ou9On1qP=6DL`Z65ipnMH0laM360f+fhafpNRDK>L*aM-JZRMjBctBxalCNMBC z2r@`9Bs4)(OMon7U|`q;6>n&Uhzmf)%NQZ*9U4)^8RFy9bMy1!i%SxVO5)=g;ypur z<5N0?Kk3;-mb6^K*(_Q{b#jFe}q6Gd(ZADAhFw#1AepPR`6@h>r>`F*Ju6 znUt6kpO}-ApIio(FiB1=Dgv{N3^VhKlH&a2?EHcf zhImlSBxlDbXJmt&z>t)gm%@-ylAl-1kd|MNn#T|ypIMxrY-Ai?T%4Skm&O3nnOee7 zTvAkAQp8Z4nwP>*lnM&B_~eSj_{5UbicFBPAm4$^E-H!7P0Y+=$N|MS$TX0uv`kQ# zRi!c{CMTyBlrW?v=j0csGUTS_7N?egsN{l5hT_zm)Z`L|oXn)+l9Xg^ZBWHx2IA(z zS?S5i@x>sXAxJzK#4=-WcXp09)-zymj)-t{@{BjsGiGr2@pN*Ih=@1VGto0PLXy$5 zM3FN>;i5g@KKMoq+?1 z&&j|A=5aGHG4L?3FfcOkf@)_l3#6U}+=gLfU}T79VgNVSK;=MyS|v9F1201{v^~Jc zz{cPNl`mjmV7Rx^gFt28l!?>W7#KJhu0hpo0rS}y z9zyv$z zBLm1K5OpyAfqDoZ)K=kwFu~*rByoPQ2!yzRBrXpT0+Tn8#GzJ##UCJv!`iSQ2@Ys^ z2`WZqK>|<=D@Q@?H&9y%Dhw}=LE^Bs6i5zM9)rZyK>{!g^)IM-53&O$%z-2h4PvmA z0FpSgDGnBwKoSSF4Z(t-It57_WH(G)14$edrZ8~>Byo_NVB!`?;;^^`$vGg2!`iAK zaStSMZjb;J2Ox>_K*c~*1d=!}NC1ixki_|*VjwC5NgR}ZL1G|WfFv#m5`f|gByr^S zY6FtEFjOsw>Oc|)wX;EDAUpv{Tofb##WRq^#h_v!Y5|hCI7k4BS0IT?K*d1R1|)Gw zkN_0#KoXaNih-yDNaC1mIlkd zKoVC4i;NPZAut*OqaiRF0;3@?8UmvsFd71*Aut*OqaiRF0;3@?8UjN-1U~c2{qksj z!{O1*dNQ7Y!K3v+3Df@z9?eHM4uie+-}F>G1H*sSGw}=z{PGSA|5ZW!43ON*2mk;7 z|F3!|o`E3)G{p7t0+_!G#0L#+y*vQsZvyc_Ls~C4fcdLHe9%zV%LQQmA`l-mg!OU) zm_G}|2Mt}lYyk5of%u>ytCs~}eiw)j8mfAk0OmJ=_@E)GmjPgY6^IWSN_^=6<`;qZ zprNXl24H>`hz}Z~dZ_^BCxQ5&p{bVwV15*c4;qqs$pGdDf%u>y$d@1ff&A+Q;)8~! zUOoWxoj`ohkkrcyV7?WI4;qSkc>v5e0`WmZP%k%t`C1@8Xz1zX0x(|*#0L#Iy_^8% zOM&>Hp{AD&V7?HD4;o^6Spep9f%u@IrI!g{J`;!!8d7>00OtRSV_?Vt4JEyF0P{bA z_|gmv3@;79{8u18Xh`X$0+{~@#0L$5z7+WP|34@_7{2xFycp-v`N*UB$%lXtSHlB_ zCp~&?Z!s`1{1=@Z%fRp@O^;u`1(XIDJbG^s*iTQIMue;?d3O3N?V^|ARDs zc^3u-n0}D^8f>p7GB7Zd8i#r`+y07SU|{g*d|JZm(QT_12Xf+z7zTzH$^ZZVKgPNy zk%56R_AtV|he3%r4Z#PQ_cHze|No&LoliZQ-v}Ic{R2w89=)PZb-@0T0=cvEnn&mH z7ySSJ|L=DF(|P>G4^@zy=sk#%?;zJgl)U--|Njn9$ecV3GI2sIGUd_y#vsL`n?)5g zvh306`or+Ri);V?|KA1Dun&}n_Jizxas1E!|DCLfu?!5|tkJOy46O$$s#_^#K#Zan~mxllFHAGcdde`t$#PTBqxy(6f?U}f zddG1GDEE1EUfcz;3~X)Z@fVl>{{P?2`ZfmSFW(2dKndKV^IYfg7gB%z|3CObzU4rP zv`4R~48#qtpt1tuhF^dF|9|b<&AKUufngsgA9o&qVfGhU&sS}bp{#-kJ;y+LBt3dX zUqYm|fvklXxBAcj{~q10KTmZQ`r`VZ|NkN3>Cybgp|kc3S|A>Gy#aD>H>)1V zoZiqI9=*N~Ag-VD`~Uw=*E`*=cUlirI(YPoc0+7zLBy&h$i~h?-L7Xkk9l-nf3Xt6 z_vmG<1gV7xihx`k;?e7R$D>y?9qbdZ;%`DAzljEdRQ9sIiw31da9Vl%`~QCrBvXUo zrs{&!LQLHUQiSH&SHF=xi{?|0URF^4_2_hc;L+{+0F)@5gh6(?UV!myAp9Gz6+F6K zFLX0>x}E`f?f45pkPEt9FL*Q`VDad5y)eO}*Y$!&r|S!kZV(pm=yko}(dqiZqZ@=7 zJS-3KPd(6bpmaw!>$NBbhR)+J&j0@Z|Ao%q|NkeT$B-$)(T#5s7(p)DyW<}y1h#@o zz0Rp?{{8=t;TKUoEd~aU?pBCOaIo=!LZq|x1xRo6UJ%7l>g&-f$_7%|%X$_R!H|T~ z^b2JCA-HyMbmT$>ds+8tf*b)BjQ9o3lP^n|7#NPXp85CxKPaCZYdsBOy<}wq$Ke5x zj^0*KDenQXLjULg|DYO?2cnYkHVe*3Y=l}oBKbZNa z9B4Vf-x9*ez|al$hQ|z#UXi09&v|{O4(kAEYdugR3h|OPIFxGHcYuo3 z*R0?OYB|8)y7TY<|F0P*cyvQjZ|CtB@4x^557Fb%9SSPZM8G9~3F`~iAFxc``J==2 z5C1mTKg~Z_OFWx@u-5xF|6nOGYyQDf@7(-@xkS172Xnn;^AE-n_U0ch%=Y6@bod-SpzsxvS^>Y-qd<~J6dwP!j@ zPatQ;7lsF#YtJy$iFov~c1M9i=J<jRJG+6Rm!9v;1-bJWn& z&q}ZwM0qUv9i#`8a$bUJT}aAl`3B3=b3yfguV@a$oSZO_^TFvb5zU;75WYt*Dr4uJ`%39q2s%A_J5iL2~zBD1hZa7Rw_va6<%P8o-_{hlqLfvi?N4@X^=* z|G`PbqdW9Piib8l?m0oKyJLUAYqWsQ+C3hfr5li=Yr1xRFp zT9ci}Uo7|n3V7EO9?i8UB*Bp-2l8Got9v*+kpFpFpV^J!@-yfo1Jr zkH$Bk^;?~L-~9XkzjG_7Y(b7=P__e0G1Q5B^s)+qL-qIzP96pZ%T|yU%$VK{%1keR zvw+HfkZ~QYpF!!hw-wZv@#t)o`2YX^gbBx5KY=Q?Zm=O9$sas=d%^O(Q$gY$y{(|0 z5GYBQ@PINORP*r{69pI;CLC}50@4iXFyyj;r9u5HkIq(5_vm=*52)m88INwTQ1?`j zkjHVB3m~^OgB36~?*$3+m)$Rdb3KwTz?}n52Noa~ zbwXVU9e?4^%D~Y4fe};%o@D?90mwvf$#XRn9+AJe7#O+*IxigjA%F0N z6yu@hNA{YBIxj#yU@6RfZq z>>fslLThl$@%LN@bvVG%y%mfey{#ZeID!jcNJa&Prbp+o7ik~=|L<(&`2YWZ^Ij0i zP|pQPDTlxf9+v-*2G7ebCQzt>xEKMyp9>U%I0F6}Gc@2!s=8aj+Ckx#0M5SP+|$Yl zDnXFK&0~f~_Z(;#!8Pb3G=zXO5Q-(m8=%$~S`EbCvKO3u;HKUT2Du#UjDH*;XL$5( z1*LOH5P`C2=kXUy-h)Dj2OL5mlCcCDLZAh#9=)Pf3ZSIUsv7_@3!D;_K7dOyh=1YD zm>!6zM=$Fhd5}`Dwcp=^lpf~a2F`bodPD)#XaE&k_g~xqOM}Y9>j*uEA%aMHz((o7 z&0mL5JN^Ct|4@Y%;0({-3i2GpX&5p2l>_83EHMf0>VdL7Xz_A0IGZzi^omZE1AE=b zAEXWJb#;i_Am+opJ`p17(aV~QaN3`DFt39OqR!(lG{Cj>F^|ssFCKv9K>juWSpf=+ z>n~2f12wlmk;e!zAC}frKpjx9qhKP9zyJT=4T?W_5pn#*A9ex($B!s9QNz5D+MQ}t``UY|NjqZ%6W9w{_yB51^3oaMPN3-6m*8Z z0Cm?qxfs^B_!I5u^;nK{6lYq0Z1Z zAW4(Bt3_Hjjf3m^~OlYHxtU1{6dWJP!V__c-{%$^%?^cxWE- z=nVbf(Omn1p;FwV+4X}1$PQ3o^#*`!hv;M_j|u+VFMBLIpaP-sAglOT1&@fQpJ{r^AV zBs312-zb1pb(VgBhI2P4xP3oBQ)n?nG1&Pq6CsK02P8tk$-ETQ=IC}sYQcJR)}HX_ zEIokSy!l~xz@yvsghw|hX)u6B96UNhL0RkoxP=PoVow6q@Ss77?od!{KJb8~)(4;* z?$PZF>NN4dI!zZ|!CF=xg#57@G)Mt&U_(L%$sgT893I`iA3VA_q5iH$QI80t3mvYY z)c66`%WkNB!%)J$6V#%4%?c`m89)IJXcyzli@aS{}rH}(2ouE_#!l64nIzvIZ z;s&^l4<6-;z2MP#Z$D_r@&yZMa0r}GKt)>Ui*BJ#0dQh(xm3dK(RmN#*-yVfEntXS z95_6B16aW9&H#?r!XDkB5LZId9v3KFJisM7q-1G+W8l$Qdjm5OgOVmBV7oyH`2{3s z(fag;FF?tYph=+42+}B9rz>{T9zREz)>#XQU2skUjn$#Vm+KFY=Gq_Nga}V(qW>OB6!?-^t; z7E}^}s*i(@m^>ISK+-EH?ln9>PU|cMW!oDboxY$#`GrR}Cka;G4E)=`>LHP_o|S>&wG^lx2aQH{9)B?hq_Fb_C{ER%gPZ~CF~dR^;y-AL zECS^sL~a1bA*hDH8jr_c%w=U@@UV2ffF&r-i-CjUI3y@KKta(4cE<4+&QC#ZaJ|s{ zz#ge9(d~Kz95%ieJPy8O^EmjB*@N+*M=xk(7F>51gIa;0f!c0RibXBX7@r}>FsLT^ z;c*;PYJ;QFv(HRQrt6yL!F#uH<(4k#Wb^@i{ z6QDu?)LDg;e6BB`MIWdo4l9E|J=Pnb9NW1U-1X|*3hFl^hvN&w10KDiGeH5=-3n3z zZ&cm`2UII)(#@lJFNgvS?ShnmLa4LV1C#D?R)B2jbZzkHc5U$JbnWoy24Sdh=md{W*BKt&tq{eo3p~1G zC%_9n@Ssz7=nRj}&=sIk6kEaf^DCs_g9n}?$kCwo7Q|)UTcP0%j)>0VFWx~S4D9)4 za2T+7bVH&EDHggN1U$4Mq6dF4d33tofCWk+NIyJ`!M+DIFCq4KPX(LX399=cX`)x8 z6%tU8PzN=-AWAy}1i$nl-EKaf&9sD6f&h@GHTt1D=n{6!c$N?nS# z6abe{p)WjoL!X1%b`vH*Sl}{C0_3e;*XN-0#{@PLR9hoE&f^g%W7}Xs+GBz~2JeUhmNjN?M@$5$uq7kYSMIz5(QzSs*@qwBHrl z7Tn>{833B;f!3nFCqU5yt`wp9n*}`F2sZhGN2fq{=nZhBgl+&SgysrUkQz{J3u%4r z@Bo$BodE*fp)WcEBtWqPYKXz-j(wK97{A0YWJ7c_#0R$Vte0C@r&i?E(jH)!Gu-XyX-RLlmN3wsS}LHvN0 zw;=y|z*@T=ouNC>3rA3kxAXW51F!=RgUhDQ<1b7>^?)XL^r+kQ0H|d&2{bMQia5~N zFg#@6-Up?7*Bv1L7(&=!zkLPws^P{x19L%HF%;C8z5$KKyPy#0X7FGHwO2ruE?DnQ zkYY>M2Sq|K*GvYfvvz$@%m))`WM*Ih`xD&oMjow>2D<_|ULd&-ROvU@K42)(22USC zS~L$Jb&M~lGW&p>2!DdAXy{xCtfq#AIw;L``huJb?PEY=AQaq31!X>1a)i}?$hoc? zyk;<79MX4&TMe=vR6~RN)8OJ{4-*3evP-*Nf52N; z@UjJ*-a*L_)PsS{q=JF~RNaEx`QSDgW6u8 zs07WBgJ{%7SI-@Iu5W&W)BJ7@P~m~>+qRFy+o6GM2Pn575*sKK;1!|k2WY;1ejDT{ zaCr(^f&vf5&f_mOf#=eXztCj_B}cT<>IYOD}+gi?O4y#z&JcfT{~{i496Epp*mZU4fd$t{*($swRNiW;Z;* z>qdys4~>kHqT|Np;uaRcN@*9RU4A25N!d&W)V(hXF!AgqD($sTw>s$Ed~4boez zgN)Dhnm`+6pn(~Xzb|@pe)0f~)SiFw5n{~`4@82!;Q{OYA&9wb}L`ojCh|Nk$ZU5AGNGbjY6-2jIGvOS;?1aQe~ z1zHjUDj6VyJ~zO%f-j`F1VsR-Uki2$I9fsdEKsTfYXyx}!$xO7vDsPr0agVyz5z{l zb?&wKk7ZqpYs0Pt1_p+G;P!6k@fYRSLDOHLb~i}mwHmaQ4VpvA;bLIm-zFje6Y2stspjN!2bA) zbGJa_>>xfPMA*6&A)=sFLP!#z<~&S- z16c>C!vc}$4S>y2d2}A@y#7M=A;<}!b}%DE0n`bd-C(Ee11&%6JpSV1BhV@dkmL4& zmWg&Af3g43|Nk$hzW{kI1m-!J?p6@H`2gcSP%v~Jf1!OJWIRZa5h4dQ{u-+BMHt2> zJOY^v>b-$OL8cqrT?CoT$^c&Hbnza@WRLic^cqQj7HT^tc#;!Aym;%)HFvjMNl8&}=5i zhT#0v4=`&0Y8L1mAFy>G^^V|!XrM|I z((;QGl5;Xs^GXyLxEP$FEQRF!yu8%p5|H{L2Dik_oYWMBl6(cwF*zW)w9Iscw9K4T zD+Pv<#3Im1J5~$|x(d;%IcYiyFd~-0CqFR-q!Fw{fx$|FL5Cs4G05FDgu&6-n<2>6 z(It{0D8z@s$I;J)A=J;?&p+IcAvhK8Ar$8YgB)3s4|lGD27@LS14KE*71%rl)&;Q? zQlO&{?+7}E$?LO$;>OUvsOS@1XiwQtpGPm!B!zBKQCROG_N=_ zJufu{WLzxR-TI})Mf#a}$vLGdsrt#<+WLm3dPaJN`bn83#rnl1Iq?ub>4A5sFfgF_ zGbukmhoDE%6yfzLILI))2QmW9f5<@zmPQI^>i9be?r{ji$|~MBz9gUS(o#4g&+DG_&;yCI*HG1_lP1GyngCHbC&4`~Uw569WV5`Tze_ zm>C$t&;S3wgq49I?)?A%M_3scQqTYY|Adu+Vflss|5?}=7^YnO|6hfTfx+|A|Nky* z3=EZ5|NjRay!hZGB7N=|Ns9M zUIvDZ_y7NQVP;@BeEPvCEDQ{{9{vCSgoS~D>+%2pEUXL+evkkER{{C&@&ErWtPBiIkN^KqVP#;L_xS(+ z7FGs^U623&H(+OAc=7oE{{VId2D2ys|Cg{cFt9xR|9=KM1B2hw|NnQeGce3~`v3n8 zb_RwsPyhe_!Op;7^z8qC2@VE^HP8P42Tkalc=rE)0tW*_-Shwd8$jaE|NmdW!N9QN z`Tze1I2ah%Ui|<6fP;Zy-mCxrSvVOOR=xWFUxkx_;oPhL|6Mp47|dS(|DVFiz!3NP z|Njn71_p~a|NpPxWMG)^=KudQoD2+q-u(anhLeFoBIm36Sx@|!an~0zk!>9q2uHK{};F! z7*>7!|NjFw1H;vi|Njf{Ffb^5`v2d6hk-%o)BpbgJPZsepZ@gVw|H8|_u>AY~{~~-044{owpn`(2Du{uxLV!`4hn-^r zBSaju=}F=A|Nr3gn_SpIT|EZSnNTJS3=9${|Ns90T5H57;KnE6#m`;N(ZFCYWvykb z0zOU(q!+Z)=;rDF|ARmlIPwX!F*)9sNQ33) zFfcHL-}(O^w3!Sp?*-NlmIs}I=yvD-e=TJ5QxWoa7#J7=?)?9+iY%XikpIKLzz}xl z|NnSod8R0^0+9P;7#SF*-TD6?d`=b2eMw+xu>CfS3=H$`{QtiaMgJ180+9X~Mg|7; zyZ`@#&N_s3Hw|e^8o&n;(ympToq!Aa?Kne`#d% z!w~YIA!i{x@^_dR7*?PB{~xp&5^lZ|LO-b7o`gqUhM9q(^5p;jPM~xSb3d~!Bgh0$ z_}MTsFw8#n|NmQL_c6TyNrS^DhM9pOAh2E-M2Ai#@CsGBA9&_aC(45-F@b!3scOEy2dXAb0=&f6(cRaQ&b-?_vV00fm(f8w10n zhq&#CVPjyZ1GO`d-H4RGYuFeVnjZfD4{m$H(hO5F*nE&17BDd|oO%c;e_39#Al!*m z1|MK!U^w^i|9{Z=ws3bcU4fhNfQ^Bn2Ll7| z)BpcL8@1u~2ZFVO?e74c+WGYVe=}tLg$Vf-91ILnPyhc1jT6K5Gev_HfXqL^!N3sp z^#6a*xyuN7NMZxYzW}unpZ@=Eh-`iiSUcE$4o(J!4Nw372RB||aTtq`*WhGe*z^>8 z8UUH^!O6g|_v!!tUdZM%xq=M<$!BmfFuZt*JFkG+rdOW+|8I`0zX+@yoW?+H);CZ8 z|1U(A2cv#vdyd=u9+3V^xYNKIE(Qj_=l}nQq1X>e zL?HXma4|5nJjdN$e8a`S@aiJ&IN;%CUH9pdA)|%sJo!4`jX$Hv_|xm$=&# zKHLlp-8keyPX7O&QD`Y6>w7MSw=5f27jiSq;WlGn`2U{~ggLBOBN_5oA$-sYwvaP( zK~+2GTzimy&^SH_gV>{ZGz3ONU^E0qLtr!nMnhmU1V%$(Gz3O;2tW^Ihuy0HyGH@k zeg=gL=#B{x4dVxb+W63WpFrZEJ1RglOdfPc1c(nh_zXmYJ7EkA44{M0K>RJ>jxGZO z=;#L!A9n8pc&rk1-xx@cfq?;Zuo#F5yXOJaj{xz(dsP@17{31dpAX_+UB9kgVJG8It@yfLFqOqJq=1PgVNie^f4%X4N5By` zfX0Kd_!l&Xh0R}})sl>$(gk|&3M@Z70+(4*3=5#{gyp-h;Jfj78EV0kK@1GwIRgd; z25v^s?KHd$CD8Z)kFPN>FjzswVdW*P9EoBCjXhw-OC}@ejy!&b55W*0f#L(iYR92| zAr5iSUO;UAJpwjgkii0aKO)F%7{;a^bWa{=gamf4Axtgk-aAGHF$VPf`wncr1iT!C zsb>MrQ8Dl`B!DNe7#LvTV-FQi0uRVBfag6ydUB!SI!+)S0|R)Tfq{Wx252q|B7{P& z0*mu9)Pe_(85qFxFboU~TcPHtfG*4f#S>K5WvKWfJBR=*UOs}&frufKOrSY_2402@ zQ1^hxhZz_cG?_v7{P8lZhSpQCa@-lJejn6d;Q1H^28KeYxRnLObnqMt0|UbpsCXMF z<1jEVfagM?_m3(u@G`st-y6li0G^{{U|={8Rj&uVDG@xL3`+Yf3=AOk3{Z2xbF!d& zd00U1gxHBp8nb|6SCT;hn*L#ORyfrALd{1HhZ3mxIS+^lp!^JC^+LrrK_eXI{&i4s zbpM`%iW@-V3mSY3Osv@5Z;nIUixm!9HPo;zk>U{DnRU9l?3@Bw`KC&+~m3>pyx zg_{Y~{owIyP zFBvw7``e%a0Bi4n=7K=xtb>{ZnhOJ2k_}bQVF}_P@=FI;93qBHE(Y7n%diA3-R=Xc z=Vjosfv5)0OEEAoe1VFi$BPC##GMzQ_JZf@7#J8Fq2foN=@ym_qu4?I9;=$q&JCVr(usAP+DYRS#&wDa3Ff>BV{|8O~P;W4-fr@VcPw+4>z{2e)*c@b& zA?){1^>3iz44M}O$?$W4+zDc#VkHiUf6?=G091S$w0r~4pD-{moz-s~+7#NN~#ha`lCV=N785kJ8L&eLW!44{J2K6Xt{uiVr9x6TsdOEJjcYqz;GEVZUYS;@H{Q(;A9-;3-LhwwE-G0u=)|SW(rjPpr=Dk0f>KL_j7~S zGJ)3h@qolp-45a{$05E8hxi$&d&HpO3~hcfJjJ2@7g(H^!58Wf@Y)a19oD=c_wX_p zLmL#}IVc7O1~FcUf6?=iC-}ZqUWRCBc?GNA!@%Z%e1eJ#LE=o@ph^Zsq8ucSA_C!b z@j}W6^!j%i)II3s(l)4gm^(y2c%GSof#DSn^Z!8Aqn9VTd=USlw<|oL;^_G+lMhti zNio3gA%xxYS`Sr^-VWKq2MOm_(1-%BodK0&Q1v~o5ck8n4Nsxsr=k7=&wVm5Fa+~M z+-U{PFR*q|F;qMpTED>Z`&6j-I;i=ua%MGD96ddsgo-Ob>k(LZ{(*{nLdzfUd^!UI zgR%gmy@cL=FaV27G3rD9U~yiC2xvnX)E)xqnhrI8E418$wVmce#S5VE0$vZnz`$?@Yz{~#Dt-f1uLG@L zU>#mjA&9?Hpy>)$9@+|l+|SF9gqE)Z!Q!Z9gLvssa~7e+O9NCq3Yy+v?Z#C?7)1cc z7=~jw%)g66{0r3ln^5yX{b!ItO2Ux*h<+clxiG}P=v$3^7pg zOlUZO*WfWQFr-7xL2s{iL&f=^;Q-AZ49lS6XQ1^m%>4br*u&u*R6TmWeg_ptuLrqA zK<)?m92F~wK>T|gntovUOH%~9z0OeeH=*eqG`;{bF$cC zWY8Vh#SHQBDf#i~Ir&M6Iq@ka`9;O?iKP_`@XZ%`2G~`^gEqy)CxS1UPR%PRs$@tj zO3Y1-PbtmKtz?LgN8*9+u0~OsoS#=xl%JE6S`-hurdcn!pa8TsA10QWTaW`5i;wa# zG=l8Mfo!>90ErkwHjY31S(NflA4zSilq2>w;)Gf z*LYVym-u*w_;`1}(0Er5sGx^S5JS96q@Sa&r!(Bm{%&r;t|9Ruj!r(VpgX+%yg`?B z$2&o`LV&Lk_Y6+-OmcNKG=__U!r0I--ZwrqFC{)PC8fv}bV;~haEYN&d>D9V3`ApQ zaH40Xt1Chjq|wEQfNGGz;LR6_C8@5tkej_7!1rOt8|i_rrv}~N9Rl8q1imc2#LzSz zd|x?KQ+|A}&Y> z17~;`pjl^GP+FW3pOl!K?Hc3_y89gzs21@~nI-X=C6Jv%;GlIjbPdSOf_mQ=MIK!W z;vcXs>@n$V=o;i5oamYB3NjBA|0ZAq;k%5I%aUBnpx39P23TrJVo4(OesxrtD8FC= zL5V8}pkWO1xFy&f$@zK32=6-^A^99z$RNujl5A39aVpIF7AOfh9^?mTganrmyyqU` zP1w!s2GIN9A)?Uq2~J0bSlnhF?;D?)mt5qU0}AP2tO?i9AO*7Z2$cE*a&y5~)uS5k z=^tNQl9HJhUs{}+!Vn)7Tw-V*A77GDlwTg7ln70#We`0OheEBx8XS=5HvknEnZ^0Z zM#k~Q#mR|zX%PMKMta5;Q0-VVw=*P5fT9bl@o2>cB447Vd{b~u1Ik|U$rXv98UnPh zG8OI$P{xmsGBQLe1wn-@xMWU+W<4|`4ME|Jx{=G%E66+3)z!7krNBP~T;AaRZ|S0LJP$+<%yXkZuv!_#i>P}c?G2<2>n=cELxcePGK-N zBtgn&XvhYa7#YA4Lr!X5dPzonYEe;sQGjY%~q6H;gW7UFM z#KY2rH=+!HI?^~bF9jUODaHBm8Hsr*IjK;8qm?;{!LCVA862fR5U8X=4Q;3f>@GBi zRZXBu2szUeDycvPYG!T_mfFoA9$aJwXPRZE=j9iry5?j;0t_0)F5!AbscCxg@#z&6 z@dc?x#rb)OIhiGu@nr^hoeK8=$f@Ai0Obd0bpVTYXaxli(VWbr#y9nYC zQ1&$e7ZJv}nR%YMu0h@*@xewQgx3Ye@xJlM*LZZ+nmmd8U}%#Fo=c@=9}4i!zhrb24)?ONu>nT#a)RD`3Het<_@z zZ33mb<_3796^bwyT7oMf=)U#T__D;D(p1+ZhT_zm)MRX~z-T^YLYhzDG6<3H!4)hl zxUu9lNYNQwl2hy%Tw-XLoRL@*Us99^@>jB}D=2pvCnXkVCdcO&mlUNY=6WWF#JdKY z#-o>8X~{YH#pu;SUU@Dg2B8@b;x^R6p}+@R?WW`el;t`&M?^R}dBz*+8S7axIu5>N2}%96?1@yQw4@t~dl5NF04>6tKq+xHAP;GO<( zVQ@7UAL8o_Yr}(b2KG>fW)M)e!mhv&6xHdeCGjBz@z8{k4{EBXfwupn+6cA-14lfv>JG|@R5p_Bx{B__%FrO?EN9%Wc+KSaXa~mql)3MLfKq2`(`-iHDa3@rm(IjQlPX`T_q zMZqOTCh_r^#qmj*CFPmLsW2TNC7vPiWv-@>j1v%S6z^&R+Qsi0Y!VMaLBWZhA+Wju z9CVm|f|Tk}!6mtw;F1PQYcbdm)IbR?0cCn@B{#N=3CU%k#u&T~LCKgnQUpA|U^>(y zJ~%$FJijOfRF;9OO;Cz1Dq#S1{=hYhMZ62BKb&9b8Ib8(77`C}H^>bbE=+QT`Wv&( zg|@&!+7QtVb~~hb18Pr0T?a}8V3$~cH|iIsmcYwItiFaA9iLQO40Ad-xuX^j5Jg}K z3}@#eIol9NE`fB%Kplw8{9Kz`FW`!psdLdADUO5nU@kDlv-Sxo9f{1>>O{bXTVULT2h>!oSk0)Ep*^5T5zil zmbp{o6FmbmgAL*#;}#(X@gecS#-PShPzWqDCTHeBVhy?6LM!gTi5y%{1%nTPVF0xV zpiHp;vBx8%U<0?6f=dj|;z5H#Iq}frDdO`2a&w_gVl?X@y^r|x-26O90tb!#fKxKo zoCK>kz$GDQWC7mM2eou_!7aUDP{gI6c@$K22FEkRgMuG>;DaoMISyL-f}39O5*XGN z1l0hkMMY>9fT}Vy?IKpFIk@<&>_!^lp+L= zK`%WyIX<-_Ikli9z66}QAr5tP^3(+l&+37a6=ZA_Vj_&4Qkj>So0-g@m!4k&CiK7# zfLNAPT+E=CoSzFi^agdrm^>b6{s%PY2wg_X5CC1!38SIYxp2c_^Z76V&^iHR{S%=2 zVKj7l8m1h!{1wi31g%j4uX%$AZGq~C(Xe&45LwXLH5dy*gQmT{{r{g2b3bhTEsTbZ zn}bfc1L;6_KWG^oXf6t-AMQZtf}0yq_kh;#g4R01^uyNcMKdrkfRw=W!`A!4XxRE* zkli2*G6O`za589)9LfE#^|LS^n0}c5LF*(z`k~7i7+~v-VKi)gGDts254!%z(D(4&W^fYFPf^UDz3 zpt)HX3qsFF(+^w60;8eJuOW&->*wGs2xBQ!KP>!V>%Cw!Gn$8C?uO`QU|5T$AGVGX zMxTKj4AFsR90TaOG*Fs^DZ2n&SO=qXK$!!g6->h756od;U;y30imcxTy6zK3!`7L= z)PvN**dY2k0|Ntc`H=zD52K;c2~rBi=gNDSLbQTOm_8US zf@VK#-N6S?!2(*>3D$)mVESMpQj81?ATgNzu=M}}&;)6s1=9vl`xk(2sAphc0A2JBs!Kp_1t|cPMIaiqP94pOAYq0JO<;=Q fENo*1gbQC!4&^d{jK`*b?sSL>4VXhATnzmHuZ%Ve literal 25624 zcmb<-^>JfjWMqH=W(GS35buK^M8p9?F+{{b84L^z4h$9yd<+f@@(gkeYzzzxEMPH+ zJWM@|7GQ>m!)Oi&mjNUU(zgY~U|?WCr)8k(U^K{0AR!P9(g$K=!w&oqQ5el20O5o5 zv4WUTK1>`&uY_6zqhaD8ePH_xK$;mC7|`eq+z@dXjjRt8HZyD?@-uu<=?tj94}cxT zzyQ<7#J~U!(+yC48=(4Nv;ar}NI8^-g(t|3ARGWS7oCR12aHD7Hvy^-omPQ3n*m0H z>;MUYXb_%Y4dO8{pwkKvVFnltQVS9aep-?OVxzkc#)r8NM#J100M&QL2xJNa13JA0 z6j7jb1f@ZCfK&!PElB~z1Bguwh9!?6sC_u&4;ua%P#S72I4^*b2q;?J{X!X-8W;|M z+yawlWMBmIIiZeWg7RVVAhjY44B-3$(%;K>@kXuC$Aj0Jj!S(N5frbL_L`%glbK{@ zqMwtZo0FMWTA^EEVWw+lqF0=+XT$(<4k)fb=0W@|z@W$=0FFOZ1_lPv89dIaxjfEL z>oKT>j0_Ay7@`aeb8v{S#UcKa0lPWBaHv0kL%acpIjz8Hu3 z={VFMz@fejhd3zT!pa)tWKn>_d{F+tralLU`8o^?@Ujfqd?p;>9LvbSAjlxazz_&g zEda8Rfq`Kr14LXQ2vwXRK0ZA+KQF$xB(bO@KAs`oGsHJOCABCuJ+rtZwJ5~bIVV3a zH6$@9Clw-xT_!5H#K<5%z9ge4zdSxCH7~s+BR;jLD8DGcJDDLqDkUeNESDiZ$}c!S zr`R7R-N=%7Q%*n}5E(1%LB&QY?fmud|nfb->Nr}a&@j01EMTtd~ z@tJvAc@oB}$iFs)ZNtt;m3>hW)dBqHA`30$Y3?QAU zB@D$SML8f|YF-LMQ7R~`;*%>9;}c6#D>6aGf_wxryQm~SH!(AhAqNzdc?|JjRcV=^ zaH>jWNK8&nEhu3~OU}tJPG!hV%`HwX0a3{Xl^`4Pl5-0fax#;OOHz`xwLyiv8Hk$; zXQd}6#}|Wmh9L1|5X+3gIU>T*$ur(i&zQm8$J5Cc&qUAI2uViI5=G7kg^MC( zYyjcL8|j(AS%wIvF%r`PuE0po6qLr97(kF2OtOGUCI(gpCI%3jje(tk1IlJ$;AG$e z%X2d@G4L?3FfcMOF)%`D7I4kVz`)4B#vsQ8X)YBoFfd$RpUuj^z{B7V6?6JAor!^g zn;{Cyue`XPn}LCoArr<2%d<0-LHQ+`lQ=*v*j6at0W8nVFcDOdGJx!3WpIVcfqWy9 znaRt*z{RitDmG=}G&TkX4u(}w{uVHwjbSU4zXQx?WjFxkmr7-FGB7YPoQ9eYYZt=W zIS-(f*8*s}4<^1t5o9|9!wNL<2~hPf0wD4*^)UVce~37!EdY}PiJU+Zhc=l(!VDLX z#O1&u5aI@sIC8o907)FVKK=o952)CKHmxBBLCZr>`v}xdfe11%z{*3AIH;Wh6K8;$ z!w!xMxF`b$k~lOd!IA<<;?O27SX=^09Mr}F3xdjYByo`4FmVkeaZuR7#0`+dL2iPH zTOf(U+IAp02PAPWkN^~WAc;e}5MY@AByk?F2!x1066b{ofyo3UaXzpJgvdY=2i1)b zQ3i$rByngL4=h!IBo0kdVDSbdabd6sgy=vL2enrrq6`cZki--!Gcb6x9w=e@f5D^q2*+Wt2mhO%vS(oUuX@Iwfq`G%f#JU@h@SzH zd->r1|NsA057{#?WPq~y%L`!sE)XA-C0`x@^EZL`pse_E1DL-G#0O=;mkYrBMIb&X z>%E)+=FbB0L0Rr)1DHPv#0O=ymjz&c7l;ok%3dab`Ar}`D9gPJ0Q0Lrd{9<<=>X;z zf%u>-_R;{%&jRs5S?i?&n4bjVgR<010Wd!b#0O=imkeNj5Qq;dB42*^2lB5Mh!4tA zFCT#UP9Q!gE4{n`=39aIpe*$A0GMwC;)Alz%MD<@7KjhZGA|c^`AQ%@D671j0Om`9 z_@FHEvH{E&0`Wmv<7EMu&jsRxvc$^-FrNv;2W5qq0bu?wI|hafDFz0Hmkwb5ClDW$ zC0-i*`~M#l--d5JJ1@q0bUyNEe)1t8#MSVC;Yp8P+gl6_4F5$p+cGeGNz>z(Zvn+I zgGaCJO*;k#k6zYGAjoi+PQsMmnfL|WeMPoP&r9FCWSArDwnl1uSn*TgH zKX@E`XYX;^1 z9?fqAj=TN=B~FiCQC3;7zkY$7(s|9J^Y{z-fB*k?yZ-4s{sL6RdGw0@lL0Haj!^RF z@BjZhKp}JTFxbS#Hy0Qf7&`ab{Qv(y#iN@=RfBt;S;}LD=2S4M4Jz=AWLNY|NkE*!GSCR%AF92-T+3A zUa$!soyR(_zxd70z|ah~ff1qr>V(eYtszKm2C*S-_T>a=0Pz{S!AhVSZXa)rK+*tW zLo~E;M0QFXUSelt_E@ie^LHAcAm%kW{5yIFlf z=JbZ%@aXk@0CD~9pa1`Ny58w_z0-Q2(!rxw^p+$j6j*~1j)?%-*mW zZU%;K29NI27dt^o$fNUC=kXW!KstIwUrK=d$jX55oGQrOpdh;g@yApM-=mjxDo8Cv zkm2Y5|F0P*cy!19fMkYXkH$9^ETH1=%x~1^%O0 zAG=pexWImX2`W<|+F(AL0@euj88gIZy;~pr{{J8Bxn2L685lZ`zgYkM|NqX`JHP+` zZ{7WqCdsK;qb#89?7zx0O~vhu^Zy+E{KpvFY6PK za){v9@1USR`<=mud156eURQxCKpDBaP`ddmzPmWRLp z|Nlbp_y7MGg`wqlq|6)O(OG-KqqFotXXqXb_ls^7V_@*;c0J(%DJA4VUIy9PT>F8c z)XSq+bQ(xOFY7c!49xxp30bf%aC~<{MS58|L8>8PUGfcD55H^&Rer8lK#BaN3L^tU zx9b&9UJyA767lGEJ>bE3;S;}r>y1zRf(+fESGryIFkZl_;sIEN>z-Z_kRdn}y#Oil z=oS4X3QjwFOhGXMF1sIo1-rc4^$Mtt0Xy9f?(`KPdZ*oB&t&0IpC5uCN24FdeS&1zh20s8}!SWf71Q!S1p9`u~5k>lMas*DKbpduok6 zdTU>Ri&k(EYibTMvfK58XXg)(&g0;$_+s{#|NonRFqdK!N>l!Ui@Fb>bouQIsNrryk2n~%SE&c?uC z*$NV`!*Bo(BPemTf=u#gJ|OUkUx34-8^-1M#4o@AW>JbGChghA@S22X^j zgF6K*SOXJe{UZcdnf@7CFuQ_E7gpe)(dv}7iL@mz2F0B0bK|Rk6zZ72B2U82Q=SDjDW6&2zm6fUKIeT1H0kb z2e3L1NYFt8`XWq_H4ve44@j_=R~>8>B%nb}OlUyMeIOjryFS1I8l^cU;?c{R4^AS- zUuV{APJ;~ln=dzvu_I^+bI@M)vZ~ z1&Q{u)`9DJaECAZ{r~^a-p7lB|NsAo^hZ28YkzojmV)bZR1v6JkLEWD9-Xz&wxjD0 zP>UQ?azA2XV3;rg(wqZzkHH3YyMoGpa3^@e$-@)C9o^OxQWo|dIH=|=?>lD(HRPADc$hsb_J<>;L#m>!K3rueo!Cl1!#!IqdWA5M`!2@ zP=l^ps8ayk*=xB}!tK#{4-}+z|Nj36dmZEk4v*dd7O=ZI12|p_dvu3FTnT9qf%kC!5SD(fpcYd z=mwa9oxPy(B#+Li6(C=9x;A)pyEb@qx^{SUgD_M$bb?2x>kNh86X2Qk zF{p>$9Xi9KGjs(g=VQyP7k@!AD?IQXGJ!J0L72+&-{8#PiKyip2vu zXxE8|1yJFm4T(H(iRO9((g8dELJ*`M7NHQ|cM2c^9UQ2gpdt*CCVE9$Apr#mb;d(5 zrJVr+;M|5hOf~-{C=5?9q7(T!>m8;-7S&We2;g^-d&VV6+Qtn=PXFErR#$t zA((4GOG!MeT^|(l!9+qC85qFcd;lt|kvrd|U{`=b3RW&bYOv1I2OiC}4;V_cA%iTS z-Z7}_4XQAGL6znQaA^-M1jIqr8g!fnR=dFh9aKSd`huMM!vj>Hf+87YO6Us@aP)%N z4?IB8h*p+^O0;h9OpFq^0tBU{?$93|-9a24-5^JBLajFf_0Hi%G{^&xHUQjekoBN? z3fwLLR}K>x7#NUU+U*KW0nqjV!T``16Fl*`et^b>?sG)x3#)rT?(IDO;t#k4JpQ5_ zYz4T@01FF{3UC7a04mh5B|y;d2TFn5=?kiukxE~1srA958|)WQN&uD8FFd+^4}em` z7lftFZ$L%+3FH(E8jRlon#}qC|G&HkIGchJAZ%O()(~|4-~m@P0aUWw@BsIVK~{qs zcgy~O(k=t2IzUcL8qYw10{1a;n1X7%7apKg)fozEQM>?GCvaOp$(4T+N0N&qjiZ!cjaL z0;3^7tq=gMvGUBzEXhpF$*juEOIHYS4p1mgEhEGK(`(Q}jTi zBFJ_p=jWwmrt6m^7Nw__6zi2#lt5&I^OLhvOB9leQWHxu^Yauy>-kbsxEP!=^HSg( zAMk=WI2URcXh9*^IUx0p;MH+ZB?@WzMGDC|nW=dt3JhEf&QO*@a(-T3YH|rky+>+J zPQF4~QGPDe7YxB5)w!v~#fj;u3MKh){Xwb8shMS|DGH9x-VhBildTjOiWuAyGjmc? zKpH?x{uH1dR7lIrNwrd7@X1e10m*{Bt-zqepqi7W1H}v>jzR9OAy%q-W@$Qb0^$yc zF(6Z6mcg9?b~8-W(b?MyVhlr&tD{RKgPW(HXRwE>3&a>0580sj_&fz0g_4ZSVqLr9 z%&JrkO;Ah}m!w!(m8B+^ z)(UX56l@i8^7GOaO7n^{)ALeOK*q&_oXwzLT3n=`nU|bXnv$xYtgWqYXsTzVXQ-c) zSyHTDT#^$H@snN#1B78^74I8glnS;xKCz$xv?h~Ln%Q~-BLf3y8KTSG|NlX2mPPLW z|9^&&fr0nI|NlHp3=HuP{{PouVqge(^#4DozY_lF|9>8428Ost|NrYSGcYW8{QtiX zGXq2KlmGv7m>C#iLG>;(1H+1E|Nn#bti(S5|Njaz14H}E|NlWFaa^ze|CeB4U?_g` z|37GI?9SW&|07r!7?!>J|Njj;1H+wn|Nr}NFfcp^(M${sKi~cTpToq!!14b7{~jg= z2A%i+|E~eHvfuyze};*HA?5x5|1~TO43pmf|G$TYf#JdX|NkGbFfgq9@c%yxD+9y5 z5C8wGure^XeEk34g_VJ!@8kdfDXa_(cRv3A-@?kk!1d|>|0S#p3}T=D|3AXYz>xCk z|Nkef3=F@&{Qu9v#=xNV_5Xhj5dH1{e;+mmhJtVZ|L3qVFs%Ca|9=k~1HST$3Oo6Kf}(z;PdnUe;y77hDAUB|JUJQVA%Nc|Nk5g28JC! z|NrmdU|;|(i~%J8#;PC&#tH#OX&!ct35*bN&^khgyZ`^IffTr~gBldzeGMiI3=9#s z|Np-Lnl|MVaO0Ek;^!{sXkf6Hveq(I0WV7g=>=_J`g!;NfAG?4M?QfzCTCtYCSM+Q z4h9C0d<_EwgZ91u|38Avfypz!VFW1v$o|9{Zp9tM~^L_1jj9tH-6);ItE zgBADoWL5ro}`kfH^V;C72obkxlFfuS`-T(jJ3B~`m2>&xOF!((9|Nkv0{9*2AdI7co zv|wfrBLjov!~g$5v$t^jU7MMij)9ec*Uvv-WMDY@@c(~rWIH^XnVIdt)_}ti)XRMH z=>Pw%DE4ds8v=?46($CT#>fBvYk`(Kz}&&C$^=pXa)%3O#P;d`|IO%jlp*ZMU}9i+ z_3Z!uOk_38DPRR4J35#c7>r+Fmj~Igf{B5l=H>tYr;yzU^6LSRDGZWmp*)+&=vO4_cfE*AEE>upez$85m4H{Qqx= zY<~_ye+*PVxIl!NAB&K$VP#b7SQ!{DmAP zX2|k|U}pUg1Yr9>VEaB``#eDD0u%zE zJun~|#@7Yag-ClO%+dIuJuD#gpoPyM8n)jUwD1|kp99+9#=yV;T1X7y!}g^}gE*l5 z#!woxuo%RI?Lz_O8xS8n>%zdm@b%ySd=UQvGl*nh08Q?KSRLR-31n|Gi2nmB59?1} zhq@PB2QV-&fEGA|)QN%!Bn;KY@B+H;1gZ_Z9|xoi)_#Oq#{gP54H91f^*Btt0csJ< z-3riv1T7>7sfXGB`#;1#44_Ru3=9nap!^L`hyRE2Vea?<rVk3s2cQ2H5^{syJlKr7@K7#PH$v>KE) zgVJtLIt)suLFqCm-3FznLFr{sdK;8J2Boh->1R+H=2zH$D9|V{ObkT3J3Ct`Xt?`@ zYAQf_mZ zkQfMq>S&NTuyO_@2Ew4(az+Lb23YLnRIK=;|e@(VvBXb+wgg9FrGuzab+2--!*%g_W0CFg$^Z?}3Vg`zs6#4D8IH(2<1ilZM&O$Bccyz9KWkzv%AqfQoBC-2+PZAhQy2 zm@^HB_-roFK5t2e2hel~G6RHH;!wYf8ML39AAT+fEZv^Oq5d7zogC0`fYq;jED(Pw zKqD3wkJ?~ykPA?;8w>XEi3E%DGVFjBJmCH*Xg>i|ycZfiu=r{Oo6pPOpb9Y`+=pUd zVAu{7-v!<52JRa&Ffd#In*&jcOg_OO{vRyP%kUCfj)BKsKxF_ccK3_o5VvQAgijtc z+(3O|kp1yE)E7b3qo>0KtPBi-Op*++`T%4G2(N{z-vJGOa9@>yf#CsE+y|Qe!F@Z> zzEL&?21O=G2G~AVSUUpL2N7c6Wq1bNYzQ7-U|?YIWrMgMJ^xm-F)#=*@iD;KUm&-D za3fTG7Sug3|AP7;(4YhV&NE}53!ja~Lq<`4C6cAAc26aw|d(iWjJ5<~YnxMdA4-56CfbIi0LHbS#8!x5mfM46XNJdEWzv`dA#|wNP`IK>HIx z8$F>`EQ5+iKm!;&_QJrxa1Lw^FM}~!I(!5chiXQmen8DZ4GE3=9WwxaT}rJ;)|h{1ob*tI&9XrIU9!%;DgLgu_{=`$6q*kckGo zp!S0l{QLvhIWa*v^1B<*X^bPLhc)$&F72oh4Ju5FekCAD^C+pOlyrpHh-vR2-jJTEUQV`v1Kw1&((GsH*v7#c$+ zQz0V74DnGWhTx-KN-~qd?gGy-gJ-AX6Z2ByQ&Njdib|6~v(@niWeoA|KK@RQKJoFz zrAhIg@$pfHhM1t{zZ950@Z@c$Y{&M_*58xHJ6S+=5+0;zJyrd|W}t;rIoY7#hWg zfhTss=ka)E1}A!Ey1F7nLBZo<5-xflhEK zPR&aR0Z(&-&%r^F105t(Vg{LjMY7+$G_eSJevpeH*nT6V_(TK>ewV?02l9tA+#hJx zSr(KQXT&EZCTF__d1o@jr=}#9Btj49A=Rz8?0|YAxP;(2MGzZdhwT_Z&-Q_cLc<2MH^gxUTpWB2Z{+%Hq?iY+6F{a?c02Cc4=MO>5 zg*py$q7kZ4YF>&PboB>Ae3X$PQt}362yiY+g+?V>y1JIR6!?cg z6Ed2v%nH|B*vUsIPC+}D2`*=B!2l{t7(huCE(l7y@%WCzf~X`Mpy1et9LogJ0u3v0 z;z#5*(3x0Bc>tO-!D$y9Bk?K4`SBTvc`2X+yrOVtNOM@G25F54rDmLI-_R%?DXTdf zx&~zCf{w$2=Nzb;4B}lGT*CE=Qq%O}Jdl*XB-bY84bMB z15{`QWP%R(!c6bbqA$3_2>%IW5XV5$fu4~i#A-B;7#hY0#}}pMd}6$zXJ)3W zUvP^&w>uH42GVLiQ@!J%$fjPdc*TskToh z(6N?i7WiRCqzN>8GUydo=9VNTG3XVS6hY_=7%MNoBvmgxuT-y~2()Ihq>@1oE|gT7 znUkWMnZlr#nOBlpRKNgMn2}hV!JwB?nO9ty3!zJjATnjCMa7xrJ7O6rMF<{)9_YBp)C$m%lkp{> z@&f8mM<-8RP^VE36eo}#8^lByJEbx&F*h@rK`%YO1Wf3G9RRT`skoRyFF8LqH#HBs z{uk7@r38b6jM7Fj>8bFE?OVdM5Nz9V=(3N*(A z5dp1n0VA+Obp4Z|@egtz*a+x6Hf-GlD1Cy(p#;b-C=*Qeq3KtE z>W9(bwW?s15CSR4(Z#SC$380B@ z1_lNg{Rb8>AkDDF8BjKe0-gQ>3O|@Ld!X~RFdE%HkXn#F7`_ggn?y=~xX!nSsYf^O zDFXuoXbc**UIjLv4m(dCJ-lG)4@Q4vU|;}^DZ=!_=I>WPCtzXfVD`iKFd8(U4hkZe ze%L%S>|O!bJpv##ATwb4VD4fA&qG7bp$Eys&XGM}58^Q}z{)QO54t!8DIU1d^uyNc zynyQe0n-SjVfMpl5j6d<`Pc{0f*(4)3Du3TA0{Bh2oZz%A3hEbD%e5m#bF5pssU;& zl*hmTI#(T(Cg9_%Fd3+R^!yJ}3sQniqgerxW>A3YUjbc@1ezuUnFaGVs4N001g#^1 ryB#C|#V4Q>fyrnY7-lb283Snf3pV}Qpo)QkfdO Tuple[float, float, float]: - """ - Load ENU origin from config file. - Returns (lat, lon, alt) tuple. - """ - with open(path, 'r') as f: - for line in f: - line = line.strip() - if not line or line.startswith('#'): - continue - parts = line.split(',') - if len(parts) == 3: - return float(parts[0]), float(parts[1]), float(parts[2]) - raise ValueError(f"No valid origin found in {path}") - - -def load_connection(path: Path) -> str: - """ - Load drone connection string from config file. - Returns connection string (e.g., 'udp:127.0.0.1:14550'). - """ - with open(path, 'r') as f: - for line in f: - line = line.strip() - if not line or line.startswith('#'): - continue - return line - raise ValueError(f"No valid connection string found in {path}") - - -def load_server(path: Path) -> Tuple[str, int]: - """ - Load controller server address from config file. - Returns (ip, port) tuple. - """ - with open(path, 'r') as f: - for line in f: - line = line.strip() - if not line or line.startswith('#'): - continue - parts = line.split(',') - if len(parts) == 2: - return parts[0].strip(), int(parts[1].strip()) - raise ValueError(f"No valid server address found in {path}") - - -def parse_target(message: str) -> Tuple[float, float, float]: - """Parse TARGET:x,y,z message and return (x, y, z) ENU coordinates.""" - if not message.startswith("TARGET:"): - raise ValueError(f"Invalid message format: {message}") - - coords_str = message[7:] # Remove "TARGET:" prefix - parts = coords_str.split(",") - if len(parts) != 3: - raise ValueError(f"Expected 3 coordinates, got {len(parts)}") - - x, y, z = float(parts[0]), float(parts[1]), float(parts[2]) - return (x, y, z) - - -def enu_to_coordinate(origin_lat: float, origin_lon: float, origin_alt: float, - enu_x: float, enu_y: float, enu_z: float): - """ - Convert ENU (East, North, Up) coordinates to lat/lon/alt. - - Args: - origin_lat, origin_lon, origin_alt: Origin in degrees/meters - enu_x: East offset in meters - enu_y: North offset in meters - enu_z: Up offset in meters (altitude above origin) - - Returns: - aerpawlib Coordinate object - """ - from aerpawlib.util import Coordinate, VectorNED - - origin = Coordinate(origin_lat, origin_lon, origin_alt) - # ENU to NED: north=enu_y, east=enu_x, down=-enu_z - offset = VectorNED(north=enu_y, east=enu_x, down=-enu_z) - return origin + offset - - -def try_connect_drone(conn_str: str, timeout: float = 10.0): - """ - Try to connect to drone flight controller. - Returns Drone object if successful, None if not available. - """ - try: - from aerpawlib.vehicle import Drone - print(f" Attempting to connect to flight controller: {conn_str}") - drone = Drone(conn_str) - print(f" Connected to flight controller") - return drone - except Exception as e: - print(f" Could not connect to flight controller: {e}") - return None - - -def try_connect_server(ip: str, port: int, timeout: float = 2.0) -> Optional[socket.socket]: - """ - Try to connect to controller server with timeout. - Returns socket if successful, None if connection fails. - """ - try: - s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - s.settimeout(timeout) - s.connect((ip, port)) - s.settimeout(None) # Reset to blocking mode after connect - return s - except (socket.timeout, ConnectionRefusedError, OSError): - return None - - -def move_to_target_test(enu_x: float, enu_y: float, enu_z: float, - lat: float, lon: float, alt: float): - """ - Placeholder movement for test mode. - """ - print(f" [TEST MODE] Target ENU: ({enu_x}, {enu_y}, {enu_z}) meters") - print(f" [TEST MODE] Target lat/lon: ({lat:.6f}, {lon:.6f}, {alt:.1f})") - print(f" [TEST MODE] Simulating movement...") - time.sleep(1.0) - print(f" [TEST MODE] Arrived at target") - - -async def move_to_target_real(drone, target_coord, tolerance: float = 2.0): - """ - Move drone to target using aerpawlib. - """ - print(f" [REAL MODE] Moving to: ({target_coord.lat:.6f}, {target_coord.lon:.6f}, {target_coord.alt:.1f})") - await drone.goto_coordinates(target_coord, tolerance=tolerance) - print(f" [REAL MODE] Arrived at target") - - -async def run_uav_client(client_id: int): - """Run a single UAV client.""" - print(f"UAV {client_id}: Starting...") - - # Load configuration - origin_path = CONFIG_DIR / "origin.txt" - connection_testbed_path = CONFIG_DIR / "connection_testbed.txt" - connection_local_path = CONFIG_DIR / "connection.txt" - server_testbed_path = CONFIG_DIR / "server_testbed.txt" - server_local_path = CONFIG_DIR / "server.txt" - - print(f"UAV {client_id}: Loading origin from {origin_path}") - origin_lat, origin_lon, origin_alt = load_origin(origin_path) - print(f"UAV {client_id}: Origin: lat={origin_lat}, lon={origin_lon}, alt={origin_alt}") - - # Load both connection configs - testbed_conn = load_connection(connection_testbed_path) - local_conn = load_connection(connection_local_path) - - # Try testbed flight controller first (AERPAW MAVLink filter) - print(f"UAV {client_id}: Trying testbed flight controller: {testbed_conn}...") - drone = try_connect_drone(testbed_conn) - - if not drone: - print(f"UAV {client_id}: Testbed not available, trying local: {local_conn}...") - drone = try_connect_drone(local_conn) - - real_mode = drone is not None - - if real_mode: - print(f"UAV {client_id}: Running in REAL mode") - else: - print(f"UAV {client_id}: Running in TEST mode (no flight controller)") - - # Try testbed server first, fall back to local - testbed_ip, testbed_port = load_server(server_testbed_path) - local_ip, local_port = load_server(server_local_path) - - print(f"UAV {client_id}: Trying testbed server at {testbed_ip}:{testbed_port}...") - s = try_connect_server(testbed_ip, testbed_port) - - if s: - print(f"UAV {client_id}: Connected to testbed server") - else: - print(f"UAV {client_id}: Testbed server not available, trying local server at {local_ip}:{local_port}...") - s = try_connect_server(local_ip, local_port) - if s: - print(f"UAV {client_id}: Connected to local server") - else: - print(f"UAV {client_id}: ERROR - Could not connect to any server") - if drone: - drone.close() - return - - try: - # Receive TARGET command - data = s.recv(1024).decode().strip() - print(f"UAV {client_id}: Received: {data}") - - # Parse target coordinates (ENU) - enu_x, enu_y, enu_z = parse_target(data) - print(f"UAV {client_id}: Parsed ENU target: x={enu_x}, y={enu_y}, z={enu_z}") - - # Convert ENU to lat/lon - target_coord = enu_to_coordinate(origin_lat, origin_lon, origin_alt, - enu_x, enu_y, enu_z) - print(f"UAV {client_id}: Target coordinate: lat={target_coord.lat:.6f}, lon={target_coord.lon:.6f}, alt={target_coord.alt:.1f}") - - # Send acknowledgment - s.sendall(b"ACK:TARGET") - print(f"UAV {client_id}: Sent ACK:TARGET") - - # Move to target - if real_mode: - await move_to_target_real(drone, target_coord) - else: - move_to_target_test(enu_x, enu_y, enu_z, - target_coord.lat, target_coord.lon, target_coord.alt) - - # Signal ready - s.sendall(b"READY") - print(f"UAV {client_id}: Sent READY") - - # Wait for FINISHED signal from server - data = s.recv(1024).decode().strip() - if data == "FINISHED": - print(f"UAV {client_id}: Received FINISHED - session ended normally") - else: - print(f"UAV {client_id}: Unexpected message: {data}") - - except ValueError as e: - error_msg = f"ERROR:{str(e)}" - s.sendall(error_msg.encode()) - print(f"UAV {client_id}: Error - {e}") - - finally: - s.close() - if drone: - drone.close() - print(f"UAV {client_id}: Connection closed") - - -def main(): - if len(sys.argv) != 2: - print(f"Usage: {sys.argv[0]} ") - print(" Run one instance per UAV, e.g.:") - print(" Terminal 1: ./uav.py 1") - print(" Terminal 2: ./uav.py 2") - sys.exit(1) - - client_id = int(sys.argv[1]) - asyncio.run(run_uav_client(client_id)) - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/aerpaw/client/uav_runner.py b/aerpaw/client/uav_runner.py index d29ea05..10ed12b 100644 --- a/aerpaw/client/uav_runner.py +++ b/aerpaw/client/uav_runner.py @@ -8,29 +8,40 @@ Run via: Or use the auto-detecting wrapper: ./run_uav.sh -Protocol: - Server sends: TARGET:x,y,z (ENU coordinates in meters) - Client sends: ACK:TARGET - Client (after moving): READY - Server sends: RTL - Client sends: ACK:RTL - Client (after returning home): RTL_COMPLETE - Server sends: LAND - Client sends: ACK:LAND - Client (after landing): LAND_COMPLETE - Server sends: FINISHED +Binary Protocol: + Server sends: TARGET (1 byte) + x,y,z (24 bytes as 3 doubles) + Client sends: ACK (1 byte) + Client (after moving): READY (1 byte) + Server sends: RTL (1 byte) + Client sends: ACK (1 byte) + Client (after returning home): READY (1 byte) + Server sends: LAND (1 byte) + Client sends: ACK (1 byte) + Client (after landing): READY (1 byte) + Server sends: READY (1 byte) - mission complete, disconnect """ +from enum import IntEnum from pathlib import Path import asyncio import os import socket - +import struct import yaml from aerpawlib.runner import BasicRunner, entrypoint from aerpawlib.util import Coordinate, VectorNED from aerpawlib.vehicle import Drone + +# Message types - must match MESSAGE_TYPE.m enum +class MessageType(IntEnum): + TARGET = 1 + ACK = 2 + READY = 3 + RTL = 4 + LAND = 5 + + AERPAW_DIR = Path(__file__).parent.parent CONFIG_FILE = AERPAW_DIR / "config" / "config.yaml" @@ -56,15 +67,42 @@ def get_environment(): return env -def parse_target(message: str): - """Parse TARGET:x,y,z message. Returns (x, y, z) ENU coordinates.""" - if not message.startswith("TARGET:"): - raise ValueError(f"Invalid message format: {message}") - coords_str = message[7:] - parts = coords_str.split(",") - if len(parts) != 3: - raise ValueError(f"Expected 3 coordinates, got {len(parts)}") - return float(parts[0]), float(parts[1]), float(parts[2]) +def recv_exactly(sock: socket.socket, n: int) -> bytes: + """Receive exactly n bytes from socket.""" + data = b'' + while len(data) < n: + chunk = sock.recv(n - len(data)) + if not chunk: + raise ConnectionError("Connection closed while receiving data") + data += chunk + return data + + +def recv_message_type(sock: socket.socket) -> MessageType: + """Receive a single-byte message type.""" + data = recv_exactly(sock, 1) + return MessageType(data[0]) + + +def send_message_type(sock: socket.socket, msg_type: MessageType): + """Send a single-byte message type.""" + sock.sendall(bytes([msg_type])) + + +def recv_target(sock: socket.socket) -> tuple[float, float, float]: + """Receive TARGET message (1 byte type + 3 doubles). + + Returns (x, y, z) ENU coordinates. + """ + # First byte is message type (already consumed or check it) + msg_type = recv_message_type(sock) + if msg_type != MessageType.TARGET: + raise ValueError(f"Expected TARGET, got {msg_type.name}") + + # Next 24 bytes are 3 doubles (little-endian) + data = recv_exactly(sock, 24) + x, y, z = struct.unpack(' + + int32 + + + @@ -145,7 +150,7 @@ true - 2026-01-30T18:21:40 + 2026-02-01T15:39:22 diff --git a/aerpaw/controller.m b/aerpaw/controller.m index e7f51c9..c839ca7 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -49,27 +49,29 @@ for i = 1:numClients target = targets(i, :); if coder.target('MATLAB') - disp(['Sending TARGET to client ', num2str(i), ': ', ... + disp(['Sending ', char(MESSAGE_TYPE.TARGET), ' to client ', num2str(i), ': ', ... num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]); else coder.ceval('sendTarget', int32(i), coder.ref(target)); end end -% Wait for TARGET acknowledgments from all clients (simultaneously using select()) +% Wait for ACK from all clients if coder.target('MATLAB') - disp('Waiting for ACK:TARGET from all clients...'); - disp('All TARGET acknowledgments received.'); + disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']); + disp('All acknowledgments received.'); else - coder.ceval('waitForAllTargetAck', int32(numClients)); + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.ACK)); end -% Wait for READY signals from all clients (simultaneously using select()) +% Wait for READY signals from all clients if coder.target('MATLAB') - disp('Waiting for READY from all clients...'); + disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']); disp('All UAVs at target positions.'); else - coder.ceval('waitForAllReady', int32(numClients)); + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.READY)); end % Wait for user input before closing experiment @@ -82,43 +84,61 @@ end % Send RTL command to all clients for i = 1:numClients if coder.target('MATLAB') - disp(['Sending RTL to client ', num2str(i)]); + disp(['Sending ', char(MESSAGE_TYPE.RTL), ' to client ', num2str(i)]); else - coder.ceval('sendRTL', int32(i)); + coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.RTL)); end end -% Wait for RTL_COMPLETE from all clients (simultaneously using select()) +% Wait for ACK from all clients if coder.target('MATLAB') - disp('Waiting for RTL_COMPLETE from all clients...'); + disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']); +else + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.ACK)); +end + +% Wait for READY from all clients (returned to home) +if coder.target('MATLAB') + disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']); disp('All UAVs returned to home.'); else - coder.ceval('waitForAllRTLComplete', int32(numClients)); + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.READY)); end % Send LAND command to all clients for i = 1:numClients if coder.target('MATLAB') - disp(['Sending LAND to client ', num2str(i)]); + disp(['Sending ', char(MESSAGE_TYPE.LAND), ' to client ', num2str(i)]); else - coder.ceval('sendLAND', int32(i)); + coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.LAND)); end end -% Wait for LAND_COMPLETE from all clients (simultaneously using select()) +% Wait for ACK from all clients if coder.target('MATLAB') - disp('Waiting for LAND_COMPLETE from all clients...'); - disp('All UAVs landed and disarmed.'); + disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']); else - coder.ceval('waitForAllLANDComplete', int32(numClients)); + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.ACK)); end -% Send FINISHED to all clients before closing +% Wait for READY from all clients (landed and disarmed) +if coder.target('MATLAB') + disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']); + disp('All UAVs landed and disarmed.'); +else + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.READY)); +end + +% Send READY to all clients to signal mission complete for i = 1:numClients if coder.target('MATLAB') - disp(['Sending FINISHED to client ', num2str(i)]); + disp(['Sending ', char(MESSAGE_TYPE.READY), ' to client ', num2str(i)]); else - coder.ceval('sendFinished', int32(i)); + coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.READY)); end end @@ -129,4 +149,4 @@ end disp('Experiment complete.'); -end \ No newline at end of file +end diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp index aba545c..1d90798 100644 --- a/aerpaw/impl/controller_impl.cpp +++ b/aerpaw/impl/controller_impl.cpp @@ -51,23 +51,6 @@ void acceptClient(int clientId) { std::cout << "Client " << clientId << " connected\n"; } -void sendMessage(int clientId) { - if(clientId <= 0 || clientId > clientSockets.size()) return; - const char* msg = "Hello from server"; - send(clientSockets[clientId-1], msg, strlen(msg), 0); - std::cout << "Sent message to client " << clientId << "\n"; -} - -int receiveAck(int clientId) { - if(clientId <= 0 || clientId > clientSockets.size()) return 0; - char buffer[1024]; - int len = recv(clientSockets[clientId-1], buffer, sizeof(buffer)-1, 0); - if(len <= 0) return 0; - buffer[len] = '\0'; - std::cout << "Received ACK from client " << clientId << ": " << buffer << "\n"; - return 1; -} - void closeServer() { for(auto sock : clientSockets) { close(sock); @@ -129,91 +112,61 @@ int loadTargets(const char* filename, double* targets, int maxClients) { return count; } -// Send target coordinates to a client -// target points to 3 doubles: [x, y, z] -void sendTarget(int clientId, const double* target) { - if (clientId <= 0 || clientId > (int)clientSockets.size()) return; - - char buffer[256]; - snprintf(buffer, sizeof(buffer), "TARGET:%.6f,%.6f,%.6f", - target[0], target[1], target[2]); - - send(clientSockets[clientId - 1], buffer, strlen(buffer), 0); - std::cout << "Sent target to client " << clientId << ": " << buffer << "\n"; +// Message type names for logging +static const char* messageTypeName(uint8_t msgType) { + switch (msgType) { + case 1: return "TARGET"; + case 2: return "ACK"; + case 3: return "READY"; + case 4: return "RTL"; + case 5: return "LAND"; + default: return "UNKNOWN"; + } } -// Receive and validate ACK:TARGET response -// Returns 1 if ACK:TARGET received, 0 otherwise -int receiveTargetAck(int clientId) { +// Send a single-byte message type to a client +int sendMessageType(int clientId, int msgType) { if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0; - char buffer[256]; - int len = recv(clientSockets[clientId - 1], buffer, sizeof(buffer) - 1, 0); - if (len <= 0) return 0; - buffer[len] = '\0'; - - std::cout << "Received from client " << clientId << ": " << buffer << "\n"; - - if (strncmp(buffer, "ACK:TARGET", 10) == 0) { - return 1; + uint8_t msg = (uint8_t)msgType; + ssize_t sent = send(clientSockets[clientId - 1], &msg, 1, 0); + if (sent != 1) { + std::cerr << "Send failed for client " << clientId << "\n"; + return 0; } - return 0; + + std::cout << "Sent to client " << clientId << ": " << messageTypeName(msg) << " (" << (int)msg << ")\n"; + return 1; } -// Wait for READY signal from client -// Returns 1 if READY received, 0 otherwise -int waitForReady(int clientId) { +// Send TARGET message with coordinates (1 byte type + 24 bytes coords) +int sendTarget(int clientId, const double* coords) { if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0; - char buffer[256]; - int len = recv(clientSockets[clientId - 1], buffer, sizeof(buffer) - 1, 0); - if (len <= 0) return 0; - buffer[len] = '\0'; + // Build message: 1 byte type + 3 doubles (little-endian) + uint8_t buffer[1 + 3 * sizeof(double)]; + buffer[0] = 1; // TARGET = 1 + memcpy(buffer + 1, coords, 3 * sizeof(double)); - std::cout << "Received from client " << clientId << ": " << buffer << "\n"; - - if (strncmp(buffer, "READY", 5) == 0) { - return 1; + ssize_t sent = send(clientSockets[clientId - 1], buffer, sizeof(buffer), 0); + if (sent != sizeof(buffer)) { + std::cerr << "Send target failed for client " << clientId << "\n"; + return 0; } - return 0; + + std::cout << "Sent TARGET to client " << clientId << ": " + << coords[0] << "," << coords[1] << "," << coords[2] << "\n"; + return 1; } -// Send COMPLETE message to signal graceful shutdown -void sendFinished(int clientId) { - if (clientId <= 0 || clientId > (int)clientSockets.size()) return; - - const char* msg = "FINISHED"; - send(clientSockets[clientId - 1], msg, strlen(msg), 0); - std::cout << "Sent FINISHED to client " << clientId << "\n"; -} - -// Send RTL (Return-To-Launch) command to a client -void sendRTL(int clientId) { - if (clientId <= 0 || clientId > (int)clientSockets.size()) return; - - const char* msg = "RTL"; - send(clientSockets[clientId - 1], msg, strlen(msg), 0); - std::cout << "Sent RTL to client " << clientId << "\n"; -} - -// Send LAND command to a client -void sendLAND(int clientId) { - if (clientId <= 0 || clientId > (int)clientSockets.size()) return; - - const char* msg = "LAND"; - send(clientSockets[clientId - 1], msg, strlen(msg), 0); - std::cout << "Sent LAND to client " << clientId << "\n"; -} - -// Wait for a specific message from ALL clients simultaneously using select() -// Returns 1 if all clients sent the expected message, 0 otherwise -static int waitForAllMessage(int numClients, const char* expectedMessage) { +// Wait for a specific message type from ALL clients simultaneously using select() +// Returns 1 if all clients responded with expected message type, 0 on failure +int waitForAllMessageType(int numClients, int expectedType) { if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0; - std::vector accumulated(numClients); + uint8_t expected = (uint8_t)expectedType; std::vector completed(numClients, false); int completedCount = 0; - char buffer[512]; while (completedCount < numClients) { // Build fd_set for select() @@ -235,17 +188,17 @@ static int waitForAllMessage(int numClients, const char* expectedMessage) { // Check each socket for (int i = 0; i < numClients; i++) { if (!completed[i] && FD_ISSET(clientSockets[i], &readfds)) { - int len = recv(clientSockets[i], buffer, sizeof(buffer) - 1, 0); - if (len <= 0) return 0; - buffer[len] = '\0'; - std::cout << "Received from client " << (i + 1) << ": " << buffer << "\n"; - accumulated[i] += buffer; + uint8_t msgType; + int len = recv(clientSockets[i], &msgType, 1, 0); + if (len != 1) return 0; - // Check if expected message received - if (accumulated[i].find(expectedMessage) != std::string::npos) { + std::cout << "Received from client " << (i + 1) << ": " + << messageTypeName(msgType) << " (" << (int)msgType << ")\n"; + + if (msgType == expected) { completed[i] = true; completedCount++; - std::cout << "Client " << (i + 1) << " completed " << expectedMessage << "\n"; + std::cout << "Client " << (i + 1) << " completed: " << messageTypeName(expected) << "\n"; } } } @@ -254,30 +207,6 @@ static int waitForAllMessage(int numClients, const char* expectedMessage) { return 1; } -// Wait for ACK:TARGET from ALL clients simultaneously -// Returns 1 if all clients acknowledged, 0 otherwise -int waitForAllTargetAck(int numClients) { - return waitForAllMessage(numClients, "ACK:TARGET"); -} - -// Wait for READY from ALL clients simultaneously -// Returns 1 if all clients are ready, 0 otherwise -int waitForAllReady(int numClients) { - return waitForAllMessage(numClients, "READY"); -} - -// Wait for RTL_COMPLETE from ALL clients simultaneously -// Returns 1 if all clients completed RTL, 0 otherwise -int waitForAllRTLComplete(int numClients) { - return waitForAllMessage(numClients, "RTL_COMPLETE"); -} - -// Wait for LAND_COMPLETE from ALL clients simultaneously -// Returns 1 if all clients completed LAND, 0 otherwise -int waitForAllLANDComplete(int numClients) { - return waitForAllMessage(numClients, "LAND_COMPLETE"); -} - // Wait for user to press Enter void waitForUserInput() { std::cout << "Press Enter to close experiment (RTL + LAND)...\n"; diff --git a/aerpaw/impl/controller_impl.h b/aerpaw/impl/controller_impl.h index c0939ca..5f6ac05 100644 --- a/aerpaw/impl/controller_impl.h +++ b/aerpaw/impl/controller_impl.h @@ -1,34 +1,28 @@ #ifndef CONTROLLER_IMPL_H #define CONTROLLER_IMPL_H +#include + #ifdef __cplusplus extern "C" { #endif +// Server lifecycle void initServer(); void acceptClient(int clientId); -void sendMessage(int clientId); -int receiveAck(int clientId); void closeServer(); -// Target location protocol functions +// Configuration int loadTargets(const char* filename, double* targets, int maxClients); -void sendTarget(int clientId, const double* target); -int receiveTargetAck(int clientId); -int waitForReady(int clientId); -void sendFinished(int clientId); -// Parallel wait functions (using select() for simultaneous processing) -int waitForAllTargetAck(int numClients); -int waitForAllReady(int numClients); -int waitForAllRTLComplete(int numClients); -int waitForAllLANDComplete(int numClients); - -// RTL and LAND protocol functions -void sendRTL(int clientId); -void sendLAND(int clientId); +// User interaction void waitForUserInput(); +// Binary protocol operations +int sendMessageType(int clientId, int msgType); +int sendTarget(int clientId, const double* coords); +int waitForAllMessageType(int numClients, int expectedType); + #ifdef __cplusplus } #endif diff --git a/geometries/REGION_TYPE.m b/geometries/REGION_TYPE.m index d271234..8a15dd1 100644 --- a/geometries/REGION_TYPE.m +++ b/geometries/REGION_TYPE.m @@ -1,4 +1,4 @@ -classdef REGION_TYPE +classdef REGION_TYPE < uint8 properties id color