From 1ae617d5f76c5403340074ad2622c05bbbc00f05 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Thu, 29 Jan 2026 16:42:19 -0800 Subject: [PATCH] sending starting positions to agents (not verified on AERPAW yet) --- aerpaw/basic_demo/{handcode => }/comms.cpp | 2 + aerpaw/basic_demo/compile.sh | 10 +- aerpaw/basic_demo/controller.cpp | 37 +- aerpaw/basic_demo/controller_app | Bin 24832 -> 0 bytes aerpaw/basic_demo/controller_data.h | 20 + aerpaw/basic_demo/controller_impl.cpp | 160 ++++ .../{handcode => }/controller_impl.h | 7 + aerpaw/basic_demo/controller_initialize.cpp | 19 + aerpaw/basic_demo/controller_initialize.h | 23 + .../{handcode => }/controller_main.cpp | 2 +- aerpaw/basic_demo/controller_terminate.cpp | 19 + aerpaw/basic_demo/controller_terminate.h | 23 + aerpaw/basic_demo/controller_types.h | 18 + aerpaw/basic_demo/copy_codegen.sh | 3 + .../basic_demo/handcode/controller_impl.cpp | 73 -- aerpaw/basic_demo/targets.txt | 2 + aerpaw/basic_demo/tmwtypes.h | 888 ++++++++++++++++++ aerpaw/basic_demo/uav.py | 96 +- aerpaw/controller.coderprj | 74 +- aerpaw/controller.m | 77 +- 20 files changed, 1398 insertions(+), 155 deletions(-) rename aerpaw/basic_demo/{handcode => }/comms.cpp (98%) delete mode 100755 aerpaw/basic_demo/controller_app create mode 100644 aerpaw/basic_demo/controller_data.h create mode 100644 aerpaw/basic_demo/controller_impl.cpp rename aerpaw/basic_demo/{handcode => }/controller_impl.h (51%) create mode 100644 aerpaw/basic_demo/controller_initialize.cpp create mode 100644 aerpaw/basic_demo/controller_initialize.h rename aerpaw/basic_demo/{handcode => }/controller_main.cpp (99%) create mode 100644 aerpaw/basic_demo/controller_terminate.cpp create mode 100644 aerpaw/basic_demo/controller_terminate.h create mode 100644 aerpaw/basic_demo/controller_types.h create mode 100755 aerpaw/basic_demo/copy_codegen.sh delete mode 100644 aerpaw/basic_demo/handcode/controller_impl.cpp create mode 100644 aerpaw/basic_demo/targets.txt create mode 100644 aerpaw/basic_demo/tmwtypes.h diff --git a/aerpaw/basic_demo/handcode/comms.cpp b/aerpaw/basic_demo/comms.cpp similarity index 98% rename from aerpaw/basic_demo/handcode/comms.cpp rename to aerpaw/basic_demo/comms.cpp index 9579959..fb71d12 100644 --- a/aerpaw/basic_demo/handcode/comms.cpp +++ b/aerpaw/basic_demo/comms.cpp @@ -9,6 +9,8 @@ #define SERVER_PORT 5000 #define SERVER_IP "127.0.0.1" +#define OPCODE_SHUTDOWN 0xFF + // Socket init void initSockets() {} void cleanupSockets() {} diff --git a/aerpaw/basic_demo/compile.sh b/aerpaw/basic_demo/compile.sh index f0a5adc..874826f 100755 --- a/aerpaw/basic_demo/compile.sh +++ b/aerpaw/basic_demo/compile.sh @@ -1,7 +1,9 @@ #!/bin/bash #g++ -I/home/kdee/matlab/R2025a/extern/include controller_main.cpp controller.cpp controller_impl.cpp -o controller_app -lpthread -wd=$(pwd) -cd /home/kdee/Desktop/miSim/aerpaw/codegen -g++ -I/home/kdee/matlab/R2025a/extern/include -I../basic_demo/handcode -I. ../basic_demo/handcode/controller_main.cpp controller.cpp ../basic_demo/handcode/controller_impl.cpp controller_initialize.cpp controller_terminate.cpp -o controller_app -lpthread -cd $wd +#wd=$(pwd) +#cd /home/kdee/Desktop/miSim/aerpaw/codegen +g++ -I/home/kdee/matlab/R2025a/extern/include -I. controller_main.cpp controller.cpp controller_impl.cpp controller_initialize.cpp controller_terminate.cpp -o controller_app -lpthread +#cd $wd + +#g++ controller_main.cpp controller.cpp controller_impl.cpp controller_initialize.cpp controller_terminate.cpp diff --git a/aerpaw/basic_demo/controller.cpp b/aerpaw/basic_demo/controller.cpp index 32f5aef..f68385a 100644 --- a/aerpaw/basic_demo/controller.cpp +++ b/aerpaw/basic_demo/controller.cpp @@ -15,21 +15,48 @@ // Function Definitions void controller(int numClients) { + static const char b_filename[12]{"targets.txt"}; + double targets[12]; + char filename[12]; + // Maximum clients supported + // Allocate targets array (MAX_CLIENTS x 3) + // Load targets from file + // Define filename as null-terminated character array for C compatibility + for (int i{0}; i < 12; i++) { + targets[i] = 0.0; + filename[i] = b_filename[i]; + } + // loadTargets fills targets array (row-major: x1,y1,z1,x2,y2,z2,...) + loadTargets(&filename[0], &targets[0], 4); // Initialize server initServer(); // Accept clients for (int i{0}; i < numClients; i++) { acceptClient(i + 1); } - // Send messages to clients + // Send target coordinates to each client for (int i{0}; i < numClients; i++) { - sendMessage(i + 1); + double target[3]; + // Get target for this client (1x3 array) + target[0] = targets[i]; + target[1] = targets[i + 4]; + target[2] = targets[i + 8]; + sendTarget(i + 1, &target[0]); } - // Receive acknowledgements + // Receive TARGET acknowledgments for (int i{0}; i < numClients; i++) { - receiveAck(i + 1); + receiveTargetAck(i + 1); + } + // Check all ACKs received + // Wait for READY signals (UAVs have reached their targets) + for (int i{0}; i < numClients; i++) { + waitForReady(i + 1); + } + // Check all READY signals received + // Send COMPLETE to all clients before closing + for (int i{0}; i < numClients; i++) { + sendFinished(i + 1); } - // Digest ACKs // Close server closeServer(); } diff --git a/aerpaw/basic_demo/controller_app b/aerpaw/basic_demo/controller_app deleted file mode 100755 index e917ebbcd0ef54f1b06b07bd55eab641d6d98acd..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 24832 zcmb<-^>JfjWMqH=W(GS35U)WHBH{p{7#t#@3;MUYXb@gt3F0v@pwkKvVFnltQVS9aep-?OVxzkc#>eH(0H{75BakT!4Cr(XD54k` z7+^HW4v@;grzI(%cmT19!LZ~J1ho%W{AEDHA4Ws12j>M)5&=c4yI&{+Qv<^RkXvB# zj0}t@d?qwLNUaD112}(x^w%_Qyj@@Qs&@N~4Cb^ui*>v>E}hWN$xJdc(a%ZI&B@Fw ztS~(>T=M!=Zi- z4slT&;=VYZRBW(#bGZf{bN(F!@vM9BaqFx zfy10&Mg|5!1}O%HK!|DqkiiTL4E+ocafKjMafbN#^xXWs_~MepqLTP{hIr2q-}sc& zqSW-v;*!*&5MSq<{JhkV#H5^5h#YpAsNfPKgZTK8jH3MV_?*zN!9?;30x5AkSTImnyAB__%Fr6ml-sU^kv z$=Ue@B@FSP5KhjHPtM2&+s2TTnU}(lQIelm%m7l8TEb9VQk0XL$55P_m%>n#3JQ$) z6yS;h6tuHf@!2@3QC_$3?Rr1CRxBF z69WqaD+3z?I|GQt!ob16$-u$w>iI2a6Ie6TzlgDsR_qB)5J)ROao@*TkP z%nT8r3I$xBurTmIS6o?{t$6c9SD;HiJU+Z z=K%>o@dYGtsFh%u8%W|(U=axM07+aLA_OK6K-~i>mc+m!Py$+Bf!YP2x)>_T04uLR z;-LB&Ce8pghaDXDa8U*hByng^f+Yoz#Gy?&u($-0IH+v_76g^KNa7&7Vd5G{;-IjD zi5nn^gWLoYw?GnyHif}b4oKo$U=ax6fh5ii5dxC|NaD~YJXkyeNt_QX0wEHR#Q7mY zU@`+q98^bv1sNC^3XsI1T^q1e1(G;4NrA;1ki?-~1F(1pk~pZH1{P#sV3>d;E)El9 zU|^VmBrb_0z5q!a*2h7T9%YV(09``hGr!y~kLEWV9^I@b?HL$6S`U;k{lDPRe1zjL z*#7^fr|cOR{;Qs`XJFu$cVPIh3gTyg%MD=uDi9x36un#k<}U*AK}FEZ31I##5Fb?ZylepTCxQ5&BIjiRnBN8B zgNpQ*31EH`hz}}yUIu{qRUkg7$a(1i<`;qZprYob0hpf!;)9C*mkMBh5{M5fa$X96 z`B5M~sHl0#0OkjQ_@E-@<%fSD|9XM=prYmF12EqS#0M2AFE4=kRvR4Pd?&hz}|{UM~3e|34`F4c~fpUX1hTeB{ymG+tKk8|lODacw-^{0{);Nx zF)(~d)8m(K0fjk(N3X3QD4u#*xj>ZVj}lRjZdNusNYruuf50!_08)4uN_+I$J_Jej zn%)9Yn*TgHKX@E`XYX*KYF3`Y)N9WTLUXO0uJUfsRD{L7UUL^nj|Nj{49R~&m#@NFM_Z|i%nluC- zWZuj4|NsAodUQVZXnrGb-1QGAv3T@~mdY|Pc=WPnf!x`7&7<@9i}-*4|989o={)`d z)OGOa70rVvaYrbz`S<_-4v>dW9)_Bz&{_NDMGXT3!*SOyAc@z4-L7An4={Eff3bp* zfuY&;3uCwI7i-ryHP4T`{(!0Y0Z~!R1XA&XvD@_rRK*{dia!t)ip(Gte;B)6|A1Az zh-P76m;kYTLM+%3P|BnEjX{b>H;bwU0|SFcr|S>H123-q|Nnm%$i#i12Eu-j&tIJU z^Z$P*tF|ozLpQ6cEdxXAfl4`#UeQ@H;NYl6_%-0~|Nk$~F)%Qs9d~`e#Bkj83CN`V z9l{I@FH-*e|DV?B`sldp9gxZ0tha4I+C%R+?f~UAkIsv`K$d~6?L7V>_3!`x-K^We zYJ4B;0wrgU&U2l|U+Db#|Nr0%`IZAE(jL8{dJs2!2c-^3l$`nV|Nm>>Zq^zb28Mm0 zT-$m4#Sauc><~SN5qf5T^hkR2ivE@cxrwy{WG%$F+CTsQdvv@0Nb%5ygi{IYij9Ac5Pxj_{r|rQlBr)Ie2-pMdyra)sS`kgXr5j38|K+ykLEWTowawG zYws}BNp-v4@kqYm(aSm;?DXR=HiE3Syiv@)0~EinS;2|E%5=w*!nsf7q`{`LR=YsLv4-LXF)kr(XI z_{M^TfuVEnncx5aBgf$j!voD=L54cn?pBa2#H$;?>Eif{W6TT;9++M&;R5^lXzq9qu@Bjas_ku`Az z^0FH&;W6V_3oA$?gGaCE9!XGKv);CZhr@-RpddX2u^ZxRUP#b*^s+7oDTfHIfT;86 zZ3UU-0TG-G6J%9JsI2?>|35fBJ-S0*qx}E{0`r|Juz=peC@Mu23;?e1P zVS-1m>jjTaS4i6F7VzkGz2VX6`oW_cgc&?65Ajbu&~l)3M>lJ$1vo6te*XXeLh$$h z{}a$N@0}m8^z6~gdLNvIj=um6&Uo~SI#_^0l=mb^w3oGx3ly;DKrT=J@&7-x(0p<5 z|NsAxV$`Fv_J>DjDcBjPB2cv+&2JPuI%^@$?sWYDatp|~M@$S16QD&hsEPp_)b08M zBnPTUCY(Gx0a{rIAj_Tk|NlRd9J1M9M}W%k2_PSXGngO97HlSU2MB;&(#_zZdB~&l zg2%xhOdg$wAYvR~suS#OHzo!K2>1949VEL!B|(HoXYC4)&e8=Q-L5M5)K?5-JvTyIzu;jbh_^F=yqM;(dl}?qucd>N2luvk8ThS-Qm$03X0$x z9^I}Wbq_qcV=s7g-rEnV8eUBJ2P%U@Z+LWuz5u1eZlO*Aa2?fhsf633^BySJ>i+%z z5B5694ICc50W4s5bp~*}7WU{4g}4$@6oRT4XfXf@WmpVBJn_NJKYj2=ut8Ub!-|6}S65-vUpgIbk zKfitjC3A@Xn%@}UGO76h2XaQjW!iEi(?E$Hlva;}3TAMycpP^CHFsfbP~FD>%K@FW zKPJS&Q?moe9Uh&f7d$#cPk1!fUSKFO@PPOTlu1BI0F;nVfD*Dtcjym~ZeI}pg$E=( zo&dFEp!o(=dfx!mES-BbK!M-671UZr4y_lULP&I-C@2lLg4BQ_o&lU#KZis=My`Ub9N9WWE zkS(3A4IbUD4IZ7Y9Uk2v3>6NY;L+(i!=t+uqS$qTM|bQ5cxHVJs&KkPXLxjmt^nnH zY?<}qFGyyE2i`*_P*Hmj=Cbas(C`LFMCb7rePDTr=bOP4i$^yknvh}vRQPB^M8PGR z>kUY~ar}iKNIyJ`!M^VlfW>F`RB)hnf{HLmn&=g2g#;8N)EN)Kly(LPfO8viYq0wh zC=pw^BcU1i8y~2Pnap zL)a%kY)+7VgmJ$>@zn_ms?Yb4@x( zowe(OVm_EiC?f*{*qaYPWi@h}Arb5fP)NbbB}fg{S^B`Ex%L4=i8iEd2&&XUEe24* z=nJYeKY&YnXdxgDs@9;*OIYm&Dw9Bg4yqtJeL>Fs0WMHMkqj~=^o0jFdco`m9-wH1 z!~sg}+YKIFPy*L@ptJ-ofPy$YxiXc(4fYEtC4frl7arZd2SApBdav-102S>g zkW(JyFTEbgi=pSdB9}tL9)fHFGAn_ z|NlbZ9XJa;@HqH@3FP61ci{FH*iuNPim;~B7t*fPc6|Y9PeI!ab*kX>)N2AQQeZB+ z=+XHJ-10vE;^$kiH9tHMQGWxY9RR5&kH6UV7o-?u3OHrn@aX3B==6Qz(HZ&y>`hQR z0+fs(bqJ^~29;PhK+gaC72$knDwz-q8bt$*d4R{*;G=4w;%^j>hQMeDjE2By2#kin zXb6mkz-S1JhQMeDjD`SxLjbh4%rh^uBr`E5vnn$$T_MCdK%qFbs4TTePfw4FAsEb5 zNXyL2EY3(x(F0AagIpM#pPZdqqL5sanpl#VpQiv?VVIi2#o&~gmjdVbfLHLrxlpq} zOB})0fz&&Km-9iDD5T{VDJ17)rskC>FmN$ALs<&R`FVM%$t5859;rDw`3h-8`MFS6 zG6aKE=cX1HC#I(=l;p$p2c;&bW|pOysnpd2eo|l>eGACMTSC|+WydVDm|AmQxq4&}M z|02u`3{{W+|2JV~VDNhK|9=EC1H;s(|Nqy3_|N|T=V51HXn6Mje-1kXL-(`)|3#P> z80J3v|KEg(fnm+F|Nldn7#J=*`~Sa$iGktGv;Y66FflN2JpccH4l@IT+4KMZ?=Ukk zv^@X+AG8=U=*9p4Dl7~PO)viccVS^*c=6)@f6xX4qnH2xx3DlUG`#%(e+df%!@8IM z{~uvtVA%EY|Nkc}3=ABv{{LrTWnh^9=Kp^URtAPsZ~p)H0MT#%|IcA%U=Vrt|9=lF z1B2hY|NqypGB9+%`~Uw8D+9y6cmMytVP#-A^X~tD9ySJsfA9YP*I{E|PHKf}hr(Ej27|2J$53?DxH|F6T&z~J`r z|9>BL1_scA5Ku%iRs}IIRtPXk^RRPFV1$T+)`#7=`~SZhNP!DGXwU(?AH#%!fg$0} z|NjY~8DTyFH$Dk3e(rLP1_pa6Yb|3H@bY+&-WUc3hKcw7{|7IPcH|RiV{+zYWAf!; z=U`v}$%7V49KQGee;vqtxO^pCehvczL;jQh|3Qmd7+~@c?O^?T7#J85pZxz1S|kIP z_X2AN%im#OU$aZ)%Gc((PtpSGmti%fTh9uLBfnx0Y2aq#6J6}6a#}aEFFR7Jwf3g^!WdO z&{A5K#mo$Fw=#Kv4FRd?U}9jXdHnzXS&(6{umJhxFjxuLo;6Gi3~EpQ{|D`Jft!(y zkUzu3z+m>||Nk^(Gnf*=3PAn<&7ssh`TyS&McxH2&%?~Xu&38k{=P)xcoO|;BzaO$ZlP6dKC=7a-85sUN z`TrlZf*Ni_uV3_sd|9{YmEVw%>5b_}`3=G>}{QnQ$G6jpv zT!efH3j@Qc7yti*7Q4aqLxKSuj-XW{J6`<%Z-{Jv4nqGHsD5xk12aDsA%BI1f#J*x z>|q4-%TAj)O+Q9_j!}j2aLbz}es*T|VbpH)h8^aH%M`7(~ za9fapfdRG`V+Ob}0NSAhF{%REk%hTi0jeLgkQ}4}X8-U15dScMHcv4yF#Ln^cR&^X zhw@?W_yFaXK;?fz`2$^E!}gDZ{DzFpz`Y3u1~(`j2Bp)WbQzRxgVNKW^fD;D4N4z_ z($}E$GbsHHO0$7h$TKi7h(T#JC~XF%-JoU+3?ISGG-Oqv=woCMhVN?!wF^P=poRR{)XOt~ zcDL~|Y(R6bB2@eVnmDNM#>BwO0L#}Pvq2ct7Gh+OVlW5>@gV!SK`c-kgONcLz7Gy0 z2Ew4W5y%`^c>xjwVbBUeMg|cESUv`cf$$3Oc@3EU1@+;u`RgD`_W=Oc*fyN80zM26Q{{r2#35%Dl zU~!O7P%)^!0*`N?3Nt(gtLJ49fmU?jJ|P1GgCq;Y-ae@L;Jz*c1A`F@C>(ehDxrpf z$6Oc~7|g-qXoiE>zPeJ3+hz9O6Yd#M{B*ybPzI=@UHW z$H2faogEUM=;;A8CMLwd%isyB_!toFr|V#IKrTSVpFrYF+@L}TMdAlY97P1e;pSjq z0Hs^>@=BZo;vV$$Zw?iggO*F+u?q$UhHQ{Ipw=mLpEc}Uh(@S-E=7oaP;Cs0pyHFE z0^l(K(78IC5PNl?>S5_n4JwWvk1kO0Q_yq;b5A)ZC zP7t&m4jxNoU|?7X75{-&zU_jFqnCTPpytEQ$AFpt44SUmpbcv9SS14k!zZvfOcjEb zV8l@0jaOSVQ)9o{FTt~gvIX^9O~CW)h~p)2h`33nSYHNR6k0=&kca3 z^EY7kfK;I3e^7Jcp!F1Z%oe01IJv0AP%ob$K0YxiGrlA-ogulXBq=pLGcQ;#p8;_~ zN_}zk^ql;p#GLq)lKi6L_{7o*hUEO*f}GTn)D%4f>?-0xlZWw% z;Im3n^Gb>;8PbXpb5r9}N^^578RFxSc;MqtP?RR;=am%Y=j5aofe!`IOD-s2K!~An z<1=#$a=>EoQ9g!7kZDZFOeF(I#27LI2oWh}h>tQc1fN?{l9>#)ITJJq3ZBo5Ps~e+ zPf0BYn)8iJ-Wp{A9A!Y?&11r&Pm@oqtm zzOM1EelGFx4Ds>qexdQM9#BCKmmr3CmqJVmvd06FoCsT@j)nzq%N?g3f%wt=cf&7jpDWVo9oNF62BG2k?<6@kV-} z!$?5Kw4exp4zVdQgG>@4S?XSzSd;=W%Egd?rErIXoahX9BATI=1*OFq@kxov*{(s} znGEr%DTyVC(6eGlbrCK*piT`gA$TSZ#Ks8dIWPv$V`dZI!+X#y!E-VaL?vOLL!$|IGDb~N;Bbmh zDbA12NX$zC9o!U!TdO%N7l9IEJSdIfOp%60@kq(s+0Zp0GZ%Cq65MvsNll)a!3OcJ z3@+h%MX70e@$u={4r;>f9JpB^=Ro`oDjp%}0xj5$Q}a^5v5s~!5+qpTjr5F7z=_T{ zH#5&O*EPsHBtF;(gdnCNiVCQ^Aq9wWJm>%{@Wel;PzcBbo#^BjjGpJAMN4pr5&k2m zAdZ2=v7V77#A-B;7#hY0#}}pMg105WO853y9(g%o3%Mx=+Q(XgsjpAKRKudC5gH7TgC@475GX#{MK_|8u=O$KQ%SYIf z79^eEPFlFa0$c(?PB~+U2Q9h*S!@yS0%~gHS9%6yx|W5+2ODB9hY=+g_Ph;|k54Kt zhS~rsu6-d(qrfEvt^?kv{$Lb-w5@a<-@H4BO@LnGuw=@|g35Fm}S5QF%T_+VpD5gZg^7!Nt5 zE*QLmjse_MgfP*MBm^HH7z{m|5Xbq0Sc(Nup$gA0LEfO!6y!V;(D8y0M?Ygfza&*JJ+D-+pa`_qt)!Ac4=$8cnwgWL zo0-C(mzh_RT2#OQR+y1koWY=%Qkhp=nG2yyiXbv&sYS(^`FSXu_#y_qqSTy3kOnBL zAg2UmVrB`0UP)?234DP0cF-b)qQ8 zpkX18OyVe^78`UO-1G<^sf z%7^KPcB2`hp=aenl`+8P8DTVRUJ>L55C)k6qG320bjT8t`(g8TFxn2P5axcEJun({ z_9AFb5XOhi3&Q9FQ2ikHgVey>4;5yp2aQ#L=ZGL8uz5)s4V#yQ*$>kX^FL^w2Nb7H z5bX@Gc}^G&n->M?2kAlAKN%YTAoqcdfX>&!CVWBZ6D$rTKz2cyV5$#IzXDW0j0Uew z1gnG)P-&R)450I9L2&}JA2#j@qaUKpo5A!$Ok-e}k7hq?92!Q$=C@($(cQlksvi_L zU^TD-5-1Hi^Aao$C1BwP6lpzX+Os*gV+^Xu%I1)`V$i zfVm$gAjJrA6U_gxao-bA{jl>}Vd|m6FfM~U)P87khl|7XqvwBE`az~)X$P*Efq~%# z)FJ^;;m*Lo06Oy&TloW;XNS8T$$%f;5RNo_!3u;AvlqexjfEoO4kiYog(E;D1A_*d I1~e`M058bHWdHyG diff --git a/aerpaw/basic_demo/controller_data.h b/aerpaw/basic_demo/controller_data.h new file mode 100644 index 0000000..a7a15dc --- /dev/null +++ b/aerpaw/basic_demo/controller_data.h @@ -0,0 +1,20 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_data.h +// +// Code generation for function 'controller_data' +// + +#ifndef CONTROLLER_DATA_H +#define CONTROLLER_DATA_H + +// Include files +#include "rtwtypes.h" +#include +#include + +#endif +// End of code generation (controller_data.h) diff --git a/aerpaw/basic_demo/controller_impl.cpp b/aerpaw/basic_demo/controller_impl.cpp new file mode 100644 index 0000000..c686efa --- /dev/null +++ b/aerpaw/basic_demo/controller_impl.cpp @@ -0,0 +1,160 @@ +#include "controller_impl.h" +#include +#include +#include +#include +#include +#include +#include + +#define SERVER_PORT 5000 +#define SERVER_IP "127.0.0.1" + +static int serverSocket = -1; +static std::vector clientSockets; + +void initSockets() {} +void cleanupSockets() {} + +void initServer() { + initSockets(); + serverSocket = socket(AF_INET, SOCK_STREAM, 0); + if(serverSocket < 0) { std::cerr << "Socket creation failed\n"; return; } + + sockaddr_in serverAddr; + serverAddr.sin_family = AF_INET; + serverAddr.sin_addr.s_addr = INADDR_ANY; + serverAddr.sin_port = htons(SERVER_PORT); + + int opt = 1; + setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&opt, sizeof(opt)); + + if(bind(serverSocket, (sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) { + std::cerr << "Bind failed\n"; return; + } + if(listen(serverSocket, 5) < 0) { + std::cerr << "Listen failed\n"; return; + } + + std::cout << "Server initialized\n"; +} + +void acceptClient(int clientId) { + sockaddr_in clientAddr; + socklen_t addrLen = sizeof(clientAddr); + int clientSock = accept(serverSocket, (sockaddr*)&clientAddr, &addrLen); + if(clientSock < 0) { std::cerr << "Accept failed for client " << clientId << "\n"; return; } + clientSockets.push_back(clientSock); + 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); + } + close(serverSocket); + cleanupSockets(); +} + +// Load target coordinates from file +// File format: one line per UAV with "x,y,z" coordinates +// Returns number of targets loaded +int loadTargets(const char* filename, double* targets, int maxClients) { + FILE* file = fopen(filename, "r"); + if (!file) { + std::cerr << "Failed to open targets file: " << filename << "\n"; + return 0; + } + + int count = 0; + double x, y, z; + // MATLAB uses column-major order, so for a maxClients x 3 matrix: + // Column 1 (x): indices 0, 1, 2, ... + // Column 2 (y): indices maxClients, maxClients+1, ... + // Column 3 (z): indices 2*maxClients, 2*maxClients+1, ... + while (count < maxClients && fscanf(file, "%lf,%lf,%lf", &x, &y, &z) == 3) { + targets[count + 0 * maxClients] = x; // Column 1 + targets[count + 1 * maxClients] = y; // Column 2 + targets[count + 2 * maxClients] = z; // Column 3 + std::cout << "Loaded target " << (count + 1) << ": " << x << "," << y << "," << z << "\n"; + count++; + } + + fclose(file); + 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"; +} + +// Receive and validate ACK:TARGET response +// Returns 1 if ACK:TARGET received, 0 otherwise +int receiveTargetAck(int clientId) { + 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; + } + return 0; +} + +// Wait for READY signal from client +// Returns 1 if READY received, 0 otherwise +int waitForReady(int clientId) { + 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, "READY", 5) == 0) { + return 1; + } + return 0; +} + +// 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"; +} diff --git a/aerpaw/basic_demo/handcode/controller_impl.h b/aerpaw/basic_demo/controller_impl.h similarity index 51% rename from aerpaw/basic_demo/handcode/controller_impl.h rename to aerpaw/basic_demo/controller_impl.h index 0e1328a..4781321 100644 --- a/aerpaw/basic_demo/handcode/controller_impl.h +++ b/aerpaw/basic_demo/controller_impl.h @@ -11,6 +11,13 @@ void sendMessage(int clientId); int receiveAck(int clientId); void closeServer(); +// Target location protocol functions +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); + #ifdef __cplusplus } #endif diff --git a/aerpaw/basic_demo/controller_initialize.cpp b/aerpaw/basic_demo/controller_initialize.cpp new file mode 100644 index 0000000..2510b0d --- /dev/null +++ b/aerpaw/basic_demo/controller_initialize.cpp @@ -0,0 +1,19 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_initialize.cpp +// +// Code generation for function 'controller_initialize' +// + +// Include files +#include "controller_initialize.h" + +// Function Definitions +void controller_initialize() +{ +} + +// End of code generation (controller_initialize.cpp) diff --git a/aerpaw/basic_demo/controller_initialize.h b/aerpaw/basic_demo/controller_initialize.h new file mode 100644 index 0000000..82e0ed2 --- /dev/null +++ b/aerpaw/basic_demo/controller_initialize.h @@ -0,0 +1,23 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_initialize.h +// +// Code generation for function 'controller_initialize' +// + +#ifndef CONTROLLER_INITIALIZE_H +#define CONTROLLER_INITIALIZE_H + +// Include files +#include "rtwtypes.h" +#include +#include + +// Function Declarations +extern void controller_initialize(); + +#endif +// End of code generation (controller_initialize.h) diff --git a/aerpaw/basic_demo/handcode/controller_main.cpp b/aerpaw/basic_demo/controller_main.cpp similarity index 99% rename from aerpaw/basic_demo/handcode/controller_main.cpp rename to aerpaw/basic_demo/controller_main.cpp index 09fc0ea..8a8120e 100644 --- a/aerpaw/basic_demo/handcode/controller_main.cpp +++ b/aerpaw/basic_demo/controller_main.cpp @@ -13,4 +13,4 @@ int main() { std::cout << "Server finished.\n"; return 0; -} +} \ No newline at end of file diff --git a/aerpaw/basic_demo/controller_terminate.cpp b/aerpaw/basic_demo/controller_terminate.cpp new file mode 100644 index 0000000..1577159 --- /dev/null +++ b/aerpaw/basic_demo/controller_terminate.cpp @@ -0,0 +1,19 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_terminate.cpp +// +// Code generation for function 'controller_terminate' +// + +// Include files +#include "controller_terminate.h" + +// Function Definitions +void controller_terminate() +{ +} + +// End of code generation (controller_terminate.cpp) diff --git a/aerpaw/basic_demo/controller_terminate.h b/aerpaw/basic_demo/controller_terminate.h new file mode 100644 index 0000000..723736f --- /dev/null +++ b/aerpaw/basic_demo/controller_terminate.h @@ -0,0 +1,23 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_terminate.h +// +// Code generation for function 'controller_terminate' +// + +#ifndef CONTROLLER_TERMINATE_H +#define CONTROLLER_TERMINATE_H + +// Include files +#include "rtwtypes.h" +#include +#include + +// Function Declarations +extern void controller_terminate(); + +#endif +// End of code generation (controller_terminate.h) diff --git a/aerpaw/basic_demo/controller_types.h b/aerpaw/basic_demo/controller_types.h new file mode 100644 index 0000000..33c4323 --- /dev/null +++ b/aerpaw/basic_demo/controller_types.h @@ -0,0 +1,18 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// controller_types.h +// +// Code generation for function 'controller' +// + +#ifndef CONTROLLER_TYPES_H +#define CONTROLLER_TYPES_H + +// Include files +#include "rtwtypes.h" + +#endif +// End of code generation (controller_types.h) diff --git a/aerpaw/basic_demo/copy_codegen.sh b/aerpaw/basic_demo/copy_codegen.sh new file mode 100755 index 0000000..41d6a92 --- /dev/null +++ b/aerpaw/basic_demo/copy_codegen.sh @@ -0,0 +1,3 @@ +#!/bin/bash +cp ../codegen/*.h . +cp ../codegen/*.cpp . diff --git a/aerpaw/basic_demo/handcode/controller_impl.cpp b/aerpaw/basic_demo/handcode/controller_impl.cpp deleted file mode 100644 index dcfa649..0000000 --- a/aerpaw/basic_demo/handcode/controller_impl.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include "controller_impl.h" -#include -#include -#include -#include -#include -#include - -#define SERVER_PORT 5000 -#define SERVER_IP "127.0.0.1" - -static int serverSocket = -1; -static std::vector clientSockets; - -void initSockets() {} -void cleanupSockets() {} - -void initServer() { - initSockets(); - serverSocket = socket(AF_INET, SOCK_STREAM, 0); - if(serverSocket < 0) { std::cerr << "Socket creation failed\n"; return; } - - sockaddr_in serverAddr; - serverAddr.sin_family = AF_INET; - serverAddr.sin_addr.s_addr = INADDR_ANY; - serverAddr.sin_port = htons(SERVER_PORT); - - int opt = 1; - setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&opt, sizeof(opt)); - - if(bind(serverSocket, (sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) { - std::cerr << "Bind failed\n"; return; - } - if(listen(serverSocket, 5) < 0) { - std::cerr << "Listen failed\n"; return; - } - - std::cout << "Server initialized\n"; -} - -void acceptClient(int clientId) { - sockaddr_in clientAddr; - socklen_t addrLen = sizeof(clientAddr); - int clientSock = accept(serverSocket, (sockaddr*)&clientAddr, &addrLen); - if(clientSock < 0) { std::cerr << "Accept failed for client " << clientId << "\n"; return; } - clientSockets.push_back(clientSock); - 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); - } - close(serverSocket); - cleanupSockets(); -} diff --git a/aerpaw/basic_demo/targets.txt b/aerpaw/basic_demo/targets.txt new file mode 100644 index 0000000..3165440 --- /dev/null +++ b/aerpaw/basic_demo/targets.txt @@ -0,0 +1,2 @@ +10.5,20.3,45.0 +25.0,30.0,30.0 \ No newline at end of file diff --git a/aerpaw/basic_demo/tmwtypes.h b/aerpaw/basic_demo/tmwtypes.h new file mode 100644 index 0000000..0ee3d69 --- /dev/null +++ b/aerpaw/basic_demo/tmwtypes.h @@ -0,0 +1,888 @@ +/* + * Copyright 1984-2023 The MathWorks, Inc. + */ + +#if defined(_MSC_VER) +# pragma once +#endif +#if defined(__GNUC__) && (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ > 3)) +# pragma once +#endif + +#ifndef tmwtypes_h +#define tmwtypes_h + +#ifndef __TMWTYPES__ +#define __TMWTYPES__ +/* + * File : tmwtypes.h + * Abstract: + * Data types for use with MATLAB/SIMULINK and the Real-Time Workshop. + * + * When compiling stand-alone model code, data types can be overridden + * via compiler switches. + * + * Define NO_FLOATS to eliminate reference to real_T, etc. + */ + +#ifdef MW_LIBTOOLING +#include "mwstdint.h" +#endif + +#include + +/* __STDC_VERSION__ version check below means "check for a C99 compiler". + + Visual Studio (checked on versions 2015 and 2017) does + not define __STDC_VERSION__, however it has stdbool.h available, + thus a separate check for _MSC_VER below. + */ +#if defined(__APPLE_CC__) \ + || (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)) \ + || (defined(_MSC_VER) && (_MSC_VER >= 1900)) +#ifndef tmwtypes_do_not_include_stdbool +#include +#endif +#endif + +#define LOGICAL_IS_A_TYPE +#define SPARSE_GENERALIZATION + +#ifdef NO_FLOATS +# define double double_not_allowed +# define float float_not_allowed +#endif /*NO_FLOATS*/ + +#ifndef NO_FLOATS + +#ifndef __MWERKS__ +# ifdef __STDC__ +# include +# else +# ifndef FLT_MANT_DIG +# define FLT_MANT_DIG 24 +# endif +# ifndef DBL_MANT_DIG +# define DBL_MANT_DIG 53 +# endif +# endif +#endif + +#endif /*NO_FLOATS*/ + +/* + * The following data types cannot be overridden when building MEX files. + */ +#ifdef MATLAB_MEX_FILE +# undef CHARACTER_T +# undef INTEGER_T +# undef BOOLEAN_T +# undef REAL_T +# undef TIME_T +#endif + +/* + * The uchar_T, ushort_T and ulong_T types are needed for compilers which do + * not allow defines to be specified, at the command line, with spaces in them. + */ + +typedef unsigned char uchar_T; +typedef unsigned short ushort_T; +typedef unsigned long ulong_T; + +#if (defined(_MSC_VER) && _MSC_VER >= 1500) \ + || defined(__x86_64__) || defined(__LP64__) \ + || defined(__LCC64__) + +typedef unsigned long long ulonglong_T; +#endif + + + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +/* When used with Real Time Workshop generated code, this + * header file can be used with a variety of compilers. + * + * The compiler could be for an 8 bit embedded processor that + * only had 8 bits per integer and 16 bits per long. + * In that example, a 32 bit integer size is not even available. + * This header file should be robust to that. + * + * For the case of an 8 bit processor, the preprocessor + * may be limited to 16 bit math like its target. That limitation + * would mean that 32 bit comparisons can't be done accurately. + * To increase robustness to this, comparisons are done against + * smaller values first. An inaccurate 32 bit comparison isn't + * attempted if the 16 bit comparison has already succeeded. + * + * Limitations on preprocessor math can also be stricter than + * for the target. There are known cases where a compiler + * targeting processors with 64 bit longs can't do accurate + * preprocessor comparisons on more than 32 bits. + */ + +/* Determine the number of bits for int, long, short, and char. + * If one fails to be determined, set the number of bits to -1 + */ + +#ifndef TMW_BITS_PER_INT +# if INT_MAX == 0x7FL +# define TMW_BITS_PER_INT 8 +# elif INT_MAX == 0x7FFFL +# define TMW_BITS_PER_INT 16 +# elif INT_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_INT 32 +# else +# define TMW_BITS_PER_INT -1 +# endif +#endif + +#ifndef TMW_BITS_PER_LONG +# if LONG_MAX == 0x7FL +# define TMW_BITS_PER_LONG 8 +# elif LONG_MAX == 0x7FFFL +# define TMW_BITS_PER_LONG 16 +# elif LONG_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_LONG 32 +# else +# define TMW_BITS_PER_LONG -1 +# endif +#endif + +#ifndef TMW_BITS_PER_SHRT +# if SHRT_MAX == 0x7FL +# define TMW_BITS_PER_SHRT 8 +# elif SHRT_MAX == 0x7FFFL +# define TMW_BITS_PER_SHRT 16 +# elif SHRT_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_SHRT 32 +# else +# define TMW_BITS_PER_SHRT -1 +# endif +#endif + +#ifndef TMW_BITS_PER_SCHAR +# if SCHAR_MAX == 0x7FL +# define TMW_BITS_PER_SCHAR 8 +# elif SCHAR_MAX == 0x7FFFL +# define TMW_BITS_PER_SCHAR 16 +# elif SCHAR_MAX == 0x7FFFFFFFL +# define TMW_BITS_PER_SCHAR 32 +# else +# define TMW_BITS_PER_SCHAR -1 +# endif +#endif + +#ifndef TMW_CHAR_SIGNED +# if SCHAR_MAX == CHAR_MAX +# define TMW_CHAR_SIGNED 1 +# else +# define TMW_CHAR_SIGNED 0 +# endif +#endif + +/* It is common for one or more of the integer types + * to be the same size. For example, on many embedded + * processors, both shorts and ints are 16 bits. On + * processors used for workstations, it is quite common + * for both int and long to be 32 bits. + * When there is more than one choice for typdef'ing + * a portable type like int16_T or uint32_T, in + * concept, it should not matter which choice is made. + * However, some style guides and some code checking + * tools do identify and complain about seemingly + * irrelevant differences. For example, a code + * checking tool may complain about an implicit + * conversion from int to short even though both + * are 16 bits. To reduce these types of + * complaints, it is best to make int the + * preferred choice when more than one is available. + */ + +#ifndef INT8_T +# if defined(MW_LIBTOOLING) +# define INT8_T int8_t +# elif TMW_BITS_PER_INT == 8 +# define INT8_T int +# elif TMW_BITS_PER_LONG == 8 +# define INT8_T long +# elif TMW_BITS_PER_SCHAR == 8 +# define INT8_T signed char +# elif TMW_BITS_PER_SHRT == 8 +# define INT8_T short +# endif +#endif +#ifdef INT8_T + typedef INT8_T int8_T; +#endif + +#ifndef UINT8_T +# if defined(MW_LIBTOOLING) +# define UINT8_T uint8_t +# elif TMW_BITS_PER_INT == 8 +# define UINT8_T unsigned int +# elif TMW_BITS_PER_LONG == 8 +# define UINT8_T unsigned long +# elif TMW_BITS_PER_SCHAR == 8 +# define UINT8_T unsigned char +# elif TMW_BITS_PER_SHRT == 8 +# define UINT8_T unsigned short +# endif +#endif +#ifdef UINT8_T + typedef UINT8_T uint8_T; +#endif + + +#ifndef INT16_T +# if defined(MW_LIBTOOLING) +# define INT16_T int16_t +# elif TMW_BITS_PER_INT == 16 +# define INT16_T int +# elif TMW_BITS_PER_LONG == 16 +# define INT16_T long +# elif TMW_BITS_PER_SCHAR == 16 +# define INT16_T signed char +# elif TMW_BITS_PER_SHRT == 16 +# define INT16_T short +# endif +#endif +#ifdef INT16_T + typedef INT16_T int16_T; +#endif + + +#ifndef UINT16_T +# if defined(MW_LIBTOOLING) +# define UINT16_T uint16_t +# elif TMW_BITS_PER_INT == 16 +# define UINT16_T unsigned int +# elif TMW_BITS_PER_LONG == 16 +# define UINT16_T unsigned long +# elif TMW_BITS_PER_SCHAR == 16 +# define UINT16_T unsigned char +# elif TMW_BITS_PER_SHRT == 16 +# define UINT16_T unsigned short +# endif +#endif +#ifdef UINT16_T + typedef UINT16_T uint16_T; +#endif + + +#ifndef INT32_T +# if defined(MW_LIBTOOLING) +# define INT32_T int32_t +# elif TMW_BITS_PER_INT == 32 +# define INT32_T int +# elif TMW_BITS_PER_LONG == 32 +# define INT32_T long +# elif TMW_BITS_PER_SCHAR == 32 +# define INT32_T signed char +# elif TMW_BITS_PER_SHRT == 32 +# define INT32_T short +# endif +#endif +#ifdef INT32_T + typedef INT32_T int32_T; +#endif + + +#ifndef UINT32_T +# if defined(MW_LIBTOOLING) +# define UINT32_T uint32_t +# elif TMW_BITS_PER_INT == 32 +# define UINT32_T unsigned int +# elif TMW_BITS_PER_LONG == 32 +# define UINT32_T unsigned long +# elif TMW_BITS_PER_SCHAR == 32 +# define UINT32_T unsigned char +# elif TMW_BITS_PER_SHRT == 32 +# define UINT32_T unsigned short +# endif +#endif +#ifdef UINT32_T + typedef UINT32_T uint32_T; +#endif + +/* The following is used to emulate smaller integer types when only + * larger types are available. For example, compilers for TI C3x/C4x DSPs + * define char and short to be 32 bits, so 8 and 16 bits are not directly + * available. This target is commonly used with RTW rapid prototyping. + * Other DSPs define char to be 16 bits, so 8 bits is not directly + * available. + */ +#ifndef INT8_T +# ifdef INT16_T +# define INT8_T INT16_T + typedef INT8_T int8_T; +# else +# ifdef INT32_T +# define INT8_T INT32_T + typedef INT8_T int8_T; +# endif +# endif +#endif + +#ifndef UINT8_T +# ifdef UINT16_T +# define UINT8_T UINT16_T + typedef UINT8_T uint8_T; +# else +# ifdef UINT32_T +# define UINT8_T UINT32_T + typedef UINT8_T uint8_T; +# endif +# endif +#endif + +#ifndef INT16_T +# ifdef INT32_T +# define INT16_T INT32_T + typedef INT16_T int16_T; +# endif +#endif + +#ifndef UINT16_T +# ifdef UINT32_T +# define UINT16_T UINT32_T + typedef UINT16_T uint16_T; +# endif +#endif + + +#ifndef NO_FLOATS + +#ifndef REAL32_T +# ifndef __MWERKS__ +# if FLT_MANT_DIG >= 23 +# define REAL32_T float +# endif +# else +# define REAL32_T float +# endif +#endif +#ifdef REAL32_T + typedef REAL32_T real32_T; +#endif + + +#ifndef REAL64_T +# ifndef __MWERKS__ +# if DBL_MANT_DIG >= 52 +# define REAL64_T double +# endif +# else +# define REAL64_T double +# endif +#endif +#ifdef REAL64_T + typedef REAL64_T real64_T; +#endif + +#endif /* NO_FLOATS*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int64_T - signed 64 bit integers * + * uint64_T - unsigned 64 bit integers * + *=======================================================================*/ + +# if defined(MW_LIBTOOLING) +# ifdef INT64_T +# undef INT64_T +# endif +# define INT64_T int64_t +# ifdef UINT64_T +# undef UINT64_T +# endif +# define UINT64_T uint64_t +# endif +#if !defined(INT64_T) || !defined(UINT64_T) || !defined(FMT64) +# if defined(__APPLE__) || defined(__clang__) +# ifndef INT64_T +# define INT64_T long long +# endif +# ifndef UINT64_T +# define UINT64_T unsigned long long +# endif +# ifndef FMT64 +# define FMT64 "ll" +# endif +# if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG) +# define INT_TYPE_64_IS_LONG +# endif +# elif (defined(__x86_64__) || defined(__LP64__))&& !defined(__MINGW64__) +# ifndef INT64_T +# define INT64_T long +# endif +# ifndef UINT64_T +# define UINT64_T unsigned long +# endif +# ifndef FMT64 +# define FMT64 "l" +# endif +# if !defined(INT_TYPE_64_IS_LONG) +# define INT_TYPE_64_IS_LONG +# endif +# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \ + || (defined(__WATCOMC__) && __WATCOMC__ >= 1100) +# ifndef INT64_T +# define INT64_T __int64 +# endif +# ifndef UINT64_T +# define UINT64_T unsigned __int64 +# endif +# ifndef FMT64 +# define FMT64 "I64" +# endif +# elif defined(__GNUC__) || defined(TMW_ENABLE_INT64) \ + || defined(__LCC64__) +# ifndef INT64_T +# define INT64_T long long +# endif +# ifndef UINT64_T +# define UINT64_T unsigned long long +# endif +# ifndef FMT64 +# define FMT64 "ll" +# endif +# endif + +#endif + +#if defined(INT64_T) +# if defined(__GNUC__) && \ + ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9))) + __extension__ +# endif + typedef INT64_T int64_T; +#endif + +#if defined(_WIN64) || (defined(__APPLE__) && defined(__LP64__)) \ + || defined(__x86_64__) \ + || defined(__LP64__) +# define INT_TYPE_64_IS_SUPPORTED +#endif + +#if defined(UINT64_T) +# if defined(__GNUC__) && \ + ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9))) + __extension__ +# endif + typedef UINT64_T uint64_T; +#endif + +/*===========================================================================* + * Format string modifiers for using size_t variables in printf statements. * + *===========================================================================*/ + +#ifndef FMT_SIZE_T +# if (defined( __GNUC__ ) || defined(_STDC_C99))&& !defined(__MINGW64__) +# define FMT_SIZE_T "z" +# elif defined (__WATCOMC__) +# define FMT_SIZE_T "l" +# elif defined (_WIN32 ) +# define FMT_SIZE_T "I" +# else +# define FMT_SIZE_T "l" +# endif +#endif + +#ifndef FMT_PTRDIFF_T +# if defined(__APPLE__) +# define FMT_PTRDIFF_T "l" +# elif defined( __GNUC__ ) || defined(_STDC_C99) +# define FMT_PTRDIFF_T "t" +# elif defined (__WATCOMC__) +# define FMT_PTRDIFF_T "l" +# elif defined (_WIN32 ) +# define FMT_PTRDIFF_T "I" +# else +# define FMT_PTRDIFF_T "l" +# endif +#endif + +/*===========================================================================* + * General or logical data types where the word size is not guaranteed. * + * real_T - possible settings include real32_T or real64_T * + * time_T - possible settings include real32_T or real64_T * + * boolean_T * + * char_T * + * int_T * + * uint_T * + * byte_T * + *===========================================================================*/ + +#ifndef NO_FLOATS + +#ifndef REAL_T +# ifdef REAL64_T +# define REAL_T real64_T +# else +# ifdef REAL32_T +# define REAL_T real32_T +# endif +# endif +#endif +#ifdef REAL_T + typedef REAL_T real_T; +#endif + +#ifndef TIME_T +# ifdef REAL_T +# define TIME_T real_T +# endif +#endif +#ifdef TIME_T + typedef TIME_T time_T; +#endif + +#endif /* NO_FLOATS */ + +#ifndef BOOLEAN_T +# if defined(UINT8_T) +# define BOOLEAN_T UINT8_T +# else +# define BOOLEAN_T unsigned int +# endif +#endif +typedef BOOLEAN_T boolean_T; + + +#ifndef CHARACTER_T +# define CHARACTER_T char +#endif +typedef CHARACTER_T char_T; + + +#ifndef INTEGER_T +# define INTEGER_T int +#endif +typedef INTEGER_T int_T; + + +#ifndef UINTEGER_T +# define UINTEGER_T unsigned +#endif +typedef UINTEGER_T uint_T; + + +#ifndef BYTE_T +# define BYTE_T unsigned char +#endif +typedef BYTE_T byte_T; + + +/*===========================================================================* + * Define Complex Structures * + *===========================================================================*/ +#ifndef NO_FLOATS + +#ifndef CREAL32_T +# ifdef REAL32_T + typedef struct { + real32_T re, im; + } creal32_T; +# define CREAL32_T creal32_T +# endif +#endif + +#ifndef CREAL64_T +# ifdef REAL64_T + typedef struct { + real64_T re, im; + } creal64_T; +# define CREAL64_T creal64_T +# endif +#endif + +#ifndef CREAL_T +# ifdef REAL_T + typedef struct { + real_T re, im; + } creal_T; +# define CREAL_T creal_T +# endif +#endif + +#endif /* NO_FLOATS */ + +#ifndef CINT8_T +# ifdef INT8_T + typedef struct { + int8_T re, im; + } cint8_T; +# define CINT8_T cint8_T +# endif +#endif + +#ifndef CUINT8_T +# ifdef UINT8_T + typedef struct { + uint8_T re, im; + } cuint8_T; +# define CUINT8_T cuint8_T +# endif +#endif + +#ifndef CINT16_T +# ifdef INT16_T + typedef struct { + int16_T re, im; + } cint16_T; +# define CINT16_T cint16_T +# endif +#endif + +#ifndef CUINT16_T +# ifdef UINT16_T + typedef struct { + uint16_T re, im; + } cuint16_T; +# define CUINT16_T cuint16_T +# endif +#endif + +#ifndef CINT32_T +# ifdef INT32_T + typedef struct { + int32_T re, im; + } cint32_T; +# define CINT32_T cint32_T +# endif +#endif + +#ifndef CUINT32_T +# ifdef UINT32_T + typedef struct { + uint32_T re, im; + } cuint32_T; +# define CUINT32_T cuint32_T +# endif +#endif + +#ifndef CINT64_T +# ifdef INT64_T + typedef struct { + int64_T re, im; + } cint64_T; +# define CINT64_T cint64_T +# endif +#endif + +#ifndef CUINT64_T +# ifdef UINT64_T + typedef struct { + uint64_T re, im; + } cuint64_T; +# define CUINT64_T cuint64_T +# endif +#endif + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) /* 127 */ +#define MIN_int8_T ((int8_T)(-128)) /* -128 */ +#define MAX_uint8_T ((uint8_T)(255)) /* 255 */ +#define MIN_uint8_T ((uint8_T)(0)) + +#define MAX_int16_T ((int16_T)(32767)) /* 32767 */ +#define MIN_int16_T ((int16_T)(-32768)) /* -32768 */ +#define MAX_uint16_T ((uint16_T)(65535)) /* 65535 */ +#define MIN_uint16_T ((uint16_T)(0)) + +#define MAX_int32_T ((int32_T)(2147483647)) /* 2147483647 */ +#define MIN_int32_T ((int32_T)(-2147483647-1)) /* -2147483648 */ +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) /* 4294967295 */ +#define MIN_uint32_T ((uint32_T)(0)) + +#if defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \ + || (defined(__WATCOMC__) && __WATCOMC__ >= 1100) \ + || defined(__LCC64__) +# ifdef INT64_T +# define MAX_int64_T ((int64_T)(9223372036854775807LL)) +# define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) +# endif +# ifdef UINT64_T +# define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) +# define MIN_uint64_T ((uint64_T)(0)) +# endif +#else +# ifdef INT64_T +# ifdef INT_TYPE_64_IS_LONG +# define MAX_int64_T ((int64_T)(9223372036854775807L)) +# define MIN_int64_T ((int64_T)(-9223372036854775807L-1L)) +# else +# define MAX_int64_T ((int64_T)(9223372036854775807LL)) +# define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) +# endif +# endif +# ifdef UINT64_T +# ifdef INT_TYPE_64_IS_LONG +# define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFUL)) +# define MIN_uint64_T ((uint64_T)(0)) +# else +# define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) +# define MIN_uint64_T ((uint64_T)(0)) +# endif +# endif +#endif + +#if (defined(_MSC_VER) && !defined(__clang__)) + +/* Conversion from unsigned __int64 to double is not implemented in Visual Studio + * and results in a compile error, thus the value must first be cast to + * signed __int64, and then to double. + * + * If the 64 bit int value is greater than 2^63-1, which is the signed int64 max, + * the macro below provides a workaround for casting a uint64 value to a double + * in windows. + */ +# define uint64_to_double(u) ( ((u) > _I64_MAX) ? \ + (double)(__int64)((u) - _I64_MAX - 1) + (double)_I64_MAX + 1: \ + (double)(__int64)(u) ) + +/* The following inline function should only be used in the macro double_to_uint64, + * as it only handles the specfic range of double between 2^63 and 2^64-1 */ +__forceinline +uint64_T double_to_uint64_helper(double d) { + union double_to_uint64_union_type { + double dd; + uint64_T i64; + } di; + di.dd = d; + return (((di.i64 & 0x000fffffffffffff) | 0x0010000000000000) << 11); +} + +/* The largest double value that can be cast to uint64 in windows is the + * signed int64 max, which is 2^63-1. The macro below provides + * a workaround for casting large double values to uint64 in windows. + */ +/* The magic number 18446744073709551616.0 is 2^64 */ +/* The magic number 9223372036854775808.0 is 2^63 */ +# define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \ + 0xffffffffffffffffULL : \ + ((d) >= 0.0) ? \ + ((d) >= 9223372036854775808.0) ? \ + double_to_uint64_helper(d) : \ + (unsigned __int64)(d) : \ + 0ULL ) +#else +# define uint64_to_double(u) ((double)(u)) +# if defined(__BORLANDC__) || defined(__WATCOMC__) || defined(__TICCSC__) +/* double_to_uint64 defined only for MSVC and UNIX */ +# else +# define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \ + (unsigned long long) 0xffffffffffffffffULL : \ + ((d) >= 0) ? (unsigned long long)(d) : (unsigned long long) 0 ) +# endif +#endif + +#if !defined(__cplusplus) && !defined(__bool_true_false_are_defined) + +#ifndef _bool_T +#define _bool_T + +typedef boolean_T bool; + +#ifndef false +#define false (0) +#endif +#ifndef true +#define true (1) +#endif + +#endif /* _bool_T */ + +#endif /* !__cplusplus */ + +/* + * This software assumes that the code is being compiled on a target using a + * 2's complement representation for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable/model) + * including the null-termination character. + */ +#define TMW_NAME_LENGTH_MAX 2049 + +/* + * Maximum values for indices and dimensions + */ +#include + +#ifdef MX_COMPAT_32 +typedef int mwSize; +typedef int mwIndex; +typedef int mwSignedIndex; +#else +typedef size_t mwSize; /* unsigned pointer-width integer */ +typedef size_t mwIndex; /* unsigned pointer-width integer */ +typedef ptrdiff_t mwSignedIndex; /* a signed pointer-width integer */ +#endif + + /* for the individual dim */ +/* If updating SLSize or SLIndex, update defintions in sl_types_def.h + as well. */ +#ifndef SLSIZE_SLINDEX + #define SLSIZE_SLINDEX + #ifdef INT_TYPE_64_IS_SUPPORTED + typedef int64_T SLIndex; + typedef int64_T SLSize; + #else + typedef int SLIndex; + typedef int SLSize; + #endif +#endif + +/* for the total size */ +#define SLIndexType size_t +#define INVALID_SIZET_VALUE (std::numeric_limits::max()) +#define MAX_VALID_SIZET_VALUE (std::numeric_limits::max() -1) + + +#if (defined(_LP64) || defined(_WIN64)) && !defined(MX_COMPAT_32) +/* Currently 2^48 based on hardware limitations */ +# define MWSIZE_MAX 281474976710655UL +# define MWINDEX_MAX 281474976710655UL +# define MWSINDEX_MAX 281474976710655L +# define MWSINDEX_MIN -281474976710655L +#else +# define MWSIZE_MAX 2147483647UL +# define MWINDEX_MAX 2147483647UL +# define MWSINDEX_MAX 2147483647L +# define MWSINDEX_MIN -2147483647L +#endif +#define MWSIZE_MIN 0UL +#define MWINDEX_MIN 0UL + +/** UTF-16 character type */ + +#if (defined(__cplusplus) && (__cplusplus >= 201103L)) || (defined(_HAS_CHAR16_T_LANGUAGE_SUPPORT) && _HAS_CHAR16_T_LANGUAGE_SUPPORT) +typedef char16_t CHAR16_T; +#define U16_STRING_LITERAL_PREFIX u +#elif defined(_MSC_VER) +typedef wchar_t CHAR16_T; +#define U16_STRING_LITERAL_PREFIX L +#else +typedef UINT16_T CHAR16_T; +#endif + +#endif /* __TMWTYPES__ */ + +#endif /* tmwtypes_h */ diff --git a/aerpaw/basic_demo/uav.py b/aerpaw/basic_demo/uav.py index d88f923..3dafa56 100755 --- a/aerpaw/basic_demo/uav.py +++ b/aerpaw/basic_demo/uav.py @@ -1,13 +1,97 @@ -#!/bin/python3 +#!/usr/bin/env python3 +""" +UAV Client for AERPAW Target Location Protocol + +Protocol: + Server sends: TARGET:x,y,z + Client sends: ACK:TARGET + Client (after moving): READY +""" + import socket +import sys +import time SERVER_IP = "127.0.0.1" SERVER_PORT = 5000 -for client_id in range(2): # match numClients + +def parse_target(message: str) -> tuple: + """Parse TARGET:x,y,z message and return (x, y, z) 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 move_to_target(x: float, y: float, z: float): + """ + Placeholder for AERPAW API integration. + In the future, this will command the UAV to move to (x, y, z). + """ + print(f" [PLACEHOLDER] Moving to target: ({x}, {y}, {z})") + # Simulate movement time + time.sleep(1.0) + print(f" [PLACEHOLDER] Arrived at target: ({x}, {y}, {z})") + + +def run_uav_client(client_id: int): + """Run a single UAV client.""" + print(f"UAV {client_id}: Connecting to {SERVER_IP}:{SERVER_PORT}") + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((SERVER_IP, SERVER_PORT)) - data = s.recv(1024) # receive message from server - print(f"Client {client_id+1} received: {data.decode()}") - s.sendall(b"ACK") # send acknowledgment - s.close() + + try: + # Receive TARGET command + data = s.recv(1024).decode().strip() + print(f"UAV {client_id}: Received: {data}") + + # Parse target coordinates + x, y, z = parse_target(data) + print(f"UAV {client_id}: Parsed target: x={x}, y={y}, z={z}") + + # Send acknowledgment + s.sendall(b"ACK:TARGET") + print(f"UAV {client_id}: Sent ACK:TARGET") + + # Move to target (placeholder) + move_to_target(x, y, z) + + # 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() + print(f"UAV {client_id}: Connection closed") + + +if __name__ == "__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]) + run_uav_client(client_id) diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj index 5aa1d80..1a4fe3e 100644 --- a/aerpaw/controller.coderprj +++ b/aerpaw/controller.coderprj @@ -4,7 +4,7 @@ - + @@ -38,6 +38,11 @@ + + int32 + + + @@ -63,89 +68,71 @@ /home/kdee/Desktop/miSim/aerpaw/codegen - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode/comms.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode/controller_impl.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode/controller_impl.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h + /home/kdee/matlab/R2025a/extern/include/tmwtypes.h GENERATED_SOURCE - - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h - - GENERATED_SOURCE - - - - /home/kdee/matlab/R2025a/extern/include/tmwtypes.h - - GENERATED_SOURCE - - /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp GENERATED_SOURCE - + /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.h @@ -153,7 +140,7 @@ true - 2026-01-28T23:10:50 + 2026-01-29T16:33:06 @@ -176,36 +163,21 @@ - customInclude - - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode - - - - - customSource - - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode/comms.cpp - /home/kdee/Desktop/miSim/aerpaw/basic_demo/handcode/controller_impl.cpp - - - - genCodeOnly - + true - + generateReport - + - + targetLang - + C++ diff --git a/aerpaw/controller.m b/aerpaw/controller.m index cfb67d5..3ff3615 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -3,17 +3,37 @@ arguments (Input) numClients (1, 1) int32; end -coder.extrinsic('disp'); +coder.extrinsic('disp', 'readmatrix'); + +% Maximum clients supported +MAX_CLIENTS = 4; + +% Allocate targets array (MAX_CLIENTS x 3) +targets = zeros(MAX_CLIENTS, 3); + +% Load targets from file +if coder.target('MATLAB') + disp('Loading targets from file (simulation)...'); + targetsLoaded = readmatrix('targets.txt'); + numTargets = min(size(targetsLoaded, 1), numClients); + targets(1:numTargets, :) = targetsLoaded(1:numTargets, :); + disp(['Loaded ', num2str(numTargets), ' targets']); +else + coder.cinclude('controller_impl.h'); + % Define filename as null-terminated character array for C compatibility + filename = ['targets.txt', char(0)]; + % loadTargets fills targets array (row-major: x1,y1,z1,x2,y2,z2,...) + coder.ceval('loadTargets', coder.ref(filename), ... + coder.ref(targets), int32(MAX_CLIENTS)); +end % Initialize server if coder.target('MATLAB') disp('Initializing server (simulation)...'); else - coder.cinclude('controller_impl.h'); coder.ceval('initServer'); end - % Accept clients for i = 1:numClients if coder.target('MATLAB') @@ -23,33 +43,60 @@ for i = 1:numClients end end - -% Send messages to clients +% Send target coordinates to each client for i = 1:numClients + % Get target for this client (1x3 array) + target = targets(i, :); + if coder.target('MATLAB') - disp(['Sending message to client ', num2str(i)]); + disp(['Sending TARGET to client ', num2str(i), ': ', ... + num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]); else - coder.ceval('sendMessage', int32(i)); + coder.ceval('sendTarget', int32(i), coder.ref(target)); end end -% Receive acknowledgements -acksReceived = zeros(1, numClients, 'int32'); +% Receive TARGET acknowledgments +targetAcks = zeros(1, numClients, 'int32'); for i = 1:numClients if coder.target('MATLAB') - disp(['Receiving ACK from client ', num2str(i)]); - acksReceived(i) = 1; % Simulate successful ACK + disp(['Waiting for ACK:TARGET from client ', num2str(i)]); + targetAcks(i) = 1; % Simulate successful ACK else - acksReceived(i) = coder.ceval('receiveAck', int32(i)); + targetAcks(i) = coder.ceval('receiveTargetAck', int32(i)); end end - -% Digest ACKs +% Check all ACKs received if coder.target('MATLAB') - disp(['All ACKs received: ', num2str(acksReceived)]); + disp(['Target ACKs received: ', num2str(targetAcks)]); end +% Wait for READY signals (UAVs have reached their targets) +readySignals = zeros(1, numClients, 'int32'); +for i = 1:numClients + if coder.target('MATLAB') + disp(['Waiting for READY from client ', num2str(i)]); + readySignals(i) = 1; % Simulate READY + else + readySignals(i) = coder.ceval('waitForReady', int32(i)); + end +end + +% Check all READY signals received +if coder.target('MATLAB') + disp(['Ready signals received: ', num2str(readySignals)]); + disp('All UAVs at target positions.'); +end + +% Send COMPLETE to all clients before closing +for i = 1:numClients + if coder.target('MATLAB') + disp(['Sending COMPLETE to client ', num2str(i)]); + else + coder.ceval('sendFinished', int32(i)); + end +end % Close server if ~coder.target('MATLAB')