From d60628e14d347720b12fa800cca6352cfdb99be8 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 18 Feb 2025 10:33:01 +0300 Subject: [PATCH] Support MAVLink console Implement receiving and sending SERIAL_CONTROL message Use global defined print function instead of Serial.printf --- docs/build.md | 6 +++++ docs/img/cli.png | Bin 0 -> 28330 bytes docs/troubleshooting.md | 2 +- flix/cli.ino | 56 ++++++++++++++++++++++++++-------------- flix/flix.ino | 4 +-- flix/imu.ino | 24 ++++++++--------- flix/log.ino | 4 +-- flix/mavlink.ino | 21 +++++++++++++++ flix/motors.ino | 8 +++--- flix/parameters.ino | 2 +- flix/rc.ino | 18 ++++++------- flix/wifi.ino | 3 ++- gazebo/flix.h | 10 ++++--- 13 files changed, 103 insertions(+), 55 deletions(-) create mode 100644 docs/img/cli.png diff --git a/docs/build.md b/docs/build.md index 5893a85..4d49aaa 100644 --- a/docs/build.md +++ b/docs/build.md @@ -191,6 +191,12 @@ You can adjust some of the drone's parameters (include PID coefficients) in QGro +#### CLI access + +In addition to accessing the drone's command line interface (CLI) using the serial port, you can also access it with QGroundControl using Wi-Fi connection. To do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*. + + + > [!NOTE] > If something goes wrong, go to the [Troubleshooting](troubleshooting.md) article. diff --git a/docs/img/cli.png b/docs/img/cli.png new file mode 100644 index 0000000000000000000000000000000000000000..04190d189f255f6e768add61f8873aef981aa7f6 GIT binary patch literal 28330 zcmZ^JWmFtZ(C#9E0KwgZI|K;so&mc;sp;l004mg^^3F`0Du4m0N^IyBfRAZ6VA@P2}ElN zMF{|)J|6AK^xfM%oST}QB%o%J?C>oesG^`D!_Cc2LqkJLOZ)Qj@>byU=g(9hsi>)` zU*C?4UtC;VoSdALl$4B&jP&&M0s;aeA|fy6zZe)8NJ&Y_$;p?OmlqZm+}zyw`1qWi zoq2e8*x1-O*y%YqIE00TA0HpZ#Kd@b`FMGGg@lBj|0S52IU^vvV`XK1y-w`w>wA8F zmXMH8S69Ekzkj_=etk^M&CQjTmZ`3;zB`W9)HD$j6NkZI=jZ3w*Vo^^sp{!j`TF_- z07S;d_RP%8O7dKh!OGm+pJipgg@uKwsOT{=(Ej=JXMBA8;NYOGtqm0wb!TVi^z?Li zcvwlX*Ik!)j0`7oy1IkBIO+Cz%j(t`Y^`w1y(CWFq_BfBxa)PhWPsJ z!==15p(Q1sq7(TGZ19qbx*T~oEs!f~@>91s@OOKUzv1ZMC-d%tpH=~f4c9bn*aj~d zMEf~1i=obnYgYvgjE!|h+@A3!#-b(dLFt1MIi~CCz=>%zyvcSo*~$wXlpXwVoZeJc zElM4VYAs81OuEpSYuO47%lsZBc4Kj*{hw0J;mB#Zx3WIt!4 ztx-DEs$`wsLTH%Glzq$C6UQ=elfP6kLbJ4@A8{I1uMd6Zip=eF{8{35Gx^Dh4OEA_ z)pq&aw{~^5ec7F9)p^^A%E$E~G}q(H6=&DG67`(PWug9#@;BAG9SDl;PjcKFTa#PT z)M~oop-gMcvz>6Lf^Q5v(StrByUZjsEX}^oLQSN5e~|r|SeF7q&=qUZvAlECBsxxh zPq%_&0v8R&(Uf_AY{s*9)E~3LdG&Vh^*NjR$~3-u;8Q(}sD9*8W1^ zpCX3KM(x5<{s!H>a3if-BQ7@l0gBPGPlfMqv!REk8@)CRRkIMnBjAMMOL7lM!o8hL zU{-cN&PsIdEnGAv>pmYYkmf-S+~zK042O@y<|pR({MFmqF~-~mIqvM44SYj) z$WWen`4b62`|m>zMDxee53@2p&P0c+0eeDy!u^B{D1HsiV!@LK@EIFlfQsMib@?}f z;tB_Onj5Qw<3sWfYc_`JJ3tk4v0@(L>EZ^Dj|Gc^aLa>0o?|yZIK`oT6P(uo?dA7E zohKu1cz>q?WFEE$zkgm?LZgri==kF!&cBo(kj_h~H-1I~a%RlttHhh+lL&X7B83YW zg-aEudqlzUd^tM&v~n1qHoDwOjIz^KWpsJAey(%XRr&3*9nE0Qc1el;IYvr!veNZj zVpUw2k)#hD$8RQOk+IWDU~*09r$CHXFQ7z9K-x(7;oq{=#|M1-$MYBBee)>x<1cj$ zhO)jwjI`irNBEWjim!>pm6qirLQ{`ug!=mWFw#Jd&r;CYVMmn$vVIAvyrpdyOZmNK zdEa!ynU^nDt&x@CX;oU5&n&qeZON&Tzu7`yhr!P}pAp9t1sgxFMr75%W+^nKof4q; z?JV~5ooB4RQ3U%8N;6GXiF$_@mW|8Dv6h8$ue^l6aR@gQXNGJVUx)24jeruq3aM;b zIrp0d$Xice!sfku2a4Bc1g>1MxaR0Our`mebSP9U(f)a79tsw6nZ9sDb3=xTntGTgoT^LFV^lfR5w5rGFt#H1w%Q0KCPl@$Zn}dJ*N&G(gny#3k_K(cE9VTXh zgW078DDxaArTMrWzm^`X30F@3umJx|&91R`3J6nYH_vgs=2%L3jG*PL7;&+MPDP%c zB*K&KO0rU{UJazR_nvPqlD7~A?yH~~I2e1kXqnNd z#Kjci{qbZ;up{_(@p+9X3NmwMK@nWWvKzS9n%QOt6bYx9IQjJUMn$efot`e?zF^YG zrM`S^6YE%t#0tQtHOo!6Rsnp72H%#1tu-n1l9_TJA^KDy>tsjKp5dBSDJp=v`?*OT z*+n>5^Q+vm;n3GeBca|E%HbvzrTCS$bwFmhX>CUJ4{9(E@&?A_evLaWNckwhfza}1 zwW`XW5u`%2|IB1FrWN_*Rxta%cK5z~yh8oNXT!$PlpChmK4HgYxY(qeMqay6o?So| z(G~GL=|A1;S-+wyQ;b{mp{>&Gj@{80@_ok^^X1qe*EC`Eu>pmTErx#rvD~9n%ib`T zifPWu->%svv;c-eR+%2<`;xSEvY7|6t+N4du7=O^3y6DK<#Y3_ za>ZZ=#O`%ydKa@NU=|k3(ePrp&>*1flgCKeo5?`BTp4C>K!Xz{notc~!ShFA6A&-v zq3|3eXW}bkUoIPYP_^z_Ewj<1#u8!roRX8bSe&L#$yadklfM42qy>%-Pb7Gj-EUY^ zUah#_v&u@e1R4h0xnZ<#R!#(|_L$bACk8ZnNt6o=ER@95P*d&8oShg|8DFP{Wp+yr zYLmFwt7X>CY=TegMKsX2=)oNNA!0pZ$Ik(q66T)O!5Geg;<72fa|#<;U_WpYU? z|7Ea?Qw7BfB!E*CACmur%Awi9SCZqNoz(8%te#x+qsjpd$=%^4OgVk$Hk_?3 zc*Lb1<|LGg(<$fn7hb7y9samg3E$fWJ@1RHVQ4`fXNL=C%x$V7DkBkg!(A?1)zdqkl7?n|h|1nnjH7tu#x^bw`OB7Kok{cz_Mf%=2{b1vh<>Xg zAl-)@Wq|JHIgmZH&Fx|mg5h%zMw{Q7(@OUq>ww`#HC>f^IQoPD!4d(AK8(BADOq{p z&vQug%ft91!>HiV;lVl4F22?7igR@P)G8pvmc4znwl*kNPPK!pNG?bhZY|Y5Q`jz^ zRQCyw^s6_#aXhLn5ZHOvo3z!Uo)MVRgk&Zpg0PHcA}Cii9Ekm}h=;kYiS?jV z%!@etgm}?!p5n$0d`WiB;hDdI(MG_*1knd*2{GkABRSby_R;IB0@1k3N2yP|@+ZUY zn>8W(uD)?j0mmTZGX5&@kNF25-A<>|zh<@Iv%x=b%mK_9Y1TNH81r`;$PtZ@lok+5 zpRs?yUD-@lcThK+douFQx-kkp)~XEd$yAISV97w#KOJd$6^jP#Y9&Yd3FSx23yN_B zOMsnGY$_o0=kQZBsLGO(LugOgw$lpDaKI?gR5tziO&*|plCI&e9JK!?%h#Fs+acWy z8d8tx(4Nxkzv!}dCff^rmWId44|uvwdRt$}4w&zU4=-O8FkihVHUqo}gj6U1o!FV16a(Bxm%as4kD5SI6@8l`N-;%Cj_2{Rwk zHIB;u7V7B5U$$V1*YZO=79C&Uo~&V(Yf`+@oWlO+@zYIdAIfO>wV)QPnzEsME5Z;T z!}T6Ef*AeO$#5+bid-A93HjKhq%q$LuZ@a&FV)VE;rTqlJs-`G+L~b?A^ChtB>zdUK0zIK4#Qu@rOAW*{j1vy z3KA(X2vS^hX)yCeU7id^CU$ThwZsY0oKHUHECxvN3rY0A@?vv{w+%G%chfi|i=y;? z|7Ed0hiPk6qF1uGdC!2ZIrrzblE3AvBs=%J)|H0_t~e$i z9(9aropHi)ERKmOntz7v!zY*tkvY+>7IZ4}y@9GL#~)C|3_P6%pz6}+q}iwFHiTG3 z@1d6O!cyov{;^DJlH<#vs4+2zV_qJGBa8XJ3Qn89^fD8LXHk0Jc8lBtc;pFzWreON zY1*xyY4QnoUEOOV9MSx{D^fpK)8F_7vRfqVB(MtsSe;a>L41LULP2ShLWeYnsG7>IzL=8r_> zM6}y|c6OIE`0i zwJM7EfFVJA7{J=sQ1Q$?O)SwS0?0J3|B1ChQ(yaYjBrhv;IOH#tl32MBNv>H*+w+@ zT(3k)!5a2}$Dt;5uF=|ZRuFhGUe1xs*SJ|awmLK$+lNo3Uad7|+L)fee3Y9$6h1y3 zttr!>wo8y)t^91qnl6Z>kgWUK z5zR9$-hJwBM8ThTu#T&E4^EqNm;5b{oADvBrWF=;9v-tR`{~2((?FLU`WcaWCkBX2 z>(Tuj`pU_Zv@34DW1d8cr3{*$h6M#2Bvigg1BGL6)_bXgiR2IeEPtyEJoMc`nQXCx=};?=8m^Ha>2MJ{Ga^>P9BY=(Mp=e zK_}n=o3671^NB2rxP^j(FpK7v5_z;bC|7Ec*I#=cC>XmL3+Pw-GxZO~Qy7i9COSpf z2e=;0J0PuFj~^EwA<+5f+{G7k#2W}rD=O>jRw=3&B|I)XB z^ojtH+)xvR2_V*V7Q_sydIyF~7n+*!df|2cgD-E*_%i z%TYY{!C}!}#95*Q{yO`pqTD*1{BzduU4^imlOSWh(s=@uqt}8I8&275-IZK^wXm}i~xIjV?|aC9ncy#@TtS#Z-03q8USI zqmIp0OfMB_B`oxg17LKsMNkfMvq)#t+w8xxuF| zXWhPOsp$EzTx-qfhtU_yGy5Mn+8};|O}tF%7Iaoer>_!JwuoQ<>aY@hL8NBHi}-a1 z$K3v9{M(1u-p7`1?g5DBGrjhYvw=t7wFybFPo#c6R+HHoU%@|jf7zzZVIWEteaNYA z9u6_chYj#e`7;iv#ZAYWncx);bZ*s)AL6uhf6MB+Cy6Ch~?%6tKiwKk}UXx zU<+;hK+?Zm-YCaBVU+8zM|0`h(32zFV-gOR+qw36f4y(?bT)ZWXNS)|2tYSq-x+#@ zCe}@3HqaEIb5l4DEE^^q$^~b)D7-)d;`|DV?!v?tNlnhB%TO2;7Avh4C_#olgT5-^ zoKTs=<%R^bL5(t;>J~OHxvK0KP$3LQ)uC7y**lWYjkxoNp?Bp!i#k@1{;Lq4~zc;R=g`Fx+=FL~_(2m>~2qfT83 zvLd__t!W8l7RqCj7fF*uhQsjgB65N{;Rri>ZNwQ#PMS}W+tKds;r#DOgqjGn{tUWRM z2#Y+>8{d${;Q)=rNtJPzWy12Unn6?-gVn|O)>@W5A?j+$doWzO99X7FZTLVzF#us; zd=IS+@eP$-hGXwiKn80=IzkI|Rq?-yH(x=cva~)P-`s1 zkv+8gME3VS9>?B=0} zrJwsA+|-!So~72D>hQNu8^H=R$$d`}eR+(btKBvmMQs-~=yc5uW}^osY@+y=3VdqI z#)K;)rfHm|!Yx4fga*!7_np-qEC*)4$i}7EX7PbjT8(k(tFII2AJvbakeU!ISyq8| zdfAko3`=5}RdQw3_dFhG=)q53Kd3>8n@(Dqx^b9yl?D5R*;0|@(m;R}4XCv$GijjU zZ)qd={@uf6!DHcP!S~u@MRs<-qaED?V4K-EvPrzNVoTnjiz;<`$?8)x27KEzKB=h3 zibmhEnpBQ5gScZ3t@yHoh?&;0Q@W-&UF^&xjaWm`?ml^C{sHb=vf@1sCy`55)k~}aYVzTk{2U$+% zT-$m4i!fVy8=;|Aj0(+o<|5l+s7EmuSR0n8(Y=A~r^%!6!0uNpbd$!oTIy6!0IoA` zFG7cMxSgXOR0y{+{XTZGMW0u65>iS33l4CObd1OeRG&n+%jeRw-iF&VzY zFM_4(IP;*nsB3t;Z}v$zeH`Fr_Zjy2{<+nB?y2+!-dIni20~!!OVYxukFT`8!i7F8 z3WGJu7%Y2{&LwTPt@)5HB06a!oi;1*$O#Mx~VN6-JZm85CU~Ggb3Z%bposQMlXT#gdFp@-)SRwm#X- z%=-KEXSo>vK70#s4^V-1=>tEPCf!ltJ7&u{!Ure{rwm~gM-5#obnarVKh|6n%6nKy zSs@u;A$0X~#Fyj2Wrc@q27+Ki6Vx!LI3;*l*10#JNg}7?e~c zTev_iW94^cUanPp_SfE0LKP|~^2gJ*&?8b%Eyd@`5LG?$c#s)Z=sBZslg^~V0%@_+ z@)ozIxao5=P$^5P(u6@gwFcWd>>lvY^bkI^y!YAdb$ViHI?edDr(ON%##0w>3n3iT zN6seQC|{{xjQ>)6cDq&+wH>FAM$^8W^ZtYtV)b$(|PW%b2Vjw`h->PAz zXNF%HFsPrRdwg1Ld)zLKdFuZUajWHVNrc9yq|s*r7ES0_Wi+-%6l1Pc#J%A@)bQ+G zWjpS?sgFuJsPray~#r(<(}= zlt6RhpE62o_rco`$ZJe$eNEg}P&z)+KQH2i4F=|p3m;5BDN~Y73va{^rMZvt<=ke| zI~1;odMV(6jFiRob%^bVNn71-;Lqzj)X;Ts1QdS=ph^+@s$hW=XX)d?kR4i>G3tq* z+bRzfaT(+JyL*`vwP%F?rGxHP#^_8qEU`Ov9Y^k)T;WItc5%B^ZYTphEvT%DZ~_1Iqg#lt zW{~Ak1R-iT4yNtkfefJ{Ea|sz$#15Kg4}Zs#g4{bBj019jaX{FDq0ZD;(|^rKC9)t zkAW^quD2|>TbCpa^WXH_p6h~T%ykS~Vn_>GI*ZSuSVv0+<3A5z$(YdihauvkGn$@d zE1*ZdkAqU~k9Azd-Sa6O=(H{>u^lfw-3?(G#*~K#LJcsdx}fN+4z`*Tb(KUL6hjf) zY@I@<2L)(h-eFfaLZOT$xDruN&u+D;1@~KN>-{gLd*2^!ps zsUq4bu(sN2+UmI~Bj_l)hM6eoVG5H}4UPFHa9f$$=j=3oojhhXsJ@7l65?-sI)nSH zwlZWF;M%EujI)$7XyPfY%t(~R>=k3ShN3%{9Fa@l=XgHw&`BSzfVWSG*2yZUj@#eU zhn=?A@wRwgM1tYc&=67pGkQ{*V#ZuPrNI|t@7q-gFb>4=Faoq`)6lDl#njJ*Dss4QB=O!A8pV>~IEjBms&cu*=irf_vyEA(y z4LEY0;JJ8``{G{P&3JeIseli+OIyY|s`sYYz%^U$n#HCzl49TQ>K&2LrZ$|FSwE~! zS<%PuN_1ULS)DQmdM*kwKJa!qKgjDeR6l#j4)~Re<)d@;%cmF{ZrpB66NGDZL^RPy zr03Ft3H(b>|Nr&^Op*-ro63;pM%Fs88=6!Ffc%yc5MPmvFdlaWh@?V1`Q)c%3RyQBQzXu@RU!<(v^1OJ zuUYdaE+4?-{M3Mg4c_+WoBLA{Hc;6{b{@-UzxS`h_8iUyNuQwQqNa2j9$IZ)pu78xmkyrm^y&%3mIBcewaQ#~hNc^7`pE;C~ zLq=xb{#FM|cjNdue@y@I-vf#tV5a{dBbBgdeR^bCK<-&GCz&_nN@W}M@6b~+slQxN zDgbql-`Ai~Hrtu5m!J>BHt1Pr$(Sj=CP2lKp${7K{}dK4H9hcWs((RN&b6Mwd=4`` zRqrqUOfX9nyowjB@>42e>IO%3LgDdB>~afGnFX|0r1~%~3X0tu?rxDmW%^HPj`aC{ ziQ9I+Lt=fpX0fr@&{ATDU06#%VGgD)GY?;|bbtvMcPS+mD=b*+ z=~DsVkUWAQiTv{qu*~wBL>J&w2|Cp-AnN0BzAZq|qJ%hI0b!+Sy@P0*!y8qDO$vo3 z6<~iZv>eE# z#wz2cFVn(PC49t^GPdUiJ$u|O2)1ZuwOkx^;1}Y4Y+GBwvA1=8Ggp2=rBL`ej%=|T z89Ch7@|iE+*l1&bazH8FP9S8tdRJA%Aog4J;IBVHLpALwDSB+VupQ}dyiqf(IFKM= zFo}0_2ZyF@^~t9!y`rT=erBafv)BjE)Z4+dyEZjj5qsm#YlGFY zDfZ+VD08M7x{cJFf`$Qufp{##u_M|^^2t3#tmli_2x%cKu@@;IuHe!l-~O+L#&)=7 zQYP8?PV~zdrPF$s-=!QDji7vC4&-3Ox1l4j08QbdNXX*e@2Xx?&QK)XNLE6{>xWXw z(QiS*=Nt}DrB?x^<<_j{iKF*mcW3IfMs{mzwyY)S?G5ex_;AVRq^!*M`gm3v5`>!* z-I_I;Q>Zg(l+VWVI<(%#&-1GY&R`K^=mV&TP-Mu2I_2{c>Nr%Xx+tnb(YAI}YBTEd z?}i?UpuV31Ib)Nw8MMrIEgQGj!VMP@M4pud72V9?@&1;<_Y=2Jxnbpdl*9=O&YpGp zd;74e)1>Hj?Xyp#ECy>jJ5k~Om>&PPv4NJu~O3=RlsGvb&$s~zV!iNza4-QD|M-zrVx(2);9f>WkP*XpvY? z@|ySz6Cw4CLa&WZq5;?zt>!2G(sm=^Ju6p!jQ+>%hJ4@kI%MTt6i!FzZqM}f)yAhr z-k<>=Te7*RUp%Nf%^6^I0SyYmL9X5WPZEw6!zI6!HS3&rxfczl#tdr)b!0u}@ejV$ zAVK*%Jvw5GD60dmklaq&uD?{ai2@Yc&bVTG=Tp0vYve57IiK=ATBYmqsNq(>TIEZdyfBqMmVF35?(dN)7zI67m{QHa$LdEaWs~>_nNtWegb7qa%oL0O4 zkfA?6C+ChfnWiy)FYCv$S)(bG9!aT#nixOFM?Q5!;%7Nuy4cdj1N&OFeU^T@c$cz%q(6{Yy^x%)7m&L(^nqeS z*Ag8?aM`w(vR8S2SPdg>V!V%I7DUPf4E#lw)RCiuD!h0Y=_v)(NC)|s)75EQt_OHw zCBCe8E&cQmf+S85V;QnjLA4rpaHc}@oSPK${#0N?p=);ZPj2L;lXOt}mMosbE#D&3 zAY;s5B6mn^_JE%?PJB5f$c5&4C73cFjtnNHqHA0Pk{h7kl0TpcT8-HbC0-R`5+-8g z-yUrit@bfoQ)GI)O;GUxq}VOfZntL9lU0uK@UIf(V0t!;Nxy1aPalfFI3)7%_NSwF z&M3@~Kz^**mem{&Pwzp#a;Z^>5nw6=!zTGF0&52lLRv(YurF{X`9EFyI{nDhc8Ez> zyqm7c;DPl%qFadfFSPkDp}9-XXMp}Hib8JS=^v(qjpEer*-JC-XUXaBE0vD! z(Zbx{o%PWmKEaiuD*vh&ut(nxsMV|fl`1}MTBX;t^5CbW{9hw4O$6lQc5DWM+|R#x z0YK;ZpAb8n^cY+SU>Wh|6T0>NupQ+UMoMw>-lW&O;CW821D8O|Y~j`}nHRj{e2G?D z|3rRBeMig%X)_i{xb@eRdfOISLr(>SrA!95KvW>zx3DH?x=%d)@>8_LEtXhd4xCG- z=ZuXYh6|!pSEZtvU0w(V-5S9)akpMAVi5%ws?oC)fz_T#Pv!_A>i7ok-&;)zG#FFst8 zWO+!!oqE0$2c_`8kBrOM>udsF2{E8(fy>_S^Z5{^cO+JeN+3r)jL;KxKqpSH63CtK z?}5gDunGA?*bn)8E>ut~6)|8XRLU*~sV1~vn(XeN=qLT7*|Z;0OFnqo)Je-F&QNL* z(m)J4^_$*G30;=n`2Dj+NXOS1qFBAF@e?amWrY8Y2E?3(?;hy<-ClNRf&KtEpR7L) zxFTMk+J^rywg?Pdc@OQ6U#046jtZbBxp3oL;Uab-#lQck9k!2Q<1)6*%N>$vH4;i1 zFOC`NfQD=Sz)XK$@lOS-V0HiBbeVqNmHk_|lpayZS0{0@UlyeRE_|p-I&b}6z$rI9 zCSouZ2HUfRH|@`GpQXtA4%KCmxB^w?{Iu3*+yB$}_e}g!-@1S&RyMl^7 zM6@9EmO+y%nkKmL+^N>iuEbH&t=`d1>SiRj@8PDnYZq92nSPFkaY4Ggjxl3)6uTo2 zn@fgx7OFC;L1Z|Kz3Cs0KK>|KbL5dA)1K4M)U_-4b(;nF>@kzcj+r)k>C*^yH!z^U zjH+}QG$br=I&J26%PAXniE(TFqcWv)8#v#S#rMkbnyw>(=Yj}c<`Bq_ci>723gi zdQ~)ElL`rwD&^pup68nh;X3JOo#oOUy9-JOnWj@1r@Mb7C&x@tk{P?YwZgxtT1iys z+e+y7cz2YG?#(pqF6=J(jXh0iPMRsZaBmVk>Fb5ZzI+OMrA#_U^*c#Cd2n*w{N>>l zHMqPuY*Zw_4Ee?JWtqoi3)R}|vt&F!1IsnP8; zQ*$8)NSxUb(#ieur~P0}+=JVO&<;6e;?Bsi6v8v_|P%lgmW+1zcnF=cd|FSt89 ztbIGbv|;MtX20n8WPHM}nBX>hmHUc&32MC(lnQ494N%M zwHY@MCgqaIPvx9|1DXzW@m~!{r}u&s7cbivaftifa-uI8;|Yv}Gck+rsMRR&wF??b z(d2*Kd^hD;h~h|WSbVNEjTV0QygyofuTCRo2qC@Ks@3LD7_SzMl{JcL0zV6tQu4Iz zEuyKO%%=RjQ__z9#rxpN<-gS;zCT>6x_DnI|8?WJ=qzd0NqYnz>G{Eb#4L}*-0XaU zo2i|YbjmEU2@+ck(a~RxW#KbBS=Li)cN^lrZt40Zvg+w+?s1Wov;MSe&$X`igQH#F zyi{W9wWHCc#PDJvxAWeeHsH7JWc(9>=bhn3_3l}N|Dp=NA~QHRIO-ZMc~&BnC)@};bj5uZ(lxA0x=Wr(W_O~_+~lw*oaw_rXgafbU;vD?RV z?WsNXH=8r{bE?YlyYq3=RLF0a(=Z`RdsrII~S>sfhF$m>oJup zVrOKF+<6MIBVVcwxaP9zkE#blsspNK*qBc1v*|H>zKF}I6-uwAJw(0lqMLfoE?tF! z$`TfsywBu}c#86ik`iGpx1#mpgE-xB|H>14TD?}drinc=)RfXOLu}pu3LNBzzXKMH zoLr1Q3&clSHQ}c@6)ijD9Hz>0XEc*d+r%3A+2Y15twT@q&;%kby4f3iMT!vSLU`ejE-1OPa$zMx<$v`SSH^oQ$l~LswVjzMb7&bOag$ zmx@TE!^8;&U!11}pj&Vve%<+Jv>Xz7x9>!V$*u-E@??ysY!Fxk?A-6vpP&N`l@mRf z^?T=lITW5(BsKW4{D7}oz5AbB-fGtgKd9uNsZLw+LPH23k+IQxaXnTsYov4n#t5*e za&41eS^&WvUUd~k_Rl~JT;!aPm!AAe|9ngxVECxadG@^5qJ7yU2ia4Fj2g8kDpQ~9 zBdO9!4}ysRl2iw)ER%RjHkeso+y~6B3E7d>S-cZ|@lF4M{cUWM5Idj>1vrjsX&1LXGWA z-$`$NiPtIE;%i({$ID}R2z#+Xq2Z#=;*;vrhg9A>bfInJ)JLT{L$}(?RNwa!0k=1` zud-S%w~MQu=XbbBhCKw?`(`*SuC$0(%?BLb0!whV|2< zD|%Kv>)V5rVzN+tit^2T6fngLuxdj?yqC!nn^v@i9UabqGAY>lFAU;>Aes zwSfa$PmToShWOj{=kFk?ML&`J~4dSr@~jS$1< zyF{tOyq@5}H~$^}4W_+RUZ3KVYDcPWAjTf)5QtJ#r|NNCb2_K{0q7PCsa+`HU~ekA zvKgJzc_cA6_Q#35ee)Vq1j8r)SQ%~7_KgbiW&LA<3Yc%FT2r^fmUozk=dIy%xx;Nn zr&T{PCnJ=%is9L+R&!RW{!~PKOThPqpE|^C>MP18ZdEpV^FXgj6bzp-al+lDw4)22 zwkgM4)f(yD+G^YOtr+pKSdXTO4{}KdZ{I?*UM{}jy2hHc4<#NB%2}Mk*?az4UOK*z zF5%&v5DKnoPVmRA{z7@rcYQQU!$hf$^z6OO;7Y!yq0Fz&B-kh=GO5a4*bpxrQr)CY!VB);@h{iO zEg9XO>93@xEkqb+f6lODFN=MK5XY9q9wwoM$x~M1<|$2Lc%>ewguINWoYpEaiT>M= zhfJ5)(Y;g!WZr3}9Pq|pICM?!RKazgE`_DuQJHAmHVN#UZn|%vZMqX8dasRl-xAB$ zMyZ>usNk+9Ty$76A;7lp$>LoxIidBDSvOMs40z`zJ5ao<%PXpM2?^zzd&?k2WsyyO zQ0YRjNd0q)OFr)MLx>iFaSL+rZWv{_F3xvFQ=9DHpF-jUJLOjtVPS59VhE(!ufP_} zvPW=U8p+&-yffczJRT@7N9`6d#Q1aFj>+0#&q({g*4a^xYHLU9@5E@YPt*FBo^{Z8 z8tb*d7PgY2jXH5nd78Gbb(;zX)&rfo{|xQnoZ7-EuFBT`8Hy{5YE3)g!`3_VKKzjP z_t{cpcA;L=Bv<-tStpzqUNPa&y*sO3i1@$^QsRf2B0}s9#g*=tFKbyk3mzi{pj3#Ekt3b4O7mPLWNQGm?mL0o+!O?(pRwk7 z^K)&Xn844Kqy7bx)EGYRC5E{E{H5F!nRHB_w9vG!X|t~3{~F}yl2;lUg9ROCwT@=- zSF1{v!aEz6IEz-w+DFbDh4$7mm~yry{QJD38C%o zpN}D~<)(RO>us>je)~SiNVk00zxrZXQbNwhW6vf~^c7BFJH^86Df2!vo`Y)=e}b6Z z_!1+s*Osj3@soF>2x!tkCPN-kLe8U^8jZ_g1Fdok*)YiJg&X%OnH1(I;lATFmNHUU zT|=hKO~~Jh_`GEXaSJko_*R|e(do?2YRtD6oBhoUCw81JL;`KtNun;pV=JHv#sQ$b zpL5E|a4Mx%U4(d)?*%Ud32%scx#|;MSwNpda!PtjMBY9gIQES=v z80l`Ky6e1EgC=1I*%5-!FI{WF|T8(q%*i8cBw4=K9o z;X>M$E1Gz9HA_&&nyjdo^8cWO^H|2@aG(B*)t+n(Ixro%TUd9?)u%Vhul0E|Z=Yr0 zxV?b>Cs2O)Mgba;4Howjv9h(v-~9QYoTG?>BA@MjeXgY*6SZK@ylj`9f6UI4t)Whn zt$*DGcHm_`nV&kx3qu?C%<9*?SvYy1?quZCIeOz$pJ`^>oTO`_Dz%;?sF^59BhY^$ zHYMn2!rQeuPwVEJH>nx8tiUFFugzo#vM6k;nCP7c!a4nf>I`s z*Wkl?>#xSsNiTM_iqA=Qwa+ueLi}~fS$gh~I<}ksNNIAIAR|V8#|q6V4QAe4Hft8a z=A3v<0;u#^r*XrnPivj6;20%VqnoQ4j=hJ0u?|JdM>%eeq9Dg&?`{NvZuF}SLU@=H zxc;GI9`R$;4+I}gbz->m2aWCLmoLF_!>`Lik9mJn*yDB{s(EEvCd`)WivsBgLw-ts zm~Pn3WW_>6OCfq+Sx~Tm#X0zD@_OT2BC`n5^f-ekp1S#^8#B)SzT?Uh;BcGFxX7c7 zBD172W>lAp8V72zCG!;#(t?UQ5n@cJi|Zxpz8#|@QjKfDpCy!-_rAvfBv=T4{f%?A z&E`vMm|$h^)%nS7hg;;}H6VW~73xDFwEY!*Ntw=ihh;>DoJi@LmWO3-&TTxBBa9XhrV=^r0@>zE}wxtCb|I|8lY33m0@f`tI>J*YJYm?$p@KJ$c+P~Hy*#w}${YmV}3B{wG;!)(Q%pj&i$FFx3S0}u5&nHDkg zQBF)_|M&Xe#(WljEH`UIdQQI<+8fibdsDy=(L;Uy0MS&5SG)m&cg`Zaon)%z+tiw? zlXQ3RZ)kTRTdk&>kcx#sQLBux3iqty%yy5LGS_>FUyAA&R%VGvBV*x&AeipoH~2rhD2hU(hMRs6+%~<*Tl9 z6LG$iz6XL8Ykcm!W3PqNRE`I-3vsbs`fq9tol3MEde$!s*G_&>Ny6+L~XZ z>>PV13QQMbdHv;%}t>H>3(oraOKREr!os>wW-35fvk zH_FT_!pw;6<#kZO-u2j8Kv^s1TATb=|=F;|czuKYSG0^64H2d%m(YmA(*c-ch)@xG-%^WAo($%*p z=hq_Gy0tPt=AQw43zKGso8#MC|H#?;TJ&A|R&k2d=DEp65*U9xz8Su7%q-XeB6hEO zgmKXqtJkO(?dNyXPHVGwc=ToLVrMl;O!}1!kqedHL}gv#&1=60snPwFQp>Kobna3~ zqf7xRx>U~p__gc!Og!Df7E^#6`-xy-!bXZuFCi^w4PP_$s$G_)Fx7ZpglziXk+ zK08RV^LTXL=cWC=#bM{Or+<^)Q9p90luGV0eqx7bo4x^Ub8i>CpMFOV+r%9=+1>@Q z-4`p+KY-6r-$g?F%nJB@c^qno!R`@R=fG`5-1NIDj8h!VUGopHKLsCLAkxCog#+&J z&j6{NTW?67NaeoRhyPR6cSpnhMDY^6i{5*QPDERE)u_>Xi5{I;z4uNKJz^1^CA!su zXpyjblvSdwPOP?q=l9Ba@BKG(=gz$|_xrtfKIfi$W}wmK zgO*>OyfkH=Ey03)0HPHz7pNbcBdU}3u94U0w)-}~b=>KMSnbLqPa)57Vn3u|n6UP) zBT2e2;?{^2O_m`tyiod(czWVF;+T*cCW5NOnTG>q_A_ct{dcid0*@t19RkAKBC{xs z=BumOX48ea_QGH(-jGeM?yXgELEj})%Nq}{rjPG(@^}K@EH3q9OKnKL;opwRntQFj zS_+*m9)*%_xJ`wqV4w*{6M-MqyOUH&p-~A!`xxf4voNawVyKYu8^dip5VBo0UDUP1 zponYkc_Fv3A2F0ZMH~26K+WL))B<$0?6VT%A`4$Du>&%)Z=XRoW0>9?W;@yKB`eri z>b}54NtvxA95cl; zfSMO;;8`Q#>Bp7=0kQuoJ;j8zhU-Ycr#N5|GKsGZgvmBhlcBI7{G^fxJFL%_JX%xK ze3qb#_6${>6+g;s1#Z7cH{`7^gCNs;7ev1%VOs%VtFzNr0@o=&e ziaahEQ!9B|@0*M1ok3=U&k}*(@3LcP#zP->wYX?=;>nUf1xZ+GzaxZQPuw6UrY{8tFzqGQ037E%7}_Ji{yHx;qqdyX z0s7+OGzcHK>aB&*{6mn|2n#?zgY9%uG@bhvUQEC3b5!s~gM11i%>D}^yfuLuwWt4W zf+P9!iSV;pm|X-F_Yf3UAR)^4lm92*&z~#nm{xp?@R|p6L5h;ftREa3ab8?rn42HI z;tOgG#OONe;Mp!Y0xX7J68P3fSE9vF$3f!uby5ZF4M<~IT#azI$n9mi(?SMPH^@)5 z0zDpiIn`BlbNzrt@uPI^Np5F90}3m)0Ed6}Gf**wKEnkCUO#M(iizH=0H`B1zpnf! zm3%f*#HoRxOT$y-44!{OnzO(v*R5O1`nE&2{k<4a?40DpE8_Tz3r_>am!p}FzLk{r z-09?~?VGJn_M@5UgvrwF=d8)qxz$dCo^&{;R^){wmOoCDuzfI$d3t2IsF8_f$Y{kM z*ZX?!rD#d2dARw7AA=_hB`h1s%3fx9a~DybRq(99v_WPvn#aa5DZ?Ebdg za<}soJi~?jx05YOJfH=V_2uyD4s~Vx^6Kadj|(xOZT*>Yma1Bcj3n zx>ik5}WgIf$zV^Lcgs?!Q_3Ye+DLHVCpxr)jm2z+KwcFY|n))o{si1}Pln*99w^cjWRFyU+byZS&T) zSF|$s-pYH3F*oAOMV6oRI$^`b%{s5?a`{1h zn%M*O8fq&c0vT(RQ)9j%JT6#Pl;8_FJ#e1Hd-3p2m%n?7Uw;)F!>up(Y!49;g&;b^ zl2mBXLCJ6CVx9#II`34uWXDhB&@1j{-B}TG;+n${+c;x@rU>-2?^-Z z5-eM|fYThWogecFSyelh5Z4!whP#D$#zN24!05GtGnh16re|%DT5yFB7SzYPKr+aI z`S+$3DLo@+x*n4u>U(7RufHRtOO=po^R|PVT2J>qBeXCI7+jOX*=pM7y?^@sr<5yw z^DhQ8JG{Rwx|#7nv<(i;gGoJWV)00$wBuf}((V%?R;x&A<+j zZ9{&b1UsI`$sc1U&M{@C*SNs`#{`mb$*@nr3?xdkk4GhgimbvZlmdpw>D;FBkxc6o zIbdColXwft#xPKRSb_aLJ;WE>-5Wruz>J0D)-X>Nf}Dsk2o-z-Ur;t{Z<#HkEtB^6FP({0 zvQnV{@I*aCPrn$9-Kk6F!MR}OV7)c zGM9~iMA14NWUjECn5)mQ=Q(2th<9?dpOGUn>{L2@-ej$D7xE`&u-Jk|U?xXHdVEStf_2sj)OvlIp>By%f zYOzn9*U!$HNPB%IQNi12w@H18ZAWmvk8+aEEv)W)`BRqnr9(as+*Q8RBL$Y5I1;J&5lrL@h@|&XN_Q^cSO^Hz|)JQe0w>S#U<)|wp z@eh1eEUv@O)w244B4t$@vZ@!Hp?qh$qC9AL3OFzuY9zWv09qF=uM?APcW=c#1(V0Z%> zAX3f=yn=8*{nc`yboTLE-wV^IpfXr4`#tHT&wGsDYXw9Dq22o_ROLA6X;sp0opYcx z)T!>BG%OrwKdDILorhpx{>}TwjIzC;Hjo4D85jf6 z_zbG?X-RZ?wNTvRAHD2v+B~&P%U%kV%()=vemaZ*BQ!<@W@cDYHxt)ypk@}MUrDfc z#{N4o-?aKogG1#S8=X@Ydedhxbfi2HAnYY#(n-S*Y3F_ANQ3+5C(}<2E@}4sKxeN6 zcvfy~QUIE)JJ?`}yu3(5QwR+p(oIPp&a>1p_brBwkwj{e0n_S-^a}gm=VDAf`DQA6 zT5uBcR(9xX2f`spLI%g z#z<<*L6Q6-W7my;>xp^@Yl^4pG@{OibbeBZX(7~S?w2&+;LAdPm_fRLX~+a29~ zW0n~G*We2HT+N8aoz`2tVwZ=lqkwv2x;3$djGnovt*bwIrApNHx1)a^>8vjVC%c{9 zM`k4Lz^5svc>^1=q2PY)2oN!9MxeGVN112HvBuQ&M`trAnT~xlwcH?Kwg5*=X#NTi zR2Ns6XTpD_VFI5+ehwHxTKiQ-2^w3Sid9aF26^~5EJ%5}OlfM{7`=Y5r7&dxP8cSW zR$P{E!z~)-)H$f0*OCyGDSebw+^bs~`hD2i__1@B{6^VKfljjS8NwFbN;R*-kHanP zYFWu;!kh{IF*>+${9Q|H(hC=VK6eOKu9JDkbnw^bdwRkS$ZjW<9OMU7VfwXssk{?| zh~)}Z67gr4C!$R9y>cdJmn`hYU+LiEGYzz0Q-zFbXpq5t6P>K&;cK{N>nqAbBv&0) zYGsvKKzus;g+QZSqeLd_&f!?d7TVN6y1k2lrr)N~deWDv+%^*2OfU&;P;rM^EA*2@|A_~^s(DUkHE((=|fEOxk~v@A%!6Z$JMYlS_1Z+mi2<-HnsdFc^af8I&ey0 zpI!wMF^GxiM=NexbYcvaN(bkOp+kjjBNz}PZpWWY8IXasKrF;y?7c1cup%^^t|1%5 zYyIMFmd=}}iLv))VEPwZHLrwfSpS}f*6NP^3e86y2BhdF>95tEE?xs`(5&*C9>ox? znHi%OoYwj)lRH~AmaKBK0emKc_o*OpI6$pH!zd=}_4(o_p(9GpXCT{W$RKbDq9x~w zT+ysc_eFW_8bNQpEnsNRlMb{nCWJD9@v;HY z7d`FL)|Ki5pck>V8{fx>P$h39D!zk6M81b%NF{^_VC}P*+Bfo7@1atJv-Ye;Vcl$+ zmf@Pc#n($tk5_@3%e@p_9!>pMMo6C3Wik+8G?pis2y76rSq?vKd`IdJbWFx)CxHqo z2(-9e^&vj$K4o%l<20bTt+cK|+rgyAn^R_bRh7ltKF`DiYQ^dLE0n7K_W}X&hcz$b z4-b;oHuSo4ln(UPZZQOV7IGiZkzA*n!yBq6Lk`u7zw*#n%6NCNX;{&+>$lDeYa62i z2s0DINOP*sGXofLL+cqM8T~fF1tQJIrY&G#wNv&y_AsV7TEQgICI}WvnxPX}w zwVJ6sWOjg~A5kr_F2<^$!u${nzH|YveylmkJX`a)PX5Q9YV~ z#EO4uh{UMfEt zi{avw&xG7*6{`02iPJR2r;qnvTFR7seAjd}2^6CE!JDx@vD%2N{N~E>aS4OQS-e$Y zn(r?rG3kSE>vPYx(C;@(XaM2YzVr!FjJv(yk}t$aDx1;ne)^-7QoR>Rt*v$ zexD~gF_&*gJCO35R@4yA_A#UWtukSZZVWfycU>a*tLQp0*QimTb0MwKt<)#sS4uB> znmEy{fcL6fLWL|@<|A6gYUeKMf98=4neDec#iyls|NeF%-1so*uU@)2%Z(rmU@b$J z`UsJ8!sFjtINIFaGLQwvlHRCE76$Ah%7i87cO#I~qn3X1DqzeyRulwi(l@f?{PN#V zj!3FVmG$KkyNCPbABHApceIfjmu%P1U`b_=GjtrQG_+96wUpFCH?%`GH|q9RVD^EY zShNn7-wSL&|5Rk~52wN<{qtAjnY?UeoaH3-*G8q;2xDRi zQ4X|T>PR{Kn2HEq$F3=&-)8~tQDFAHjF&o^E_{ZD<)!RXj8>7)YyE0Zf`%L8Ad90~-_vBGCa9M{B+G3qHaWG$)cqU(U zf-TcMRmr`(vnHW&;(67oqLXYp-{5JB`QTBj{y0^^(fcX+JxEn2f0 zjQ+1MoOU2VEhx79hzmT6WusKDB+_Lw$BV1W%=r;SK5{N$A+jn{mU zMTvPP?^kiKlfp0W4eQ3$5)iMIGuqi>d^eGuTiM!kbgcLLqKy)MewQFTsSK*-cD)zG z2cTroGVX;~x)fk;lG9qih(U+2XOukzRrpg$csKUm(0orq7<+`tz>Oq zy&OzBlA{+(wQDDUX8Cv}qj6!2m2g*8_4*lv{)iQzMbOlY#%P~#hh~ScR^{}|LQpXU z0d3|f2y7YKkKw=NJFite(!Ihu?P^Qa=jkuA#gx2c-J$mH(#t;%9*?ak<0+S@kKy*t zF+bcZqE;E;sy#v*DT6JS=I-8Risp4s7rcM1+1#{xBs`CcACc_BintRB==B0$ ze_P)I%WDHqCm5}wLzqdCyL<>8O^MS_AuvOl_Cpau;^=;*dMJT~B8<~otw=U;(qt_8p*R(eZ!ZGr9#7Nhl{NY?*1>a3iNJqxZi*T5^)fv;q z8UmqBju^7Vyc8OdK0gZ+9=G z%htN2pjjgv_f#w(k;nm88r!tAjh`y8mfmad1s+!FKkFSv)@33WXsQB@uaU5VxVNN z=x;f`3(B1G(|V-JHD#eOmY-Ay;G&$XV7F6qltg&}d+9yB{9y9j_*sA16$@v=;Nz#e z8DZl7uHGRg0IAafJtFZ&jk>*@o2mq#FqtDO6}$4g35dUUAV{(9{G#P*c%Kyp^0I7X zKD9Pb+{zoh?jn)&qr5y(DEYM!c+i-VDloIhiEdTzMXieA$R_=##jRF6ap?jZV}sL) znvhX$$uKpV-VYV%#o1=f zty!G8&^G=S2ePDhhK{-Xa<9}VMHJJ>lDytC)+V_!xZ4#fbJc$_tK275Z@0|$;@T`O zcVOMCz9vHL&8&OX3tT{e)I~c>^$hoXdmF#frW~774Hk4YF`zQnx?w>IYNe#eMS&r# zx9e6A6->rB@Z*O=o$oVE5eK*c{I)r8idkJ;02N3LriY8lP=RyqO+7wXvHh>H38zxz z$?jV=#0$*(QH4ho`->+5=ESh|9cI-hen51(A}ivFH4e;T{yP&erl<^P#GfepBo7ueG9zBQ$Y zey~u8;G>S4JSx%FV~ucDx@}Ou7q`#%qJ7#3Hf(&MKbw1eek}Ni+k-k#_-jj0P^7bB@R*uJ1A+pldWEZ|G{qISl0Xk8&v{z88P5yyYSAM?qzO zXpHFiijPPOIX=*a z5OHq#V7jgLg{`=7to%k@m@bMv&H^;_fa}(CwEPcyU%Al9l>)4GH+%nFieEIJ!O8X` z?&079Ig|l8drYw&{Oy#rnde}*nwLBh+L5zhbcI*jQ?D^yT~3M|j{MwJ)Y>BUHVcnB zNpmPD4t!ko|RQWZ_(#du5AwS{okh!nvXHkMEc-ILvDvjG6pP1*nb z5;#!etyIGK93xZvcr;nRm;B!D8McEFgieqL=6EPh zbK2ICj_gY0(!0(eSm{#i7%D;1eV+^m-e}HblD-2i2?8Vpzd4g$ zh5z|B|B)Tdwxp=%#aZ2aUSD&0f#R~I!YXX0%*Bb2KF0$+I6J0~-QU(Z>mTukn2r#c zHI?OMSBBu{@|M7Y>*W6VA0M zxo$o#`fLc#{vxmyegYKQ;y%g#!DuSvoKda(?nDae8#Y~U1D7a^AxFef^{!$<$zE*P zxrLmK)9gs~;q8!W9JHv&Z;KZV-*dd1X94uBxN$|QFApfKO*k=ks$3sj?;z0lk*iPf zS|~=xL`@=r;isLnBF)jOiJgsAV~;`O_${ZckME z!g@%SRZHQnSd>k3*+-JZ(&MWK?!>&);H-Z$Z6+%!-t7DL@5?_OH43ZyZG2rL6fEt8 zzX|-sv_V1r6y_qA+?2(^6NcCd_8t(Yv0Ulm%1KLY@_|li@5`AEr_W^YdBY*H=kMWAO}YRyG{P_c zy%9hm0vKg&{$aTUd$rk6L@fWk?nHcA`>R0$Spjo$Wq}2m?nLcK#!6J#`>sl98lR^# zC*_$HHS_x$t9hPmXZX7^O+mu;0PvZt=xmbC{@hW5WWpUgS@~@H=G5X^zl=$S@DKP^ z6{9+oMmRe{avHSF@Z^H{$i<#MR7Dkh)b`ROkX_#BO=G%vP1GXKQ|*>E_;FUNYICb! z>hXMM+kNGMImG$yQ)_#i55ZS>V7-_uw(_CFu4N^cltk%F7l){i`L07O>|0u?0wII* z3>~k(xySw?_5I4}GG1$|YsC*)fTJw)OX>0O(zx~Tg=O`P>75y)Ek*1{`fxRYCwmUh4gzNEd*8;e%K?e}!};{a@GY!>yL0A_4}Npha`hRoY2eq8_3~RB--do0h-JBoyWEIyM4Pdb)9Lg~%fivz zzg`iSIQ4G(BVxZHilPD_tpCEftSa1j!EW*2^e(q=%1?Mf6Vf}afN7n?aJ|@hWQp;B z>%sEod(1;|vTWKf%a)Rw{iP}c+w$sH;?pNiy49I-e7LB~SNHa5m7{ki+FNQgabO#C zlp$dpiu2Wt>4`Rt>sJANFB5+sDO)af;)X|dkeX)E|$H{4C4TV2w)UVyWS zBicTeYBrVdxIP zYIoHB3HU<^9Qrf2rPdr}71GKm{i2k+YW4RZ<0W%Pn%ym8*@c4x=j~(Z{0NB+FRj; z3~XG+l6Fq@7;dD)Z!xUik<8FO`K{_*aGKU#d@gU6Tu}PwaVFsjv*B_4Z>>%1tL34m z0@9?vxG`C*mfTL9yX$aKxalXviR;IK5bH+LiFuC>t;C(Kc%rq>jeEW|oS%c+3q8sE z`j6J4KCJJ6ZM!V}tb|ceO{HMFg={~Qbz_yGc@-M0^pG~}GrrwrNR^i>ELN0|ojOqA z0kTOeUTnky3aFD>toB3j5x8165CVZ_t*M4Z--peA<7COXq!WWr9rv51`S_{x4Js$N zV#~9cKH^U(sv<-PQcg3{eMCGl)g)d)nYc5r5*8~hMcR|L5BusfHTYnys@m+8n^?7U z#=H7CK2g`+tcVU zFq-ph)^g1@*sT2<#g)sr-rJU*MMvW2$ma@gf<8MR{r!Rze?#u9#C1z0K$-Urs~od? z7)WQ;FdoCOLQHa^SkX+(+G$yS$52cy{;rm>_1(AQT$EX$J-ve0^pYa-Th$P#6utEB za8)-QGAkI^tMm$GF=lI0c(*N1&wAd+j zuujW(j4GXC^JXlHG-%qwtfQHrllyAF3YMY$%j8?^=frz9Vh4d1i%i z6)5+Kn$Az7W+i^XSew&nIX3Iv?2cs7$O*4W!*X|ICu7D?=pT*mg0t;LXz-as%X~xv zSjk$!A>X{-SeH^WjVN0O4>lsRq6XtW1_0`sq}6^-uc6RqkNrXtNXMC(cnVTyWsP$O z*7HP{uJg+?K9*loFKT>yx+QzjG;Xi+Q3^HSiSc85%E-tiZWUje6@h@<7`5Y68&dR`F*BM|IIdrGRx~U_D!$c1TxVUBr;fyIThEx56p)<6E9G{c=DOCX%eiRv)|)ROGZDZ z)qIeK+)Mi$V*2wu2z4v$KktLG-$9No!rziz97`bzi^;-L$i>cXn_Z~(Kf;=L5Vq0B z5&8pzJcuYY9B+)Q2>R`NO9HOgZ|O6kpQ};rF4%ydJoD`?#ul07ZVj;MOET8tfGEed zIx?Q%&8Ah=rDi~xvU0B?Zv!u@6tz%0JXFn@T=CSw&8_a6yOd(yzK2+dEKpLe3|vyr z#qveoWpLcyoE+Fs2b#nV@{a?F^C0Bhlf39v5@+cld&jk3nm;mckD#?8x*#i&8oO>& zA=-JP%E3!1>&Vi!E=*B{;x!!$R~Y(x55=s{$DC444w9JMS>3R_u5D;kNx7zPyVV}b zs{d+Jrhh!H5r3pIZ<}uVTe$2pHBq3fSU&!HKPF)RZTciWPE{2%HE{>U+lLL%CK9PY zsskl1@(Dg=OgUL$gzK5&bsTw+ z=uySmpTbv`LUOC(yo8V@qW);a&64a@%)Ky(n+EzuD;>g-d!37wfHmcBYW^&KD0y?C zv|}8P7V}=3*?^Jjai-?xHtuv_*LApGR~t}-**B8}a(TCZkry2<_q^_8V>|l8SZv+< zR*;D5)Hk!-;ZZ}i_s9VW^-m#xmwew1agXWV6T}GtP-h1GVdRfCA=g(~!Zg@#dpsJ~ zQ=4@clkgO&XQIBRR;T<{(bLs+RmlT7#sQTtAC&^fTEQuW9>-zfCc#?{N1K;TUU$t= z1C;NFgwDUZI0x3FAY8x)zOgFt(tqjLdDF`kAJ=!l@AJO`ygAP|7U_+S2sUymToUMXw+ao>7vBvh=KCR6BMlIkx!yIvIv1yHpx- z@qVp$Gx$BVqtv=}Kwtjn=j&YZiX<@2=7>#sbvtXo3r#ZzCKXgi-p+s-_<^mA>G9sX#YLJQyq zF{ai97^OMhtUlQqml3TLg9Q``Lnj49Mlhi0ZTM7M+lr|Lp5Br}Q&-VZhA6&`{vVxt B;0OQ! literal 0 HcmV?d00001 diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md index fb1b466..c95dca9 100644 --- a/docs/troubleshooting.md +++ b/docs/troubleshooting.md @@ -14,7 +14,7 @@ Do the following: * **Check the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V. * **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 output. * **Make sure correct IMU model is chosen**. If using ICM-20948 board, change `MPU9250` to `ICM20948` everywhere in the `imu.ino` file. -* **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. +* **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*). * **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**. * **Make sure you're not moving the drone several seconds after the power on**. The drone calibrates its gyroscope on the start so it should stay still for a while. * **Check the IMU is working**. Perform `imu` command and check its output: diff --git a/flix/cli.ino b/flix/cli.ino index 79d002b..b70ffe8 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -5,6 +5,7 @@ #include "pid.h" #include "vector.h" +#include "util.h" extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; extern float loopRate, dt; @@ -39,52 +40,69 @@ const char* motd = "reset - reset drone's state\n" "reboot - reboot the drone\n"; -void doCommand(String str) { +void print(const char* format, ...) { + char buf[1000]; + va_list args; + va_start(args, format); + vsnprintf(buf, sizeof(buf), format, args); + va_end(args); + Serial.print(buf); +#if WIFI_ENABLED + mavlinkPrint(buf); +#endif +} + +void doCommand(String str, bool echo = false) { // parse command String command, arg0, arg1; splitString(str, command, arg0, arg1); + // echo command + if (echo && !command.isEmpty()) { + print("> %s\n", str.c_str()); + } + // execute command if (command == "help" || command == "motd") { - Serial.println(motd); + print("%s\n", motd); } else if (command == "p" && arg0 == "") { printParameters(); } else if (command == "p" && arg0 != "" && arg1 == "") { - Serial.printf("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str())); + print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str())); } else if (command == "p") { bool success = setParameter(arg0.c_str(), arg1.toFloat()); if (success) { - Serial.printf("%s = %g\n", arg0.c_str(), arg1.toFloat()); + print("%s = %g\n", arg0.c_str(), arg1.toFloat()); } else { - Serial.printf("Parameter not found: %s\n", arg0.c_str()); + print("Parameter not found: %s\n", arg0.c_str()); } } else if (command == "preset") { resetParameters(); } else if (command == "time") { - Serial.printf("Time: %f\n", t); - Serial.printf("Loop rate: %f\n", loopRate); - Serial.printf("dt: %f\n", dt); + print("Time: %f\n", t); + print("Loop rate: %f\n", loopRate); + print("dt: %f\n", dt); } else if (command == "ps") { Vector a = attitude.toEulerZYX(); - Serial.printf("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z)); + print("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z)); } else if (command == "psq") { - Serial.printf("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w); + print("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w); } else if (command == "imu") { printIMUInfo(); - Serial.printf("gyro: %f %f %f\n", rates.x, rates.y, rates.z); - Serial.printf("acc: %f %f %f\n", acc.x, acc.y, acc.z); + print("gyro: %f %f %f\n", rates.x, rates.y, rates.z); + print("acc: %f %f %f\n", acc.x, acc.y, acc.z); printIMUCal(); - Serial.printf("rate: %f\n", loopRate); + print("rate: %f\n", loopRate); } else if (command == "rc") { - Serial.printf("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n", + print("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n", channels[throttleChannel], channels[yawChannel], channels[pitchChannel], channels[rollChannel], channels[armedChannel], channels[modeChannel]); - Serial.printf("Control: throttle %g yaw %g pitch %g roll %g armed %g mode %g\n", + print("Control: throttle %g yaw %g pitch %g roll %g armed %g mode %g\n", controls[throttleChannel], controls[yawChannel], controls[pitchChannel], controls[rollChannel], controls[armedChannel], controls[modeChannel]); - Serial.printf("Mode: %s\n", getModeName()); + print("Mode: %s\n", getModeName()); } else if (command == "mot") { - Serial.printf("Motors: front-right %g front-left %g rear-right %g rear-left %g\n", + print("Motors: front-right %g front-left %g rear-right %g rear-left %g\n", motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]); } else if (command == "log") { dumpLog(); @@ -109,7 +127,7 @@ void doCommand(String str) { } else if (command == "") { // do nothing } else { - Serial.println("Invalid command: " + command); + print("Invalid command: %s\n", command.c_str()); } } @@ -118,7 +136,7 @@ void handleInput() { static String input; if (showMotd) { - Serial.println(motd); + print("%s\n", motd); showMotd = false; } diff --git a/flix/flix.ino b/flix/flix.ino index b33f1db..2d89177 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -22,7 +22,7 @@ float motors[4]; // normalized motors thrust in range [-1..1] void setup() { Serial.begin(SERIAL_BAUDRATE); - Serial.println("Initializing flix"); + print("Initializing flix"); disableBrownOut(); setupParameters(); setupLED(); @@ -34,7 +34,7 @@ void setup() { setupIMU(); setupRC(); setLED(false); - Serial.println("Initializing complete"); + print("Initializing complete"); } void loop() { diff --git a/flix/imu.ino b/flix/imu.ino index 2bdcbcc..d75d86a 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -14,7 +14,7 @@ Vector gyroBias; Vector accScale(1, 1, 1); void setupIMU() { - Serial.println("Setup IMU"); + print("Setup IMU\n"); IMU.begin(); configureIMU(); delay(500); // wait a bit before calibrating @@ -49,7 +49,7 @@ void rotateIMU(Vector& data) { void calibrateGyro() { const int samples = 1000; - Serial.println("Calibrating gyro, stand still"); + print("Calibrating gyro, stand still\n"); IMU.setGyroRange(IMU.GYRO_RANGE_250DPS); // the most sensitive mode gyroBias = Vector(0, 0, 0); @@ -65,7 +65,7 @@ void calibrateGyro() { } void calibrateAccel() { - Serial.println("Calibrating accelerometer"); + print("Calibrating accelerometer\n"); IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode Serial.setTimeout(60000); @@ -108,22 +108,22 @@ void calibrateAccelOnce() { if (acc.x < accMin.x) accMin.x = acc.x; if (acc.y < accMin.y) accMin.y = acc.y; if (acc.z < accMin.z) accMin.z = acc.z; - Serial.printf("acc %f %f %f\n", acc.x, acc.y, acc.z); - Serial.printf("max %f %f %f\n", accMax.x, accMax.y, accMax.z); - Serial.printf("min %f %f %f\n", accMin.x, accMin.y, accMin.z); + print("acc %f %f %f\n", acc.x, acc.y, acc.z); + print("max %f %f %f\n", accMax.x, accMax.y, accMax.z); + print("min %f %f %f\n", accMin.x, accMin.y, accMin.z); // Compute scale and bias accScale = (accMax - accMin) / 2 / ONE_G; accBias = (accMax + accMin) / 2; } void printIMUCal() { - Serial.printf("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z); - Serial.printf("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z); - Serial.printf("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z); + print("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z); + print("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z); + print("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z); } void printIMUInfo() { - IMU.status() ? Serial.printf("status: ERROR %d\n", IMU.status()) : Serial.println("status: OK"); - Serial.printf("model: %s\n", IMU.getModel()); - Serial.printf("who am I: 0x%02X\n", IMU.whoAmI()); + IMU.status() ? print("status: ERROR %d\n", IMU.status()) : print("status: OK\n"); + print("model: %s\n", IMU.getModel()); + print("who am I: 0x%02X\n", IMU.whoAmI()); } diff --git a/flix/log.ino b/flix/log.ino index d6be3e9..0257da4 100644 --- a/flix/log.ino +++ b/flix/log.ino @@ -67,13 +67,13 @@ void logData() { void dumpLog() { // Print header for (int i = 0; i < logColumns; i++) { - Serial.printf("%s%s", logEntries[i].name, i < logColumns - 1 ? "," : "\n"); + print("%s%s", logEntries[i].name, i < logColumns - 1 ? "," : "\n"); } // Print data for (int i = 0; i < LOG_SIZE; i++) { if (logBuffer[i][0] == 0) continue; // skip empty records for (int j = 0; j < logColumns; j++) { - Serial.printf("%g%s", logBuffer[i][j], j < logColumns - 1 ? "," : "\n"); + print("%g%s", logBuffer[i][j], j < logColumns - 1 ? "," : "\n"); } } } diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 94e8490..b358316 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -147,6 +147,14 @@ void handleMavlink(const void *_msg) { sendMessage(&msg); } + if (msg->msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) { + mavlink_serial_control_t serialControl; + mavlink_msg_serial_control_decode(msg, &serialControl); + char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1]; + strlcpy(data, (const char *)serialControl.data, serialControl.count); // data might be not null-terminated + doCommand(data, true); + } + // Handle commands if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { mavlink_command_long_t commandLong; @@ -167,6 +175,19 @@ void handleMavlink(const void *_msg) { } } +// Send shell output to GCS +void mavlinkPrint(const char* str) { + // Send data in chunks + for (int i = 0; i < strlen(str); i += MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN) { + char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1]; + strlcpy(data, str + i, sizeof(data)); + mavlink_message_t msg; + mavlink_msg_serial_control_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, + SERIAL_CONTROL_DEV_SHELL, 0, 0, 0, strlen(data), (uint8_t *)data, 0, 0); + sendMessage(&msg); + } +} + // Convert Forward-Left-Up to Forward-Right-Down quaternion inline Quaternion FLU2FRD(const Quaternion &q) { return Quaternion(q.w, q.x, -q.y, -q.z); diff --git a/flix/motors.ino b/flix/motors.ino index 5c28aea..bf792d9 100644 --- a/flix/motors.ino +++ b/flix/motors.ino @@ -24,7 +24,7 @@ const int MOTOR_FRONT_RIGHT = 2; const int MOTOR_FRONT_LEFT = 3; void setupMotors() { - Serial.println("Setup Motors"); + print("Setup Motors\n"); // configure pins ledcAttach(MOTOR_0_PIN, PWM_FREQUENCY, PWM_RESOLUTION); @@ -33,7 +33,7 @@ void setupMotors() { ledcAttach(MOTOR_3_PIN, PWM_FREQUENCY, PWM_RESOLUTION); sendMotors(); - Serial.println("Motors initialized"); + print("Motors initialized\n"); } int getDutyCycle(float value) { @@ -56,12 +56,12 @@ bool motorsActive() { } void testMotor(uint8_t n) { - Serial.printf("Testing motor %d\n", n); + print("Testing motor %d\n", n); motors[n] = 1; delay(50); // ESP32 may need to wait until the end of the current cycle to change duty https://github.com/espressif/arduino-esp32/issues/5306 sendMotors(); delay(3000); motors[n] = 0; sendMotors(); - Serial.printf("Done\n"); + print("Done\n"); } diff --git a/flix/parameters.ino b/flix/parameters.ino index acfe7e5..4447219 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -135,7 +135,7 @@ void syncParameters() { void printParameters() { for (auto ¶meter : parameters) { - Serial.printf("%s = %g\n", parameter.name, *parameter.variable); + print("%s = %g\n", parameter.name, *parameter.variable); } } diff --git a/flix/rc.ino b/flix/rc.ino index 23112b4..1227229 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -21,7 +21,7 @@ float channelNeutral[16] = {NAN}; // first element NAN means not calibrated float channelMax[16]; void setupRC() { - Serial.println("Setup RC"); + print("Setup RC\n"); RC.begin(); } @@ -44,15 +44,15 @@ void normalizeRC() { } void calibrateRC() { - Serial.println("Calibrate RC: move all sticks to maximum positions in 4 seconds"); - Serial.println("··o ··o\n··· ···\n··· ···"); + print("Calibrate RC: move all sticks to maximum positions in 4 seconds\n"); + print("··o ··o\n··· ···\n··· ···\n"); delay(4000); while (!readRC()); for (int i = 0; i < 16; i++) { channelMax[i] = channels[i]; } - Serial.println("Calibrate RC: move all sticks to neutral positions in 4 seconds"); - Serial.println("··· ···\n··· ·o·\n·o· ···"); + print("Calibrate RC: move all sticks to neutral positions in 4 seconds\n"); + print("··· ···\n··· ·o·\n·o· ···\n"); delay(4000); while (!readRC()); for (int i = 0; i < 16; i++) { @@ -62,8 +62,8 @@ void calibrateRC() { } void printRCCal() { - for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) Serial.printf("%g ", channelNeutral[i]); - Serial.printf("\n"); - for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) Serial.printf("%g ", channelMax[i]); - Serial.printf("\n"); + for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) print("%g ", channelNeutral[i]); + print("\n"); + for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) print("%g ", channelMax[i]); + print("\n"); } diff --git a/flix/wifi.ino b/flix/wifi.ino index 5343433..0ebd19d 100644 --- a/flix/wifi.ino +++ b/flix/wifi.ino @@ -17,12 +17,13 @@ WiFiUDP udp; void setupWiFi() { - Serial.println("Setup Wi-Fi"); + print("Setup Wi-Fi\n"); WiFi.softAP(WIFI_SSID, WIFI_PASSWORD); udp.begin(WIFI_UDP_PORT); } void sendWiFi(const uint8_t *buf, int len) { + if (WiFi.softAPIP() == IPAddress(0, 0, 0, 0) && WiFi.status() != WL_CONNECTED) return; udp.beginPacket(WIFI_UDP_IP, WIFI_UDP_PORT); udp.write(buf, len); udp.endPacket(); diff --git a/gazebo/flix.h b/gazebo/flix.h index 1437623..0d4c24d 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -34,7 +34,8 @@ void controlTorque(); void showTable(); void sendMotors(); bool motorsActive(); -void doCommand(String str); +void print(const char* format, ...); +void doCommand(String str, bool echo); void cliTestMotor(uint8_t n); void normalizeRC(); void printRCCal(); @@ -43,6 +44,7 @@ void sendMavlink(); void sendMessage(const void *msg); void receiveMavlink(); void handleMavlink(const void *_msg); +void mavlinkPrint(const char* str); void failsafe(); void armingFailsafe(); void rcLossFailsafe(); @@ -51,8 +53,8 @@ inline Quaternion FLU2FRD(const Quaternion &q); // mocks void setLED(bool on) {}; -void calibrateGyro() { printf("Skip gyro calibrating\n"); }; -void calibrateAccel() { printf("Skip accel calibrating\n"); }; -void printIMUCal() { printf("cal: N/A\n"); }; +void calibrateGyro() { print("Skip gyro calibrating\n"); }; +void calibrateAccel() { print("Skip accel calibrating\n"); }; +void printIMUCal() { print("cal: N/A\n"); }; void printIMUInfo() {}; Vector accBias, gyroBias, accScale(1, 1, 1);